#include <ESP32Servo.h>

#define SERVO_PIN_V 12 // ESP32 pin GPIO26 connected to servo motor
#define SERVO_PIN_H 14 // ESP32 pin GPIO27 connected to servo motor

#define LDR_TL_PIN 32 // Top Left
#define LDR_TR_PIN 33 // Top Right

#define LDR_BL_PIN 25 // Botton Left
#define LDR_BR_PIN 26 // Botton Right

#define TRACKING_BTN 2
#define TRACKING_LED 4

Servo servoV;
int angleV = 0;
int high_limit_V = 100;
int low_limit_V = 0;

Servo servoH;
int angleH = 180;
int high_limit_H = 175;
int low_limit_H = 5;

int TOLERANCE = 400;
bool tracking = false;
bool btn_pressed = false;

void setup() {
  servoV.attach(SERVO_PIN_V);  // attaches the servo on ESP32 pin
  servoH.attach(SERVO_PIN_H);  // attaches the servo on ESP32 pin
  pinMode(TRACKING_BTN, INPUT_PULLUP);
  pinMode(TRACKING_LED, OUTPUT);

  servoV.write(low_limit_V);
  servoH.write(low_limit_H);

  Serial.begin(115200);

  delay(2500);
}

void lightTracking(){
  int tl = analogRead(LDR_TL_PIN);
  int tr = analogRead(LDR_TR_PIN);
  int bl = analogRead(LDR_BL_PIN);
  int br = analogRead(LDR_BR_PIN);

  int avg_top = (tl + tr) / 2;
  int avg_botton = (bl + br) / 2;
  int avg_left = (tl + bl) / 2;
  int avg_right = (tr + br) / 2;

  int diff_tb = avg_top - avg_botton;
  int diff_lr = avg_left - avg_right;

  if (abs(diff_tb) > TOLERANCE)
  {
    if (avg_top > avg_botton){
      angleV += 1;
      if (angleV > high_limit_V){
        angleV = high_limit_V;
      }
    }
    else{
      angleV -= 1;
      if (angleV < low_limit_V){
        angleV = low_limit_V;
      }
    }
    servoV.write(angleV);
  }

  if (abs(diff_lr) > TOLERANCE)
  {
    if (avg_right > avg_left){
      angleH += 1;
      if (angleH > high_limit_H){
        angleH = high_limit_H;
      }
    }
    else {
      angleH -= 1;
      if (angleH < low_limit_H){
        angleH = low_limit_H;
      }
    }
    servoH.write(angleH);
  }
}

void loop() {

  if (digitalRead(TRACKING_BTN)){
    btn_pressed = false;
  }
  if (!digitalRead(TRACKING_BTN) && !btn_pressed){
    btn_pressed = true;
    tracking = !tracking;
    digitalWrite(TRACKING_LED, tracking);
  }

  if (tracking) {
    lightTracking();
  }
  else {
    int analogV = analogRead(35); // GPIO35
    int angleV = map(analogV, 0, 4095, low_limit_V, high_limit_V);
    int analogH = analogRead(34); // GPIO34
    int angleH = map(analogH, 0, 4095, low_limit_H, high_limit_H);

    servoV.write(angleV);
    servoH.write(angleH);
  }
  
  delay(5);
}