#include <Servo.h>
#include <LowPower.h>

// Define the pin numbers for the servos
const int servo_base = 10;
const int servo_panel = 11;

// Create servo objects
Servo servo_b;
Servo servo_p;

// Define the range of angles. panel contrained based on geometry 
const int minAngle_b = 0;
const int maxAngle_b = 180;
const int minAngle_p = 40;
const int maxAngle_p = 140;

//max angle and max sensor reading. global variable for the rest of the logic
float angle_b_max = 0;
float angle_p_max = 0;
float avgValue_max = 0;
float lux_c = 0;

// Define the step increment
const int stepIncrement_b = 36;
const int stepIncrement_p = 25;
const int stepIncrement_b_micro = 5;
const int stepIncrement_p_micro = 5;

// define lighting factors
const float lux_factor =1.1; // how much higher lux needs to be to move
const float lux_reset = 0.3; // how much max from sweep needs to decrease to run again 
const float low_lux_int = 75; // voltage is if avg ldr reading below 0.388V

// define sample number
const int samples =3;

//delay constant
const int delay_s = 100; // for measurement sample
const int delay_m = 225; // for move
const int delay_sleep = 250; // for move

//Define digital pins for ldr
const int digitalPin_LDR1 = 2; // Digital LDR 1
const int digitalPin_LDR2 = 3; // Digital LDR 2
const int digitalPin_LDR3 = 4; // Digital LDR 3
const int digitalPin_LDR4 = 5; // Digital LDR 4
const int analog_LDR_1 = A0;  // Analog pin number for D2
const int analog_LDR_2 = A1;  // Analog pin number for D3
const int analog_LDR_3 = A2;  // Analog pin number for D4
const int analog_LDR_4 = A3;  // Analog pin number for D5


void setup() {
  // Attach the servos to their respective pins
  servo_b.attach(servo_base);
  servo_p.attach(servo_panel);

  // Move the servos to the starting position
  servo_b.write(minAngle_b);
  servo_p.write(minAngle_p);

  // Initialize the digital pin as an output
  pinMode(digitalPin_LDR1, OUTPUT);
  pinMode(digitalPin_LDR2, OUTPUT);
  pinMode(digitalPin_LDR3, OUTPUT);
  pinMode(digitalPin_LDR4, OUTPUT);

  // Wait a bit for the servos to reach the starting position
  delay(delay_m);

  // Initialize serial communication at 9600 baud rate
  Serial.begin(9600);
  
  // Initial call to sweep function
  sweepServos();
  
  
}

void loop() {

  // calls functions to check space near optimal
  checkAndAdjustServos( stepIncrement_b_micro, stepIncrement_p_micro, lux_factor,  delay_m);
 
  //if current poisiton has decrease below reset value then rerun sweep.
  lux_c = sample_LDR(); 
  if (lux_c < avgValue_max*lux_reset) {
    sweepServos();
  }

  //night time detection. sleeps for a long time then sweeps
  if (lux_c < low_lux_int) {
    //sleep.pwrDownMode(); //set sleep mode
    //sleep.sleepDelay(3600000); //sleep for an hour
    sweepServos();
  }

}

// Function to sample an analog input pin 'n' times and average the values
float sampleAndAverage(int pin, int samples) {
  long total = 0;
  for (int i = 0; i < samples; i++) {
    total += analogRead(pin);  // Read the analog value and add to total
    delay(delay_s/samples);  // Small delay between readings (adjust if needed)
  }
  return total / (float)samples;  // Return the average value
}

