#include <ESP32Servo.h>

Servo servoVertical;     // Controls North/South
Servo servoHorizontal;   // Controls East/West

// ---------------- SERVO SETTINGS ----------------
int verticalPos = 90;
int horizontalPos = 90;

const int servoLimitHigh = 180;
const int servoLimitLow  = 0;

const int servoVerticalPin   = D7;   
const int servoHorizontalPin = D8;    

// ---------------- LDR PINS ----------------
const int ldrNorth = D0;
const int ldrSouth = D1;
const int ldrEast  = D2;
const int ldrWest  = D3;

// Movement tolerance
const int tolerance = 150;

void setup() {
  Serial.begin(115200);

  servoVertical.attach(servoVerticalPin);
  servoHorizontal.attach(servoHorizontalPin);

  servoVertical.write(verticalPos);
  servoHorizontal.write(horizontalPos);

  delay(1000);

  // ---- Print Pin Configuration ----
  Serial.println("===== PIN CONFIGURATION =====");
  Serial.print("North LDR  -> Pin D0 (GPIO "); Serial.print(digitalPinToGPIONumber(ldrNorth)); Serial.println(")");
  Serial.print("South LDR  -> Pin D1 (GPIO "); Serial.print(digitalPinToGPIONumber(ldrSouth)); Serial.println(")");
  Serial.print("East  LDR  -> Pin D2 (GPIO "); Serial.print(digitalPinToGPIONumber(ldrEast));  Serial.println(")");
  Serial.print("West  LDR  -> Pin D3 (GPIO "); Serial.print(digitalPinToGPIONumber(ldrWest));  Serial.println(")");

  Serial.print("Vertical Servo   -> Pin "); Serial.println(servoVerticalPin);
  Serial.print("Horizontal Servo -> Pin "); Serial.println(servoHorizontalPin);
  Serial.println("=============================");
}

void loop() {

  // -------- READ LDR VALUES --------
  int northValue = analogRead(ldrNorth);
  int southValue = analogRead(ldrSouth);
  int eastValue  = analogRead(ldrEast);
  int westValue  = analogRead(ldrWest);

  // -------- CALCULATE DIFFERENCES --------
  int diffVertical   = northValue - southValue;
  int diffHorizontal = eastValue - westValue;

  // -------- DEBUG OUTPUT --------
  Serial.print("N(D0): "); Serial.print(northValue);
  Serial.print(" S(D1): "); Serial.print(southValue);
  Serial.print(" E(D2): "); Serial.print(eastValue);
  Serial.print(" W(D3): "); Serial.print(westValue);
  Serial.print(" | DV: "); Serial.print(diffVertical);
  Serial.print(" DH: "); Serial.println(diffHorizontal);

  // -------- VERTICAL MOVEMENT --------
  if (abs(diffVertical) > tolerance) {

    int stepSize = map(abs(diffVertical), tolerance, 4095, 1, 5);

    if (diffVertical > 0) {
      verticalPos += stepSize;
      Serial.println("Moving UP (North brighter)");
    }
    else {
      verticalPos -= stepSize;
      Serial.println("Moving DOWN (South brighter)");
    }

    verticalPos = constrain(verticalPos, servoLimitLow, servoLimitHigh);
    servoVertical.write(verticalPos);
  }

  // -------- HORIZONTAL MOVEMENT --------
  if (abs(diffHorizontal) > tolerance) {

    int stepSize = map(abs(diffHorizontal), tolerance, 4095, 1, 5);

    if (diffHorizontal > 0) {
      horizontalPos += stepSize;
      Serial.println("Moving RIGHT (East brighter)");
    }
    else {
      horizontalPos -= stepSize;
      Serial.println("Moving LEFT (West brighter)");
    }

    horizontalPos = constrain(horizontalPos, servoLimitLow, servoLimitHigh);
    servoHorizontal.write(horizontalPos);
  }

  delay(50);
}