#include <Servo.h>
#include <SPI.h>
#include <SD.h>

File myFile;
 
// change this to match your SD shield or module;
const int chipSelect = 6;
int vSolar1;
float vSolar;
float pSolar;
int photoL;
float Vl;
int photoR;
float Vr;
int photoU;
float Vu;
int photoD;
float Vd;
int difRL;
int UD;
int RL;
float time;

Servo s_right_left;
Servo s_up_down;

void setup() {
  // servo setup
  s_right_left.attach(9);
  s_up_down.attach(10);
  //starting serial for trouble shooting
  Serial.begin(9600);
  // setting initial values for time and rotation
  RL = 90;
  UD = 90;
  time = 0;
// initialising SD card
  Serial.print("Initializing SD card...");

  if (!SD.begin(6)) {

    Serial.println("initialization failed!");

    

  }

  Serial.println("initialization done.");

  if (SD.exists("example.csv")) {

    Serial.println("example.csv exists.");

  } else {

    Serial.println("example.csv doesn't exist.");

  }

  // open a new file and immediately close it:

  Serial.println("Creating example.csv...");

  myFile = SD.open("example.csv", FILE_WRITE);

  myFile.close();

  // Check to see if the file exists:

  if (SD.exists("example.csv")) {

    Serial.println("example.csv exists.");

  } else {

    Serial.println("example.csv doesn't exist.");
  }
    
}

void loop() {
  // opening a file to collect data and assigning pin values to variables
  myFile = SD.open("example.csv", FILE_WRITE);
  photoR = analogRead(A0);
  Vr = (5./1023.)*photoR;
  photoL = analogRead(A1);
  Vl = (5./1023.)*photoL;
  photoU = analogRead(A2);
  Vu = (5./1023.)*photoU;
  photoD = analogRead(A3);
  Vd = (5./1023.)*photoD;


  // calculating difference in horizontal light and then rotating.
  int difRL= photoR - photoL;
  float Vrl = Vr - Vl;

  if((difRL > 20) && (UD <= 90))
  {
    RL = RL - 2;
    s_right_left.write(RL);
  }
  else if( (-20 > difRL) && (UD <= 90) ) 
  {
    RL = RL + 2;
    s_right_left.write(RL);
  }
  // Due to being upside down if the vertical angle is greater than 90 we need to reverse the rotating angles

   else if( (20 < difRL) && (UD > 90) ) 
  {
    RL = RL + 2;
    s_right_left.write(RL);
  }
 else if( (-10 > difRL) && (UD > 90) ) 
  {
    RL = RL - 2;
    s_right_left.write(RL);
  }

 

  // calculating difference in vertical light and rotating.
  int difUD= photoU - photoD;
  float Vud = Vu - Vd;
;
  if(difUD > 20)
  {
    UD = UD + 2;
    s_up_down.write(UD);
  }
  else if(-20 > difUD)
  {
    UD = UD - 2;
    s_up_down.write(UD);
  }


// calculating voltage and power of solar panel
  vSolar1 = analogRead(A4);
  vSolar = (25/15)*(5*(vSolar1/1023));
  Serial.println(vSolar1);
  pSolar = ((vSolar)*(vSolar))/(25000); // using p = V^2/R to calculate power 

// writing it to a file.
  myFile.print(time);
  myFile.print(',');
  myFile.print(Vrl);
  myFile.print(',');
  myFile.print(Vud);
  myFile.print(',');
  myFile.print(vSolar);
  myFile.print(',');
  myFile.print(pSolar);
  myFile.println();
  myFile.close();
  // adding delay to stop it moving too fast and breaking
  delay(20);
  // adds time and constrains the angle to between 0 and 180 (withing servo operating range)
  time = time + 0.02;
  RL = constrain(RL, 2, 178);
  UD = constrain(UD, 2, 178);
}