#include <Arduino.h>
#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

// ================= DISPLAY =================
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ================= LDR PINS =================
#define LDR_TL 34
#define LDR_TR 35
#define LDR_BL 32
#define LDR_BR 33

// ================= SERVO PINS =================
#define SERVO_HORIZ 18
#define SERVO_VERT 19

// ================= SERVO LIMITS =================
int servoH = 90;
int servoV = 45;

const int HORIZ_MIN = 5;
const int HORIZ_MAX = 175;
const int VERT_MIN  = 5;
const int VERT_MAX  = 100;

// ================= CONTROL SETTINGS =================
const float KP_VERT = 0.015;
const float KP_HORZ = 0.018;

const int TOLERANCE = 60;           // Deadband
const int LIGHT_THRESHOLD = 400;    // Ignore low light
const unsigned long SENSOR_PERIOD_MS = 500;  // Slower = smoother + lower power
const float EMA_ALPHA = 0.25;       // Smoothing factor

// ================= INTERNAL =================
Servo horizontal;
Servo vertical;

float ema_tl, ema_tr, ema_bl, ema_br;
unsigned long lastReadTime = 0;

// ================= EMA FUNCTION =================
float emaUpdate(float previous, int current)
{
    return previous + EMA_ALPHA * (current - previous);
}

// ================= SETUP =================
void setup()
{
    Serial.begin(115200);

    // ADC attenuation for full 0–3.3V range
    analogSetPinAttenuation(LDR_TL, ADC_11db);
    analogSetPinAttenuation(LDR_TR, ADC_11db);
    analogSetPinAttenuation(LDR_BL, ADC_11db);
    analogSetPinAttenuation(LDR_BR, ADC_11db);

    horizontal.attach(SERVO_HORIZ);
    vertical.attach(SERVO_VERT);

    horizontal.write(servoH);
    vertical.write(servoV);

     Wire.begin(21, 22);  // SDA, SCL
  
  lcd.init();
  lcd.backlight();



    // Initialize EMA with first readings
    ema_tl = analogRead(LDR_TL);
    ema_tr = analogRead(LDR_TR);
    ema_bl = analogRead(LDR_BL);
    ema_br = analogRead(LDR_BR);
}

// ================= MAIN LOOP =================
void loop()
{
    if (millis() - lastReadTime >= SENSOR_PERIOD_MS)
    {
        lastReadTime = millis();

        // Read raw values
        int tlRaw = analogRead(LDR_TL);
        int trRaw = analogRead(LDR_TR);
        int blRaw = analogRead(LDR_BL);
        int brRaw = analogRead(LDR_BR);

        // Apply smoothing
        ema_tl = emaUpdate(ema_tl, tlRaw);
        ema_tr = emaUpdate(ema_tr, trRaw);
        ema_bl = emaUpdate(ema_bl, blRaw);
        ema_br = emaUpdate(ema_br, brRaw);

        int tl = (int)ema_tl;
        int tr = (int)ema_tr;
        int bl = (int)ema_bl;
        int br = (int)ema_br;

        // Averages
        int avgTop    = (tl + tr) / 2;
        int avgBottom = (bl + br) / 2;
        int avgLeft   = (tl + bl) / 2;
        int avgRight  = (tr + br) / 2;
        int avgAll    = (tl + tr + bl + br) / 4;

        // Debug
        Serial.printf("TL:%d TR:%d BL:%d BR:%d AVG:%d\n", tl, tr, bl, br, avgAll);

        // Only move if enough light
        if (avgAll > LIGHT_THRESHOLD)
        {
            int diffVert  = avgTop - avgBottom;
            int diffHoriz = avgLeft - avgRight;

            float moveV = 0;
            float moveH = 0;

            if (abs(diffVert) > TOLERANCE)
                moveV = KP_VERT * diffVert;

            if (abs(diffHoriz) > TOLERANCE)
                moveH = KP_HORZ * diffHoriz;

            // Limit movement speed (max 3 degrees per update)
            moveV = constrain(moveV, -3, 3);
            moveH = constrain(moveH, -3, 3);

            int newV = servoV - moveV;
            int newH = servoH + moveH;

            // Constrain to mechanical limits
            newV = constrain(newV, VERT_MIN, VERT_MAX);
            newH = constrain(newH, HORIZ_MIN, HORIZ_MAX);

            // Only update if changed
            if (newV != servoV)
            {
                servoV = newV;
                vertical.write(servoV);
            }

            if (newH != servoH)
            {
                servoH = newH;
                horizontal.write(servoH);
            }
        }

        // ================= DISPLAY =================
        lcd.clear();
// Line 1: Servo angles
lcd.setCursor(0, 0);
lcd.print("H:");
lcd.print(servoH);
lcd.print(" V:");
lcd.print(servoV);

// Line 2: Status
lcd.setCursor(0, 1);
if (avgAll > LIGHT_THRESHOLD) {
    lcd.print("TRACKING     ");   // 
} else {
    lcd.print("LOW LIGHT    ");
}
}
  


}