#include <ESP32Servo.h>

// Servo objects and pins
Servo servo_horizontal;
Servo servo_vertical;
const int SERVO_H_PIN = 12;  // Horizontal servo (azimuth)
const int SERVO_V_PIN = 13;  // Vertical servo (elevation)

// LDR input pins (using potentiometers to simulate LDRs)
const int LDR_TOP = 35;
const int LDR_BOTTOM = 34;
const int LDR_LEFT = 33;
const int LDR_RIGHT = 32;

// ACS712 current sensor pin (GPIO26 for measurement)
const int ACS_PIN = 26;
const int ACS_PIN2 = 25;
const float ACS_SENSITIVITY = 0.185; // 185mV/A for 5A module

// Servo angle limits
int servo_h_angle = 90;  // Start at center
int servo_v_angle = 90;  // Start at center
const int MIN_ANGLE = 0;
const int MAX_ANGLE = 180;
const int SERVO_STEP = 2;  // Degrees per adjustment
const int DEAD_ZONE = 50;  // Threshold before moving

// Timing
unsigned long last_update = 0;
const int UPDATE_INTERVAL = 200;  // Update every 200ms

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  // Attach servos
  servo_horizontal.attach(SERVO_H_PIN);
  servo_vertical.attach(SERVO_V_PIN);
  
  // Initialize servos to center
  servo_horizontal.write(90);
  servo_vertical.write(90);
  
  // Setup ADC for LDR readings
  analogSetAttenuation(ADC_11db);
  
  Serial.println("\n=== Dual-Axis Solar Tracker Started ===");
  Serial.println("Initializing tracking...");
}

void loop() {
  unsigned long current_time = millis();
  
  if (current_time - last_update >= UPDATE_INTERVAL) {
    last_update = current_time;
    
    // Read all LDR values
    int ldr_top = analogRead(LDR_TOP);
    int ldr_bottom = analogRead(LDR_BOTTOM);
    int ldr_left = analogRead(LDR_LEFT);
    int ldr_right = analogRead(LDR_RIGHT);
    
    // Calculate average values for error correction
    int top_avg = ldr_top;
    int bottom_avg = ldr_bottom;
    int left_avg = ldr_left;
    int right_avg = ldr_right;
    
    // Calculate vertical and horizontal errors
    int vertical_error = top_avg - bottom_avg;
    int horizontal_error = right_avg - left_avg;
    
    // Update vertical servo (elevation)
    if (abs(vertical_error) > DEAD_ZONE) {
      if (vertical_error > 0) {
        servo_v_angle = constrain(servo_v_angle + SERVO_STEP, MIN_ANGLE, MAX_ANGLE);
      } else {
        servo_v_angle = constrain(servo_v_angle - SERVO_STEP, MIN_ANGLE, MAX_ANGLE);
      }
    }
    
    // Update horizontal servo (azimuth)
    if (abs(horizontal_error) > DEAD_ZONE) {
      if (horizontal_error > 0) {
        servo_h_angle = constrain(servo_h_angle + SERVO_STEP, MIN_ANGLE, MAX_ANGLE);
      } else {
        servo_h_angle = constrain(servo_h_angle - SERVO_STEP, MIN_ANGLE, MAX_ANGLE);
      }
    }
    
    // Write servo positions
    servo_horizontal.write(servo_h_angle);
    servo_vertical.write(servo_v_angle);
    
    // Read current sensor (ACS712)
    int acs_raw = analogRead(ACS_PIN);
    int acs_raw2 = analogRead(ACS_PIN2);
    float acs_voltage = (acs_raw / 4095.0) * 3.3;
    float acs_current = (acs_voltage - 1.65) / ACS_SENSITIVITY;
    
    // Output data for monitoring
    Serial.print("Time: "); Serial.print(current_time / 1000); Serial.print("s | ");
    Serial.print("LDR[T,B,L,R]: ");
    Serial.print(ldr_top); Serial.print(",");
    Serial.print(ldr_bottom); Serial.print(",");
    Serial.print(ldr_left); Serial.print(",");
    Serial.print(ldr_right); Serial.print(" | ");
    Serial.print("Servo[H,V]: ");
    Serial.print(servo_h_angle); Serial.print(",");
    Serial.print(servo_v_angle); Serial.print(" | ");
    Serial.print("Current: ");
    Serial.print(acs_current); Serial.println(" A");
  }
  
  delay(50);
}