#include <MPU6050_light.h>
#include <Wire.h>
#include <ESP32Servo.h>
#include <Adafruit_SSD1306.h> 

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
#define AddrOled 0x3C
bool foundOled = true;

MPU6050 mpu(Wire);

static const int servoPin = 5;
bool useServo;
#define UseServo 4
#define LedPin 2
#define Trig 18
#define Echo 19
//define sound speed in cm/uS
#define SOUND_SPEED 0.034
float deg2rad = PI/180;

long duration;
float distance_mm;

Servo servo1;
int regServo = 0;

unsigned long timer = 0;
unsigned long timer2 = 0;
// put function declarations here:
void ReadMPU();
void ControlServo(int);
float ReadHCSR04();
void PrintDisplay(float,float,bool);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial2.begin(115200);
  Wire.begin();

  pinMode(LedPin, OUTPUT);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(UseServo, INPUT);
  useServo = digitalRead(UseServo);
  if (useServo)
  {
    Serial.println("servo run");
  } else Serial.println("servo stop");
  
  if(!display.begin(SSD1306_SWITCHCAPVCC, AddrOled)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    foundOled = false;
  }else Serial.println(F("SSD1306 ok"));

  byte status=mpu.begin();
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while(status!=0){
    digitalWrite(LedPin, HIGH);
    delay(500);
    if (foundOled)
    {
      display.clearDisplay();
      display.setTextSize(2);
      display.setTextColor(WHITE);
      display.setCursor(0,0);
      display.print("Error\n -> Resert");
      display.display();
    }
    
  } // stop everything if could not connect to MPU6050
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
  servo1.attach(servoPin);
  Serial.println("Done!\n");
}


void loop() {
  // put your main code here, to run repeatedly:
  if((millis()-timer)>10){ // print data every 10ms
    ReadMPU();
    // Serial.print("X : ");
    // Serial.print(mpu.getAngleX());
    // Serial.print("\tY : ");
    // Serial.print(mpu.getAngleY());
    // Serial.print("\tZ : ");
    // Serial.println(mpu.getAngleZ());
    timer = millis();  
  }
  if((millis()-timer2)>200) // print data every 10ms
  {
    int ang = map(mpu.getAngleY(), -90, 90, 0, 180);
    Serial2.println(ang);
    distance_mm=ReadHCSR04()*10;
    PrintDisplay(distance_mm, mpu.getAngleY(), useServo);
    if(useServo) ControlServo(ang);
    timer2 = millis();  
  }
}

// put function definitions here:
void ReadMPU()
{
  mpu.update();
}

/// @brief xuất góc từ imu sang servo 
/// @param ang góc imu từ [0;180]
void ControlServo(int ang)
{
  if (regServo != ang) 
  { 
    servo1.write(ang);
    regServo = ang;
  }
}

float ReadHCSR04()
{
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  
  long duration = pulseIn(Echo, HIGH);
  
  // Calculate the distance
  return duration * SOUND_SPEED/2;
}

/// @brief in ra màn hình
/// text size = 2 kich thuoc x=15, y= 16 (1 ky tu)
/// text size = 3 kich thuoc x=18 ,y= 22 (1 ky tu)
/// @param msg 
void PrintDisplay(float dis, float ang, bool servoRun)
{
  if (!servoRun)
  {
    //cos = kề / huyền
    // Serial.print(ang);
    // Serial.print("-");
    ang *= deg2rad;
    dis = abs(dis/cos(ang));
    // Serial.print(ang);
    // Serial.print("-");
    // Serial.print(dis);
    // Serial.println();
  }
  
  display.clearDisplay();
  display.setTextSize(3);
  display.setTextColor(WHITE);
  int centerH = SCREEN_HEIGHT / 2 - 8;
  int centerW = SCREEN_WIDTH / 2;
  float disShow = dis;
  
  if (dis >= 1000) disShow /= 1000;
  if (disShow < 100)
  {
    display.setCursor(centerW - 38, centerH);
    display.print(disShow);
  }else if (100 <=  disShow && disShow < 1000)
  {
    display.setCursor(centerW - 54, centerH);
    display.print(disShow);
  }
  
  //don vi
  if (dis >= 1000)
  {
    display.setTextSize(2);
    display.setCursor(SCREEN_WIDTH - 16, SCREEN_HEIGHT - 16);
    display.print("M");
  }else
  {
    display.setTextSize(2);
    display.setCursor(SCREEN_WIDTH - 25, SCREEN_HEIGHT - 16);
    display.print("mm");
  }
  display.setTextSize(2);
  display.setCursor(0, 0);
  display.print(ang/deg2rad);
  display.setTextSize(1);
  display.print("O");
  display.display();
}