#include <PWM.h>
int32_t frequency = 62500;
int32_t frequency2 = 62500;

char charofinput [10]; 
float current1;
float current2;
float voltage1 = 1;
float voltage2;
float average;
float volt;
String l1;
String l2;
String l1V = "V1 ";
String l1I = " I1 ";
String l2V = "V2 ";
String l2I = " I2 ";

#include <LiquidCrystal.h>

LiquidCrystal lcd(9, 8, 5, 6, 3, 2); //9 is rs, 8 is e, d4 is 5..., d5 is 6, d6 is 3, d7 is 2

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  InitTimersSafe();
  SetPinFrequencySafe(13, frequency);
  SetPinFrequencySafe(4, frequency2);

  //test led
  pinMode(LED_BUILTIN, OUTPUT);

  // force digital pin 24 to high 5v
  pinMode(24, OUTPUT);
  digitalWrite(24, HIGH);
  //Block #1
  pinMode(13,OUTPUT);
  TCCR0B = TCCR0B & B11111000 | B00000001; //62.5KHz
  pinMode(12,OUTPUT);
  //Block #2
  //pinMode(11,INPUT); //V1 IN
  //pinMode(12,INPUT); //V2 IN
  //pinMode(10, INPUT); //I2 IN
  //pinMode(7, INPUT); //I1 IN
  lcd.begin(16,2);
  lcd.clear();
  lcd.println("TEST");
  Serial.println(voltage1);
  delay (2000);
  //Serial.println("Ready for serial input");
}

