/*
Solar_Tracker_1.0 Copy (2 nano)
https://www.youtube.com/watch?v=ehgPL8rRmDY&t=333s$0
https://github.com/BrownDogGadgets/SolarTracker/blob/master/Dual%20Axis%20Tracker/Dual_Axis_Tracker_V3.ino$0

Brown Dog Gadgets <https://www.browndoggadgets.com/>

Hardware:
SG90 9g Micro Servos (2)

I believe this is the best code to use for Chloe's project.
Seems simple enough to understand and isn't overly complicated.

*/

// include Servo library
#include <Servo.h>
#include "SdFat.h"

#define SPI_SPEED SD_SCK_MHZ(4)
#define CS_PIN 10
SdFat sd;

// left / right servo sweep
Servo horizontal;
int servoH = 90;

// start at 90 which is due south
int servoH_limitHigh = 180; // plus 90 = 180
int servoH_limitLow = 0; // minus 90 = 0

// up / down servo sweep
Servo vertical;
int servoV = 90;

// start servo at 90 which is 45 degrees to horizon
int servoV_limitHigh = 135; // plus 45 sweep = 135
int servoV_limitLow = 45; // minus 45 sweep = 45


// LDR pin connections
int ldrTR = A0; // LDR top right
int ldrTL = A1; // LDR top left
int ldrBR = A2; // LDR bottom right
int ldrBL = A3; // LDR bottom left


void setup() {
  Serial.begin(9600);
  if (!sd.begin(CS_PIN, SPI_SPEED)) {
    if (sd.card()->errorCode()) {
      Serial.println("SD initialization failed.");
    } else if (sd.vol()->fatType() == 0) {
      Serial.println("Can't find a valid FAT16/FAT32 partition.");
    } else {
      Serial.println("Can't determine error type");
    }
    return;
  }
  // servo connections
  horizontal.attach(5); // PWM pin 5
  vertical.attach(6); // PWM pim 6
  // move servos
  horizontal.write(90);
  vertical.write(90);

  // reads list of files and file size
  sd.ls(LS_R | LS_SIZE);
  delay(3000);
}


void loop() {

  int tr = analogRead(ldrTR); // top right
  int tl = analogRead(ldrTL); // top left
  int br = analogRead(ldrBR); // bottom right
  int bl = analogRead(ldrBL); // bottom left

  int dtime = 500; // change for debugging only
  int tol = 50;

  int avgUp = (tl + tr) / 2; // average value top
  int avgDown = (bl + br) / 2; // average value bottom
  int avgLeft = (tl + bl) / 2; // average value left
  int avgRight = (tr + br) / 2; // average value right

  int diffVert = avgUp - avgDown; // check the difference of up and down
  int diffHorz = avgLeft - avgRight; // check the difference of left and right


  // send data to the serial monitor if desired
  Serial.print("TR:");
  Serial.print(tr);
  Serial.print(" TL:");
  Serial.print(tl);
  Serial.print(" BR:");
  Serial.print(br);
  Serial.print(" BL:");
  Serial.println(bl);

  Serial.print("avgUp:");
  Serial.print(avgUp);
  Serial.print(" avgDown:");
  Serial.print(avgDown);
  Serial.print(" avgLeft:");
  Serial.print(avgLeft);
  Serial.print(" avgRight:");
  Serial.println(avgRight);

/*
  Serial.print("dtime:");
  Serial.print(dtime);
  Serial.print(" tol:");
  Serial.println(tol);
*/

  Serial.print("servoV:");
  Serial.print(servoV);
  Serial.print(" servoH:");
  Serial.println(servoH);


  // check if the difference is in the tolerance else change vertical angle
  if (-1 * tol > diffVert || diffVert > tol) {
    if (avgUp > avgDown) {
      servoV = ++servoV;
      if (servoV > servoV_limitHigh) {
        servoV = servoV_limitHigh;
      }
    }
    else if (avgUp < avgDown) {
      servoV = --servoV;
      if (servoV < servoV_limitLow) {
        servoV = servoV_limitLow;
      }
    }
    vertical.write(servoV);
  }

  // check if the difference is in the tolerance else change horizontal angle
  if (-1 * tol > diffHorz || diffHorz > tol) {
    if (avgLeft > avgRight) {
      servoH = --servoH;
      if (servoH < servoH_limitLow) {
        servoH = servoH_limitLow;
      }
    }
    else if (avgLeft < avgRight) {
      servoH = ++servoH;
      if (servoH > servoH_limitHigh) {
        servoH = servoH_limitHigh;
      }
    }
    else if (avgLeft = avgRight) {
      // nothing
    }
    horizontal.write(servoH);
  }
  
  delay(dtime);
  
}