#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal_I2C.h>
#include <WiFi.h>
#include <WiFiClientSecure.h>
#include <UniversalTelegramBot.h>
#include <ArduinoJson.h>

const char* ssid = "Wokwi-GUEST";
const char* password = "";

#define BOTtoken "7404330463:AAGW8Crr-6j9-2z0qz8WXsHhzjygfqu_npM"
#define CHAT_ID "1708405036"

WiFiClientSecure client;
UniversalTelegramBot bot(BOTtoken, client);

LiquidCrystal_I2C lcd(0x27, 16, 2);

Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;

unsigned long prevTime = 0;
unsigned long interval = 1000;
unsigned long currentTime = millis();
unsigned long lastTime = 0;
unsigned long lastTimeTemperature = 0;
unsigned long lastTimeAcc = 0;
unsigned long gyroDelay = 10;
unsigned long temperatureDelay = 1000;
unsigned long accelerometerDelay = 200;

const long double radTodeg = 57.2957795;
//float tilt_x = g.gyro.x * radTodeg;
//float tilt_y = g.gyro.y * radTodeg; 
float gyroX, gyroY, gyroZ;
float accX, accY, accZ;
float temperature;
float gyroXerror = 0.07;
float gyroYerror = 0.03;
float gyroZerror = 0.01;

void initMPU(){
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
}

String getGyroReadings(){
  mpu.getEvent(&a, &g, &temp);

  float gyroX_temp = g.gyro.x;
  if(abs(gyroX_temp) > gyroXerror)  {
    gyroX += gyroX_temp/50.00;
  }
  
  float gyroY_temp = g.gyro.y;
  if(abs(gyroY_temp) > gyroYerror) {
    gyroY += gyroY_temp/70.00;
  }

  float gyroZ_temp = g.gyro.z;
  if(abs(gyroZ_temp) > gyroZerror) {
    gyroZ += gyroZ_temp/90.00;
  }

}

void initWiFi(){
  Serial.print("Connecting Wifi: ");
  Serial.println(ssid);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  client.setCACert(TELEGRAM_CERTIFICATE_ROOT);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(500);
  }
}

String getTemperature(){
  mpu.getEvent(&a, &g, &temp);
  temperature = temp.temperature;
  return String(temperature);
}

void setLocalIP() {
  Serial.println(WiFi.localIP());
  Serial.print("Getting local IP: ");
  configTime(0, 0, "pool.ntp.org"); 
  time_t now = time(nullptr);
  while (now < 24 * 3600)
  {
    Serial.print(".");
    delay(100);
    now = time(nullptr);
  }
  Serial.println(now);
}

void setGyroRange(){
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }
}

void setFilterBandwidth(){
  mpu.setFilterBandwidth(MPU6050_BAND_260_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }
  Serial.println("");
  delay(100);
}

void setup() {
  lcd.begin(16, 2);
  lcd.init();
  lcd.backlight();
  lcd.clear();
  Serial.begin(115200);
  initMPU();
  initWiFi();
  setLocalIP();
  setGyroRange();
  setFilterBandwidth();
}

void loop() {
  mpu.getEvent(&a, &g, &temp);
  if ((millis() - lastTime) > gyroDelay) {
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("X: ");
  lcd.print(g.gyro.x);
  lcd.println(char(223));
  lcd.setCursor(0,1);
  lcd.print("Y: ");
  lcd.print(g.gyro.y);
  lcd.println(char(223));
  Serial.println(g.gyro.x);
  Serial.println(g.gyro.y);
  lastTime = millis();
  }
  TelegramBot();
}

void TelegramBot(){
  if (millis() > prevTime + interval){
    int NewMessages = bot.getUpdates(bot.last_message_received + 1);
    while(NewMessages) {
      Serial.println("got response");
      NewMessagesHandle(NewMessages);
      NewMessages = bot.getUpdates(bot.last_message_received + 1);
    }
    prevTime = millis();
  }
}

void NewMessagesHandle(int NewMessages) {
  Serial.println("NewMessagesHandle");
  Serial.println(String(NewMessages));
  for (int i=0; i<NewMessages; i++) {
    String chat_id = String(bot.messages[i].chat_id);
    if (chat_id != CHAT_ID){
      bot.sendMessage(chat_id, "Unauthorized user", "");
      continue;
    }

    String text = bot.messages[i].text;
    Serial.println(text);

    String from_name = bot.messages[i].from_name;

    if (text == "/start") {
      String welcome = "Welcome, " + from_name + ".\n";
      welcome += "Use the /angle commands to show angle measurements of the excavator.\n\n";
      bot.sendMessage(chat_id, welcome, "");
    }
    if (text == "/angle") {
      String text = getReadings();
      bot.sendMessage(chat_id, text);
    }
  }
}

String getReadings(){
  String message = "Measurement Result\n";
  //message += "X Axis = " + String(tilt_x) + char(223);
  //message += "Y Axis = " + String(tilt_y) + char(223);
  return message;
}