/********* Pleasedontcode.com **********

	Pleasedontcode thanks you for automatic code generation! Enjoy your code!

	- Terms and Conditions:
	You have a non-exclusive, revocable, worldwide, royalty-free license
	for personal and commercial use. Attribution is optional; modifications
	are allowed, but you're responsible for code maintenance. We're not
	liable for any loss or damage. For full terms,
	please visit pleasedontcode.com/termsandconditions.

	- Project: **Servo Control**
	- Source Code NOT compiled for: ESP32 DevKit V1
	- Source Code created on: 2025-02-13 13:10:39

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* solar tracker,        one vertical servo,one */
	/* rotational servo, 4 ldr */
/****** END SYSTEM REQUIREMENTS *****/

/* START CODE */

/****** DEFINITION OF LIBRARIES *****/
#include 	//https://madhephaestus.github.io/ESP32Servo/annotated.html
#include 	//https://madhephaestus.github.io/ESP32Servo/annotated.html
#include 	//https://madhephaestus.github.io/ESP32Servo/annotated.html
#include 	//https://madhephaestus.github.io/ESP32Servo/annotated.html
#include 	//https://madhephaestus.github.io/ESP32Servo/annotated.html

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/

// Servo configuration
#define SERVO_YAW 26   // Horizontal servo pin
#define SERVO_TILT 27  // Vertical servo pin

// LDR pins
#define LDR_TL 34  // Top-Left LDR
#define LDR_TR 35  // Top-Right LDR
#define LDR_BL 32  // Bottom-Left LDR
#define LDR_BR 33  // Bottom-Right LDR

Servo servoYaw;
Servo servoTilt;

// Servo angle limits
#define MIN_ANGLE 45
#define MAX_ANGLE 135

// Movement and control parameters
#define MOVEMENT_STEP 1
#define THRESHOLD 20
#define DEADZONE 10            // Increased to prevent jitter
#define SERVO_DELAY 20
#define DEBUG_INTERVAL 1000

int yawAngle = 90;
int tiltAngle = 90;
int lastYawAngle = 90;
int lastTiltAngle = 90;

unsigned long lastDebugTime = 0;

// Function to read and average LDR values
int readLDR(int pin) {
    long total = 0;
    const int readings = 5;
    for (int i = 0; i < readings; i++) {
        total += analogRead(pin);
        delay(2);
    }
    return total / readings;
}

// Normalize LDR values between 0 and 100
int normalizeLDR(int value) {
    const int minVal = 300;   // Adjusted for more tolerance
    const int maxVal = 4095;
    return constrain(map(value, minVal, maxVal, 0, 100), 0, 100);
}

void moveServos() {
    // Clamp angles within range
    yawAngle = constrain(yawAngle, MIN_ANGLE, MAX_ANGLE);
    tiltAngle = constrain(tiltAngle, MIN_ANGLE, MAX_ANGLE);

    // Move Yaw servo gradually
    if (yawAngle != lastYawAngle) {
        if (yawAngle > lastYawAngle) lastYawAngle += MOVEMENT_STEP;
        else if (yawAngle < lastYawAngle) lastYawAngle -= MOVEMENT_STEP;
        servoYaw.write(lastYawAngle);
    }

    // Move Tilt servo gradually
    if (tiltAngle != lastTiltAngle) {
        if (tiltAngle > lastTiltAngle) lastTiltAngle += MOVEMENT_STEP;
        else if (tiltAngle < lastTiltAngle) lastTiltAngle -= MOVEMENT_STEP;
        servoTilt.write(lastTiltAngle);
    }

    delay(SERVO_DELAY);
}

void setup() {
    Serial.begin(115200);
    // The following lines are compatible with ESP32
    // analogSetAttenuation(ADC_11db); // Uncomment if using ADC attenuation settings
    // analogSetWidth(12); // Uncomment if using ADC width settings

    servoYaw.attach(SERVO_YAW);
    servoTilt.attach(SERVO_TILT);
    delay(100);

    servoYaw.write(90);
    servoTilt.write(90);
    delay(500);

    Serial.println("System Initialized.");
}

void loop() {
    int topLeft = readLDR(LDR_TL);
    int topRight = readLDR(LDR_TR);
    int bottomLeft = readLDR(LDR_BL);
    int bottomRight = readLDR(LDR_BR);

    int tl = normalizeLDR(topLeft);
    int tr = normalizeLDR(topRight);
    int bl = normalizeLDR(bottomLeft);
    int br = normalizeLDR(bottomRight);

    int horizontalDiff = ((tr + br) - (tl + bl)) / 2;
    int verticalDiff = ((bl + br) - (tl + tr)) / 2;

    if (abs(horizontalDiff) > DEADZONE) {
        yawAngle += (horizontalDiff > 0) ? MOVEMENT_STEP : -MOVEMENT_STEP;
    }

    if (abs(verticalDiff) > DEADZONE) {
        tiltAngle += (verticalDiff > 0) ? -MOVEMENT_STEP : MOVEMENT_STEP; 
    }

    moveServos();

    if (millis() - lastDebugTime >= DEBUG_INTERVAL) {
        Serial.println("\n--- Debug Info ---");
        Serial.printf("Raw ADC: TL=%d, TR=%d, BL=%d, BR=%d\n", topLeft, topRight, bottomLeft, bottomRight);
        Serial.printf("Normalized: TL=%d, TR=%d, BL=%d, BR=%d\n", tl, tr, bl, br);
        Serial.printf("Diff: H=%d, V=%d\n", horizontalDiff, verticalDiff);
        Serial.printf("Servos: Yaw=%d, Tilt=%d\n", lastYawAngle, lastTiltAngle);
        Serial.println("-------------------");
        lastDebugTime = millis();
    }

    delay(20);
}

/* END CODE */