// sweeps space - finds highest lux location for solar panel. calls this on startup
void sweepServos() {

 // loops through panel angles then goes thorugh base
  for(int angle_p = minAngle_p; angle_p <= maxAngle_p; angle_p += stepIncrement_p){
    servo_p.write(angle_p);
    delay(delay_m);
    
    for(int angle_b = minAngle_b; angle_b <= maxAngle_b; angle_b += stepIncrement_b){
      servo_b.write(angle_b);
      
      delay(delay_m);

      // Turn on the digital pins for data
      digitalWrite(digitalPin_LDR1, HIGH);
      digitalWrite(digitalPin_LDR2, HIGH);
      digitalWrite(digitalPin_LDR3, HIGH);
      digitalWrite(digitalPin_LDR4, HIGH);
      delay(delay_s);
      
      float avgValue_1 = sampleAndAverage(analog_LDR_1, samples);
      digitalWrite(digitalPin_LDR1, LOW);
      delay(delay_s);

      float avgValue_2 = sampleAndAverage(analog_LDR_2, samples);
      digitalWrite(digitalPin_LDR2, LOW);
      delay(delay_s);

      float avgValue_3 = sampleAndAverage(analog_LDR_3, samples);
      digitalWrite(digitalPin_LDR3, LOW);
      delay(delay_s);

      float avgValue_4 = sampleAndAverage(analog_LDR_4, samples);
      digitalWrite(digitalPin_LDR4, LOW);
      delay(delay_s);


      if (avgValue_max < (avgValue_1+avgValue_2+avgValue_3+avgValue_4)/4){
        avgValue_max = (avgValue_1+avgValue_2+avgValue_3+avgValue_4)/4;
        angle_b_max = angle_b;
        angle_p_max = angle_p;
      }
    }
  
  }
  // commmand to max
  servo_b.write(angle_b_max);
  servo_p.write(angle_p_max);
}

// sample LDR - all 4 pins
float sample_LDR() {
  delay(delay_s);
  // Turn on the digital pins
  digitalWrite(digitalPin_LDR1, HIGH);
  digitalWrite(digitalPin_LDR2, HIGH);
  digitalWrite(digitalPin_LDR3, HIGH);
  digitalWrite(digitalPin_LDR4, HIGH);

  delay(delay_s);
  float avgValue_1 = sampleAndAverage(analog_LDR_1, samples);
  digitalWrite(digitalPin_LDR1, LOW);
  delay(delay_s);

  float avgValue_2 = sampleAndAverage(analog_LDR_2, samples);
  digitalWrite(digitalPin_LDR2, LOW);
  delay(delay_s);

  float avgValue_3 = sampleAndAverage(analog_LDR_3, samples);
  digitalWrite(digitalPin_LDR3, LOW);
  delay(delay_s);

  float avgValue_4 = sampleAndAverage(analog_LDR_4, samples);
  digitalWrite(digitalPin_LDR4, LOW);
  delay(delay_s);

  float avg_lux = (avgValue_1+avgValue_2+avgValue_3+avgValue_4)/4;

  return avg_lux;  
}
  
// Function to perform incremental checks and adjust servo angles based on light source movement
void checkAndAdjustServos( float stepIncrement_b_micro, float stepIncrement_p_micro, float lux_factor, int delay_m) {
  float angle_b_c = angle_b_max;
  float angle_p_c = angle_p_max;

  // Put Arduino to sleep for 10 minutes (600 seconds)
  for (int i = 0; i < 1; i++) { // 60 times 10 seconds = 600 seconds (10 minutes)
    LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF);
    delay(2000); // Sleep 8 seconds, then wait 2 seconds
  }
  //sleep.pwrDownMode(); //set sleep mode
  //sleep.sleepDelay(1800000); //sleep for half hour
  
  // Code below does incremental checks of space and index up or down if light source is moving
  lux_c = sample_LDR(); // Finds value of current position
 
  
  servo_p.write(angle_p_max + stepIncrement_p_micro); // Move panel up
  delay(delay_m);
  float lux_p_plus = sample_LDR(); // Find up panel position lux
  
  if (lux_p_plus > lux_c * lux_factor) {
    angle_p_c = angle_p_max + stepIncrement_p_micro;
  }

  servo_p.write(angle_p_max - stepIncrement_p_micro); // Move panel down
  delay(delay_m);
  float lux_p_minus = sample_LDR();

  if (lux_p_minus > lux_c * lux_factor) {
    angle_p_c = angle_p_max - stepIncrement_p_micro;
  }
  
  angle_b_max = angle_b_c;
  angle_p_max = angle_p_c;

  servo_b.write(angle_b_max); // Set base servo to the final position
  delay(delay_m);
  servo_p.write(angle_p_max); // Set panel servo to the final position

  
}