#include <LiquidCrystal.h>
#include <Servo.h>
Servo myservo;
LiquidCrystal lcd(12,11,10,9,8,7);

bool solar_moving;
bool solar_static;
bool direction;
int position;
float trackerP;
float staticP;
int force;
unsigned long displayTime = millis();
unsigned long servoTime = millis();
unsigned long interTime = millis();

void move(float pos){
  myservo.attach(4);
  myservo.write(pos);
  delay(500);
  myservo.detach();
}

void tracking_mode(){
  int interval = 0;
  trackerP = 0;
  int mode = 0;
  while (digitalRead(5) == HIGH && mode == 0){
    float resVoltage = analogRead(A1)/1024.*5;
    float solarCurrent = resVoltage/100;
    float resistance = 2000*resVoltage/(1-resVoltage/5);
    float instPower = resVoltage*solarCurrent;
    float photoValue = analogRead(A0);
    instant(1);
    unsigned long currentTime = millis();
    if (servoTime - currentTime > 1000){
      if (analogRead(A0) < analogRead(A2) && position > 0){
        position -= 5;
        move(position);
      }
      else if (analogRead(A0) > analogRead(A2) && position < 1800){
        position += 5;
        move(position);
      }
      else{
        servoTime = currentTime;
      }
     }
  }

  lcd.setCursor(0,0);
  
  lcd.print("Total Power ");
  lcd.setCursor(0,1);
  lcd.print(trackerP);
  delay(5000);
  mode = 1;
  lcd.clear();

  solar_moving = 0;


}


void setup() {
  trackerP = 0;
  force = 0;
  lcd.begin(16,2);
  lcd.clear();
  position = 90;
  move(90);
  pinMode(6,INPUT_PULLUP);
  pinMode(5,INPUT_PULLUP);
  pinMode(4,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(A0,INPUT);
  solar_moving = 0;
  solar_static = 0;
  direction = 0;

  // put your setup code here, to run once:

}

void static_mode(){
  staticP = 0;
  force = 0;
  if (position != 90){
    move(90);
  }
  while (digitalRead(6)==HIGH){
    instant(2);
    

  }
  lcd.setCursor(0,0);
  lcd.print("Total Power ");
  lcd.setCursor(0,1);
  lcd.print(staticP);
  delay(5000);
  lcd.clear();
  solar_static = 0;
  
}
void instant(int mode){
  float resVoltage = analogRead(A1)/1024.*5;
  float solarCurrent = resVoltage/100;
  float resistance = 2000*resVoltage/(1-resVoltage/5);
  float instPower = resVoltage*solarCurrent;
  
  unsigned long currentTime = millis();
  if (displayTime - currentTime > 500){
    lcd.setCursor(0,0);
    lcd.print(instPower);
    lcd.print("W");
    displayTime = currentTime;
  }
  if (mode == 1){
    trackerP += instPower;
  }
  if (mode == 2){
    staticP += instPower;
  }
}

void loop() {
  lcd.setCursor(0,0);
  lcd.print("Select");
  if (solar_moving == 0 && solar_static == 0){ 
    if (digitalRead(6) == LOW){
      lcd.clear();
      solar_moving = 1;
      delay(500);
      tracking_mode();
      
    }
    if (digitalRead(5) == LOW){
      //solar_static = 1;
      //static_mode();
      interMode();
    }
  }
  if (force == 1){
    lcd.clear();
    solar_static = 1;
    static_mode();
  }


}
void interMode(){
  lcd.clear();
  int mode = 0;
  unsigned long currentTime = millis();
  interTime = currentTime;
  lcd.setCursor(0,0);
  lcd.print("Press large to ");
  lcd.setCursor(0,1);
  lcd.print("compare or wait.");
  while (currentTime - interTime < 10000){
    currentTime = millis();

    if (digitalRead(6) == 0){
      mode = 1;
      lcd.clear();
      if (trackerP > staticP){
        lcd.print("Tracking >");
        delay(5000);


      }
      else if(trackerP < staticP){
        lcd.print("Static >");
        delay(5000);

  
      }
      else{
        lcd.print("Tracker = Static");
        delay(5000);

        
      }

    }

  }
  if (currentTime - interTime > 10000){
    interTime = currentTime;
    if(mode == 0){
      lcd.clear();
      solar_static = 1;
      static_mode(); 
    }
    else{
      lcd.clear();
    }
  }

}
/* The left button will start the tracker in "moving mode" and stop when the button is pressed again 
The Right button will start the tracker in "static" mode and stop when it is pressed again 
The green LED will tell the user when the system is on. The red LED will turn on when the servo is moving*/