#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

//////////////Running Switching//////////
/////////////////////////////////////////
///using for multi tasking main loop/////
/////////////////////////////////////////

int run ; //Main run

//Run slave
int wait;
int wait2;
int wait3;
int wait4;

//slave VVVV
int runswitch,gyroinfo1,gyroinfo2             ;




////////////////LCD////////////////
//////////////////////////////////

LiquidCrystal_I2C lcd (0x27, 16, 2);
int seconds = 0;





//////////////MPU6050///////////////
///////////////////////////////////

Adafruit_MPU6050 mpu;
Adafruit_MPU6050 mpu1;
sensors_event_t event;

int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
float xByGyro, yByGyro, zByGyro;      // Global variables for the rotation by gyro
const int mpuAddress = 0x68;


////////////////////////////SERVO//////////////////////
//////////////////////////////////////////////////////
Servo horizontal; // horizontal servo
int servoh = 180; 
int servohLimitHigh = 175;
int servohLimitLow = 5;
// 65 degrees MAX

int servoflag = 0;

Servo vertical; // vertical servo
int servov = 45; 
int servovLimitHigh = 60;
int servovLimitLow = 1;

///////// LDR pin connections///////////////////////////////////
/////////////////////////////////////////////////////
// name = analogpin;
int ldrlt = 32; //LDR top left - BOTTOM LEFT <--- BDG
int ldrrt = 34; //LDR top rigt - BOTTOM RIGHT
int ldrld = 33; //LDR down left - TOP LEFT
int ldrrd = 35; //ldr down rigt - TOP RIGHT




////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
void setup()
{
 Serial.begin(9600);
 Serial.println();
 Serial.println("Mechatronics Eng");
 Serial.println("MiniProject Solar Tacking 2axis");
 Serial.println("");
 Serial.println("By..............");




 lcd.init();         // initial LCD on lib
 lcd.backlight();    // Blink LCD backlight on
 lcd.setCursor(0,0); // Clear LCD display (colunm,row)
 lcd.print("Mechatronics Eng");

 lcd.setCursor(0,1);
 lcd.print("MiniProject");

delay(3000);

 lcd.setCursor(0,0);
 lcd.print("Solar Tack 2axis");

 lcd.setCursor(0,1);
 lcd.print("Servo Control");



 

 horizontal.attach(5);
 vertical.attach(18);
 horizontal.write(180);
 vertical.write(45);
 delay(2000);
 lcd.init();  

 while (!mpu.begin()) 
 {
    Serial.println("MPU6050 MPU0 not connected!");
    delay(1000);
 }
  Serial.println("MPU6050 MPU0 ready!");


 while (!mpu1.begin()) 
 {
    Serial.println("MPU6050 MPU1 not connected!");
    delay(1000);
 }
  Serial.println("MPU6050  MPU1 ready!");
}



/////////////////////////////////////////////////////////////////////////  \/\/\/\/\/\/\/\/\/\/\/\/
/////////////////////////////////////////////////////////////////////////
//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////

void loop() 
{
  MAINRUN();

  if(run-wait>1000) //running switching servoflag
    {
      switching();
      wait=run;
    }

  if(run-wait2>100)  //GYRO dectecing
  {
      GyroSensor1();
      GyroSensor2();

      wait2=run;
  }
  if(run-wait3>5)   //SERVO running
  {
    Servo();

    wait3=run;
  }


  if(run-wait4>100) // Display running
  {
  Display();
  

  wait4=run;
  }
}

/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP//MAIN LOOP
/////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////// /\/\/\/\/\/\/\/\/\/\/\/\/\/\/\

void GyroSensor1 (void)
{
    /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */

if(run-gyroinfo1>1000)
{
  Serial.println("MPU 1 ////////////////1111111111////////////////// MPU 1");
  Serial.print("MPU SENSER1 ");

  Serial.println("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");
  Serial.println("MPU 1 ///////////////1111111/////////////////// MPU 1");
  Serial.println("");
  gyroinfo1=run;
}

}

/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////


