#include <Servo.h>   // Including the Servo library
#include <Wire.h>    // Including the i2c library
#include <BH1750.h>  // Including the Light sensor library

//Defining the LDRs used to track the sun
#define LDR0 A0  // if sun is left down
#define LDR1 A1  // if sun is right down
#define LDR2 A2  // if sun is right up
#define LDR3 A3  // if sun is left up

#define check_light 1800000  //Sets the time when the solar panel should be realigned
// (30 minutes * 60 seconds * 1000 milliseconds)
#define light_offset 10  // Defines the light offset for the I2C sensor. If the difference is within this value, don't do anything
#define ldr_offset 5     // Defines the difference between LDR values. If the difference is within this value, don't do anything

unsigned long TimerMS;

int ldr0_val, ldr1_val, ldr2_val, ldr3_val;  // Define variables for the LDRs
uint16_t last_lightS, lightS_val;            // Uint16_T gives us wider number range, compatible with library
bool rotating;                               // Flag to indicate if there is a difference in LDR sensors

Servo tilt;  // Declaring a servo for tilting up and down
Servo pan;   // Declaring a servo for rotating left and right

int X = 90, Y = 90;  // X = pan, Y = tilt, Initial position of the servos

BH1750 lightSen;  // Create object for light Sensor

void setup() {
  Serial.begin(9600);  // Serial monitor for debugging
  lightSen.begin();    // Begin communication with the light sensor
  tilt.attach(9);      // Attaching the servo object to a PWM pin
  pan.attach(6);

  DefaultPosition();  // Moves the servos to their initial positions (flat)
  LightServo();       // Calls the function to read LDR values and adjust the servos accordingly
}

void loop() {
  if (millis() - TimerMS > check_light) {  // Checks if the set time has elapsed
    LightServo();                          // Calls the function to read LDR values and adjust the servos accordingly
    TimerMS = millis();                    // Resets the timer
  }
  delay(10);  // Small not important delay
}

void LightServo() {  // Function to read LDR values and adjust the servos accordingly
  Serial.println("Checking light....");

  lightS_val = lightSen.readLightLevel();  // Reads light intensity data from the I2C sensor

  if (last_lightS + light_offset > lightS_val || last_lightS - light_offset < lightS_val) {
    // If the light intensity data has not changed much since last reading, don't do anything and return
    Serial.println(lightS_val);
    Serial.println("Light on i2c sensor did not change, staying on the same position.");
    TimerMS = millis();
    return;
  }
  last_lightS = lightS_val;  // Stores the current light intensity value for next comparison

  rotating = true;  // Default state

  while (rotating) {

    ldr0_val = analogRead(LDR0);  // Reads LDR values
    ldr1_val = analogRead(LDR1);
    ldr2_val = analogRead(LDR2);
    ldr3_val = analogRead(LDR3);

    /*  // For debugging:
        Serial.println(ldr0_val);
        Serial.println(ldr1_val);
        Serial.println(ldr2_val);
        Serial.println(ldr3_val);
        Serial.println();
    */

    if (ldr1_val < 249 && ldr2_val < 249 && ldr3_val < 249 && ldr0_val < 249) {  // If the sunlight is too weak, the solar panel should be moved to its default position and stop rotating
      DefaultPosition();
      rotating = false;
    }

    if (ldr1_val + ldr_offset < ldr0_val && ldr1_val + ldr_offset < ldr2_val && ldr1_val + ldr_offset < ldr3_val) {  // If the reading from LDR1 is less than the readings from the other LDRs by a certain offset, the solar panel should rotate right and tilt down
      Serial.println("Sun is down right");
      rotateRIGHT();
      tiltDOWN();
    } else if (ldr0_val + ldr_offset < ldr1_val && ldr0_val + ldr_offset < ldr2_val && ldr0_val + ldr_offset < ldr3_val) {  // If the reading from LDR0 is less than the readings from the other LDRs by a certain offset, the solar panel should rotate right and tilt down
      Serial.println("Sun is down left");
      rotateLEFT();
      tiltDOWN();
    } else if (ldr2_val + ldr_offset < ldr1_val && ldr2_val + ldr_offset < ldr0_val && ldr2_val + ldr_offset < ldr3_val) {  // If the reading from LDR2 is less than the readings from the other LDRs by a certain offset, the solar panel should rotate right and tilt down
      Serial.println("Sun is up right");
      rotateRIGHT();
      tiltUP();
    } else if (ldr3_val + ldr_offset < ldr1_val && ldr3_val + ldr_offset < ldr2_val && ldr3_val + ldr_offset < ldr0_val) {  // If the reading from LDR3 is less than the readings from the other LDRs by a certain offset, the solar panel should rotate right and tilt down
      Serial.println("Sun is up left");
      rotateLEFT();
      tiltUP();
    } else if (ldr0_val + ldr_offset < ldr2_val && ldr0_val + ldr_offset < ldr3_val || ldr1_val + ldr_offset < ldr2_val && ldr1_val + ldr_offset < ldr3_val) {  // If the reading from LDR0 or LDR1 is less than the reading from LDR2 and LDR3, the solar panel should tilt down
      Serial.print("Sun is down");
      tiltDOWN();
    } else if (ldr0_val + ldr_offset < ldr2_val && ldr0_val + ldr_offset < ldr1_val || ldr3_val + ldr_offset < ldr2_val && ldr3_val + ldr_offset < ldr1_val) {  // If the reading from LDR0 or LDR3 is less than the reading from LDR2 and LDR3, the solar panel should tilt down
      Serial.println("Sun is left");
      rotateLEFT();
    } else if (ldr1_val + ldr_offset < ldr0_val && ldr1_val + ldr_offset < ldr3_val || ldr2_val + ldr_offset < ldr0_val && ldr2_val + ldr_offset < ldr3_val) {  // If the reading from LDR1 or LDR2 is less than the reading from LDR2 and LDR3, the solar panel should tilt down
      Serial.println("Sun is right");
      rotateRIGHT();
    } else if (ldr3_val + ldr_offset < ldr0_val && ldr3_val + ldr_offset < ldr1_val || ldr2_val + ldr_offset < ldr0_val && ldr2_val + ldr_offset < ldr1_val) {  // If the reading from LDR3 or LDR2 is less than the reading from LDR2 and LDR3, the solar panel should tilt down
      Serial.println("Sun is up");
      tiltUP();
    } else {  // If the readings from LDRs are the same then the location is OK
      Serial.println("location OK");
      rotating = false;  // comment this out for debugging
    }

    // delay(500); // OPTIONAL for simulation
  }
}

void DefaultPosition() {  // Flat position
  X = 90;
  Y = 90;
  pan.write(X);
  tilt.write(Y);
}

void rotateRIGHT() {
  if (X < 180) X++;  // Check the angle of the servo, turn accordingly, if reached endpoint, exit the loop
  else {
    rotating = false;
    return;
  }
  pan.write(X);
}

void rotateLEFT() {
  if (X > 0) X--;  // Check the angle of the servo, turn accordingly, if reached endpoint, exit the loop
  else {
    rotating = false;
    return;
  }
  pan.write(X);
}

void tiltUP() {
  if (Y < 180) Y++;  // Check the angle of the servo, turn accordingly, if reached endpoint, exit the loop
  else {
    rotating = false;
    return;
  }
  tilt.write(Y);
}

void tiltDOWN() {
  if (Y > 0) Y--;  // Check the angle of the servo, turn accordingly, if reached endpoint, exit the loop
  else {
    rotating = false;
    return;
  }
  tilt.write(Y);
}