#include <LiquidCrystal.h>
const int rs = 8, enable = 10, d0 =9, d1 = 13, d2 = 2, d3 = 3, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
LiquidCrystal lcd(rs, enable, d4, d5, d6, d7);

int ARDUINO_HIGH = 11;
int ARDUINO_LOW = 12;
int StroomPin = A0; //maal 3 doen voor stroom
int VoltagePin = A2; //schalen naar 12 dus *12/5

//ver als test voor het echte Vermogen + verander ver in loop naar Vermogen
float volt = 9.2894;

void setup() {
  lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  pinMode(StroomPin, INPUT);
  pinMode(VoltagePin, INPUT);
  pinMode(ARDUINO_HIGH, OUTPUT);
  pinMode(ARDUINO_LOW, OUTPUT);
  Serial.begin(9600);

}

void loop() {
  Backwards();
  delay(5000);
}


void Backwards(){
  lcd.clear();
  float voltage = MeetVoltage();
  String BitString = Encode(voltage);
  //String BitString = Encode(volt); //wegcommenten bij meten
  Serial.println(BitString);
  lcd.setCursor(0,0);
  lcd.print(voltage);
  //lcd.print(volt);
  lcd.setCursor(0,1);
  lcd.print(BitString);
  Bestuur(BitString);
}

float MeetVoltage(){
  float Voltage = analogRead(VoltagePin) * (12/1023.0);
  return Voltage;
}


float MeetVermogen(){
  float Stroom = FilterStroom();
  float Voltage = analogRead(VoltagePin) * (12/1023.0);
  float Vermogen = Stroom * Voltage;
  return Vermogen;
}

void Bestuur(String bitstring){
  for (int i = bitstring.length()-1; i > -1; i--){
    char cbit = bitstring.charAt(i);
    Serial.println(cbit);

    if(cbit == '1'){
      digitalWrite(ARDUINO_HIGH, HIGH);
      digitalWrite(ARDUINO_LOW, LOW);
    }
    else if(cbit == '0'){
      digitalWrite(ARDUINO_LOW, HIGH);
      digitalWrite(ARDUINO_HIGH, LOW);
    }
    else{
      Serial.println("ERROR: BIT WAS NEITHER 1 NOR 0");
    }
    if(digitalRead(ARDUINO_HIGH) == HIGH && digitalRead(ARDUINO_LOW) == HIGH){
      Serial.println("SLECHT: BEIDE HOOG");
    }
    if(digitalRead(ARDUINO_HIGH) == LOW && digitalRead(ARDUINO_LOW) == LOW){
      Serial.println("SLECHT: BEIDE LAAG");
    }
    int HighState = digitalRead(ARDUINO_HIGH);
    int LowState = digitalRead(ARDUINO_LOW);
    Serial.print("ARDUINO_HIGH Pin State: ");
    if (HighState == HIGH) {
    Serial.println("HIGH");
  } else {
    Serial.println("LOW");
  }
    Serial.print("ARDUINO_LOW Pin State: ");
    if (LowState == HIGH) {
    Serial.println("HIGH");
  } else {
    Serial.println("LOW");
  }
   delay(99);
  }
}

//i=4 is 2 kommagetallen
String Encode(float getal) {
    float roundedNum = roundf(getal * 10.0) / 10.0;
    int rounded = int(roundedNum);
    int getalnakomma = int(roundedNum * 10) % 10;
    String binairvoorkomma = String(rounded, BIN);
    String binairvoorkommavol = fillASCII(binairvoorkomma);
    String binairnakomma = String(getalnakomma, BIN);
    String binairnakommavol = fillASCII(binairnakomma);
    String binaircompleet = binairvoorkommavol + binairnakommavol;
  return binaircompleet;
}


String fillASCII(String bitstring){
  while(bitstring.length() < 4){
    bitstring = "0" + bitstring;
  }
  return String(bitstring);
}


float FilterStroom(){
  int i = 0;
  float sum = 0;
  while(i < 10){
      float Stroom = analogRead(StroomPin) * (5.0 / 1023.0) * 3;
      if(Stroom != 0){
        sum = sum + Stroom;
        i = i + 1;
      }
  }
  return sum/10;
}