void GyroSensor2 (void)
{
  Wire.beginTransmission( mpuAddress);
  Wire.write( 0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission( false);        // No stop condition for a repeated start

  // The MPU-6050 has the values as signed 16-bit integers.
  // There are 7 values in 14 registers.
  //int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

  Wire.requestFrom( mpuAddress, 14);   // request a total of 14 bytes
  AcX = Wire.read()<<8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY = Wire.read()<<8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read()<<8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read()<<8 | Wire.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read()<<8 | Wire.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)
    // The acceleration is directly mapped into the angles.
  // That is rather artificial.
  // The combined gravity could be used for an angle, while ignoring the strength.
  //
  // The gyro sets the rotation speed.
  // The angle created by the rotation speed is added to angle by the accelerometer.
  //
  // The conversion from the sensor values to the rotation is just a value
  // that makes it look good on the display.

  float xByAccel = (float) AcX * 0.0001;      // static angle by accelerometer
  float yByAccel = (float) AcY * 0.0001;
  float zByAccel = (float) AcZ * 0.0001;

  xByGyro += (float) GyX * 0.00001;           // moving angle by gyro
  yByGyro += (float) GyY * 0.00001;
  zByGyro += (float) GyZ * 0.00001;

  float x = xByAccel + xByGyro;               // combine both angles
  float y = yByAccel + yByGyro;
  float z = zByAccel + zByGyro; 


  if(run-gyroinfo2>1000)
  {
  Serial.println("MPU 2 ////////////////222222222////////////////// MPU 2");
  Serial.print("MPU SENSER2 ");

  Serial.print("Acceleration 2AX: ");
  Serial.print(xByAccel);
  Serial.print(", 2AY: ");
  Serial.print(yByAccel);
  Serial.print(", 2AZ: ");
  Serial.print(zByAccel);
  Serial.println(" m/s^2");
  Serial.print("Rotation X 2GX: ");
  Serial.print(xByGyro);
  Serial.print(", 2GY: ");
  Serial.print(yByGyro);
  Serial.print(", 2GZ: ");
  Serial.print(zByGyro);
  Serial.println(" m/s^2");

  Serial.println("MPU 2 /////////////////22222222///////////////// MPU 2");

  

  Serial.println(" ");

   gyroinfo2=run;
  }



 
}
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////

void Servo (void)
{
int lt = analogRead(ldrlt); // top left
int rt = analogRead(ldrrt); // top right
int ld = analogRead(ldrld); // down left
int rd = analogRead(ldrrd); // down right
int dtime = 0; int tol = 90; // dtime=diffirence time, tol=toleransi
int avt = (lt + rt) / 2; // average value top
int avd = (ld + rd) / 2; // average value down
int avl = (lt + ld) / 2; // average value left
int avr = (rt + rd) / 2; // average value right
int dvert = avt - avd; // check the diffirence of up and down
int dhoriz = avl - avr;// check the diffirence og left and rigt




if (-1*tol > dvert || dvert > tol) 
 {
 if (avt > avd)
 {
 servov = ++servov;
 if (servov > servovLimitHigh)
 {servov = servovLimitHigh;}
 }
 else if (avt < avd)
 {servov= --servov;
 if (servov < servovLimitLow)
 { servov = servovLimitLow;}
 }
 vertical.write(servov);
 }
if (-1*tol > dhoriz || dhoriz > tol) // check if the diffirence is in the tolerance else change horizontal angle
 {
 if (avl > avr)
 {
 servoh = --servoh;
 if (servoh < servohLimitLow)
 {
 servoh = servohLimitLow;
 }
 }
 else if (avl < avr)
 {
 servoh = ++servoh;
 if (servoh > servohLimitHigh)
 {
 servoh = servohLimitHigh;
 }
 }
 else if (avl = avr)
 {
 delay(5000);
 }
 horizontal.write(servoh);
 }
 


 delay(dtime);

}

/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////


void Display(void)
{
   
    
  float xByAccel = (float) AcX * 0.0001;      // static angle by accelerometer
  float yByAccel = (float) AcY * 0.0001;
  float zByAccel = (float) AcZ * 0.0001;

  xByGyro += (float) GyX * 0.00001;           // moving angle by gyro
  yByGyro += (float) GyY * 0.00001;
  zByGyro += (float) GyZ * 0.00001;

  float x = xByAccel + xByGyro;               // combine both angles
  float y = yByAccel + yByGyro;
  float z = zByAccel + zByGyro; 

  Serial.println(x);

 if (!servoflag) ////check0
 {
   

   lcd.setCursor(0,0); // Clear LCD display (colunm,row)
   lcd.print("MPU 1 x:"+ String(x)+ "d");
   lcd.setCursor(0,1); // Clear LCD display (colunm,row)
   lcd.print("y:"+ String(y)+ "d"
            +"z:"+ String(z)+ "d"
            );
   /*
   lcd.setCursor(0,2); // Clear LCD display (colunm,row)
   lcd.print("Axis y : "+ String(event.acceleration.y)+ " degree");
   lcd.setCursor(0,3); // Clear LCD display (colunm,row)
   lcd.print("Axis z : "+ String(event.acceleration.z)+ " degree");
   */
 }
 else 
 {
   //lcd.setCursor(0,0); // Clear LCD display (colunm,row)
   //lcd.print("");
   lcd.setCursor(0,0); // Clear LCD display (colunm,row)
   lcd.print("Servoh : "+ String(servoh)+ " degree");
   lcd.setCursor(0,1); // Clear LCD display (colunm,row)
   lcd.print("Servov : "+ String(servov)+ " degree");

 }

}


/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////



//////// ไจโรล์ ควบคุม Servo และ เข้าdisplay เเยก

void switching (void)
{
  
  if(run-runswitch>3000)
  {
    runswitch =run;
    lcd.init(); 
    servoflag = !servoflag;
    Serial.print("--------------------------------------------------------------------flag-state---->"+String(servoflag)+"");
    Serial.println("-----> running at"+String(run)+"");

  }
}

void MAINRUN (void)
{
  run = millis();
}