void loop() {
  //Block #1
  if (Serial.available() > 0){  //Channel 1 check (PORT 5)
    String cmd = Serial.readStringUntil('\r');
    if(cmd == "0"){
      analogWrite(13, 0); //Channel is off
      Serial.println("A, 0% Duty Cycle");
    }
    else if(cmd == "a"){ 
      Serial.println("A, Retain Pulse Width Output");
    }
    else if(cmd == "b"){ 
      //pwmWrite(13, 6);  //test
      analogWrite(13, 1); //2V desired output 2V/30V = 1/15, x/255 = 1/15, x /= 17
      Serial.println("A, 6.67% Duty Cycle");
      //digitalWrite(LED_BUILTIN, HIGH);
    }
    else if(cmd == "c"){ 
      analogWrite(13, 21);  //2.5V 2.5V/30V = 8.3% * 255 /= 22
      Serial.println("A, 8.33% Duty Cycle");
    }
    else if(cmd == "d"){ 
      analogWrite(13, 5); //Desired Voltage is 3V // 10% Approx. 25.5
      Serial.println("A, 10% Duty Cycle");
    }
    else if(cmd == "e"){
      analogWrite(13, 28);  //Desired voltage is 3.3V 
      Serial.println("A, 11% Duty Cycle");
    }
    else if(cmd == "f"){ 
      analogWrite(13, 35); //Desired Voltage is 4V
      Serial.println("A, 13.33% Duty Cycle");
    }
    else if(cmd == "g"){
      analogWrite(13, 39); //Desired Voltage is 4.5V
      Serial.println("A, 15% Duty Cycle");
    }
    else if(cmd == "h"){
      analogWrite(13, 43);  //Desired VOltage is 5V
      Serial.println("A, 16.66% Duty Cycle");
    }
    else if(cmd == "i"){
      analogWrite(13, 47); //Desired Voltage is 5.5V
      Serial.println("A, 18.33% Duty Cycle");
    }
    else if(cmd == "j"){
      analogWrite(13, 51);  //Desired voltage is 6V
      Serial.println("A, 20% Duty Cycle");
    }
    else if(cmd == "k"){
      analogWrite(13, 55); //Desired Voltage is 6.5V
      Serial.println("A, 21.66% Duty Cycle");
    }
    else if(cmd == "l"){
      analogWrite(13, 60); //Desired Volrtage is 7V (FROM BUCK CONVERTER)
      Serial.println("A, 23.33% Duty Cycle");
    }
    else if(cmd == "m"){
      analogWrite(13, 64); //Desired Voltage is 8.5V
      Serial.println("A, 25% Duty Cycle");
    }
    else if(cmd == "n"){ 
      analogWrite(13, 68); //Desired Voltage is 8V
      Serial.println("A, 26.67% Duty Cycle");
      //digitalWrite(LED_BUILTIN, HIGH);
    }
    else if(cmd == "o"){ 
      analogWrite(13, 72);  //Desired VOltage is 8.5V
      Serial.println("A, 28.33% Duty Cycle");
    }
    else if(cmd == "p"){ 
      analogWrite(13, 77); //Desired Voltage is 9V
      Serial.println("A, 30% Duty Cycle");
    }
    else if(cmd == "q"){
      analogWrite(13, 81);  //Desired voltage is 9.5V
      Serial.println("A, 31.67% Duty Cycle");
    }
    else if(cmd == "r"){ 
      analogWrite(13, 85); //Desired Voltage is 10V
      Serial.println("A, 33.33% Duty Cycle");
    }
    else if(cmd == "s"){ 
      analogWrite(13, 89); //Desired Volrtage is 10.5V (FROM BUCK CONVERTER)
      Serial.println("A, 35% Duty Cycle");
    }
    else if(cmd == "t"){ 
      analogWrite(13, 94); //Desired Voltage is 11V
      Serial.println("A, 36.67% Duty Cycle");
    }
    else if(cmd == "u"){ 
      analogWrite(13, 98); //Desired Volrtage is 11.5V (FROM BUCK CONVERTER)
      Serial.println("A, 38.33% Duty Cycle");
    }
    else if(cmd == "v"){ 
      analogWrite(13, 102); //Desired Voltage is 12V
      Serial.println("A, 40% Duty Cycle");
    }
    else if(cmd == "w"){ 
      analogWrite(13, 106); //Desired Voltage is 12.5V
      Serial.println("A, 41.67% Duty Cycle");
    }
    else if(cmd == "x"){ 
      analogWrite(13, 111);  //Desired VOltage is 13V
      Serial.println("A, 43.33% Duty Cycle");
    }
    else if(cmd == "y"){ 
      analogWrite(13, 115); //Desired Voltage is 13.5V
      Serial.println("A, 45% Duty Cycle");
    }
    else if(cmd == "z"){
      analogWrite(13, 200);  //Desired voltage is 14V
      Serial.println("A, 200/255 Duty Cycle");
    }
      //channel 2
    else if(cmd == "1"){
      analogWrite(4, 0); //Channel is off
      Serial.println("B, 0% Duty Cycle");
    }
    else if(cmd == "A"){ 
      Serial.println("B, Retain PWM output");
    }
    else if(cmd == "B"){ 
      analogWrite(4, 17); //2V desired output 2V/30V = 1/15, x/255 = 1/15, x /= 17
      Serial.println("B, 6.67% Duty Cycle");
      digitalWrite(LED_BUILTIN, LOW);
    }
    else if(cmd == "C"){ 
      analogWrite(4, 21);  //2.5V 2.5V/30V = 8.3% * 255 /= 22
      Serial.println("B, 8.33% Duty Cycle");
    }
    else if(cmd == "D"){ 
      analogWrite(4, 26); //Desired Voltage is 3V // 10% Approx. 25.5
      Serial.println("B, 10% Duty Cycle");
    }
    else if(cmd == "E"){ 
      analogWrite(4, 28);  //Desired voltage is 3.3V 
      Serial.println("B, 11% Duty Cycle");
    }
    else if(cmd == "F"){
      analogWrite(4, 35); //Desired Voltage is 4V
      Serial.println("B, 13.33% Duty Cycle");
    }
    else if(cmd == "G"){ 
      analogWrite(4, 39); //Desired Voltage is 4.5V
      Serial.println("B, 15% Duty Cycle");
    }
    else if(cmd == "H"){ 
      analogWrite(4, 43);  //Desired VOltage is 5V
      Serial.println("B, 16.66% Duty Cycle");
    }
    else if(cmd == "I"){ 
      analogWrite(4, 47); //Desired Voltage is 5.5V
      Serial.println("B, 18.33% Duty Cycle");
    }
    else if(cmd == "J"){ 
      analogWrite(4, 51);  //Desired voltage is 6V
      Serial.println("B, 20% Duty Cycle");
    }
    else if(cmd == "K"){
      analogWrite(4, 55); //Desired Voltage is 6.5V
      Serial.println("B, 21.66% Duty Cycle");
    }
    else if(cmd == "L"){
      analogWrite(4, 60); //Desired Volrtage is 7V (FROM BUCK CONVERTER)
      Serial.println("B, 23.33% Duty Cycle");
    }
    else if(cmd == "M"){
      analogWrite(4, 64); //Desired Voltage is 8.5V
      Serial.println("B, 25% Duty Cycle");
    }
    else if(cmd == "N"){
      analogWrite(4, 68); //Desired Voltage is 8V
      Serial.println("B, 26.67% Duty Cycle");
    }
    else if(cmd == "O"){ 
      analogWrite(4, 72);  //Desired VOltage is 8.5V
      Serial.println("B, 28.33% Duty Cycle");
    }
    else if(cmd == "P"){ 
      analogWrite(4, 77); //Desired Voltage is 9V
      Serial.println("B, 30% Duty Cycle");
    }
    else if(cmd == "Q"){
      analogWrite(4, 81);  //Desired voltage is 9.5V
      Serial.println("B, 31.67% Duty Cycle");
    }
    else if(cmd == "R"){ 
      analogWrite(4, 85); //Desired Voltage is 10V
      Serial.println("B, 33.33% Duty Cycle");
    }
    else if(cmd == "S"){
      analogWrite(4, 89); //Desired Volrtage is 10.5V (FROM BUCK CONVERTER)
      Serial.println("B, 35% Duty Cycle");
    }
    else if(cmd == "T"){ 
      analogWrite(4, 94); //Desired Voltage is 11V
      Serial.println("B, 36.67% Duty Cycle");
    }
    else if(cmd == "U"){
      analogWrite(4, 98); //Desired Volrtage is 11.5V (FROM BUCK CONVERTER)
      Serial.println("B, 38.33% Duty Cycle");
    }
    else if(cmd == "V"){
      analogWrite(4, 102); //Desired Voltage is 12V
      Serial.println("B, 40% Duty Cycle");
    }
    else if(cmd == "W"){ 
      analogWrite(4, 106); //Desired Voltage is 12.5V
      Serial.println("B, 41.67% Duty Cycle");
    }
    else if(cmd == "X"){ 
      analogWrite(4, 111);  //Desired VOltage is 13V
      Serial.println("B, 43.33% Duty Cycle");
    }
    else if(cmd == "Y"){
      analogWrite(4, 115); //Desired Voltage is 13.5V
      Serial.println("B, 45% Duty Cycle");
    }
    else if(cmd == "Z"){ 
      analogWrite(4, 119);  //Desired voltage is 14V
      Serial.println("B, 46.67% Duty Cycle");
      digitalWrite(LED_BUILTIN, LOW);
    }
    else if(cmd == "!"){
      delay(500);    //delay to let system settle changes before trying to update the display
      //digitalWrite(LED_BUILTIN, HIGH);  //test
      updateLCD();    //update LCD with new values 
    }
  } 
}

void updateLCD(){
//Set variables from current and voltage sense
  lcd.clear();
  Serial.println("Updating...");
  average = 0;
  for (int i = 0; i < 100; i++){//Gather samples to average current1
    volt = (float)5.0/1024 * analogRead(A2);
    Serial.println(volt);
    average =  average + 5.4 * (volt-2.5); //5.405 to swamp from voltage reading to current value
    delay(1);
  }
  current1 = average/100.00; //100.0 to get 2 decimal places

  average = 0;
  for(int i = 0;i < 100; i++){//Gather samples to average current2
    
    average = average + 5.4 * ((float)5.0/1024 * analogRead(A3) - 2.5); //0-1023 options * 5 for range 0 - 5V then * 5.405 to get current
    delay(1); //Let adc settle
  }
  current2 = average/100.00;
  Serial.println(current1);

  voltage1 = (float)1/1024 * 5 * 5 * analogRead(A0); //simply voltage divider
  voltage2 = (float)1/1024 * 5 * 5 * analogRead(A1); //V2 is A1   
  //Print to LCD
  l1 = l1V + voltage1 + l1I + current1;
  lcd.setCursor(0, 0);//ln1
  lcd.println(l1);
  l2 = l2V + voltage2 + l2I + current2;
  lcd.setCursor(0, 1);//ln2
  lcd.println(l2);
  //LED illumination if statements
  if (current1 >= 1.3){
    analogWrite(11,128);//50% duty cycle on PWM port 1
  }if (current1 < 1.3){
    analogWrite(11,0);//0% duty cycle on PWM port 1    
  }
  if (current2 >= 1.3){
    analogWrite(12,128);//50% duty cycle on PWM port 2
  }if (current2 < 1.3){
    analogWrite(12,0);//0% duty cycle on PWM port 2
  }
  if(current1 >= 1.5){
    analogWrite(13,0); //turn off pwm
  }
  if(current2 >= 1.5){
    analogWrite(4,0);  //turn off pwm
}
}