#include <LiquidCrystal_I2C.h>
#include "BTS7960.h" //Arduino-BTS7960

LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 20, 4);


byte START_DIV_0_OF_1[8] = {  B01111,   B10000,  B10000,  B10000,  B10000,  B10000,  B10000,  B01111}; 
byte START_DIV_1_OF_1[8] = {  B01111,   B10000,  B10111,  B10111,  B10111,  B10111,  B10000,  B01111};
byte DIV_0_OF_1[8] = {  B11111,   B00000,  B00000,  B00000,  B00000,  B00000,  B00000,  B11111}; 
byte DIV_1_OF_1[8] = {  B11111,   B00000,  B11111,  B11111,  B11111,  B11111,  B00000,  B11111};
byte END_DIV_0_OF_1[8] = {  B11100,   B00010,  B00011,  B00011,  B00011,  B00011,  B00010,  B11100};
byte END_DIV_1_OF_1[8] = {  B11100,   B00010,  B11011,  B11011,  B11011,  B11011,  B00010,  B11100};
byte frontArrowSX[8] = {  B00001,  B00001,  B00011,  B00011,  B00111,  B00111,  B01111,  B11111};
byte frontArrowDX[8] = {  B10000,  B10000,  B11000,  B11000,  B11100,  B11100,  B11110,  B11111};


char buffer [20];
int battMinVolt = 23.70;
int battMaxVolt = 25.60;
float voltage;
float current;
float maxCurrent=45.0;
int speed;
float tempMotor;
float tempDriver;


//OUT
#define pinL_PWM 8
#define pinR_PWM 7
#define pinEN
//IN
#define pinDirL
#define pinDirR
#define pinSwitchThrottle 4
#define pinSwitchBrake 3
#define pinPowerOn 2
#define pinPotThrottle
//Analog IN
#define pinVoltage 
#define pinCurrent 
#define pinSpeedMeter 
#define pinTemp

BTS7960 motorController(pinEN, pinL_PWM, pinR_PWM);





bool stateSwitchThrottle;
bool stateSwitchBrake;
bool statePowerOn;
bool stateDirL;
bool stateDirR;

int potThrottle;
uint8_t stepStatus;
bool enableThrottle;



void setup() {

  lcd.init();
  lcd.backlight();
  Serial.begin(115200);
  pinMode(5, INPUT_PULLUP);

//INPUT
  
  pinMode(pinSwitchThrottle, INPUT_PULLUP);
  pinMode(pinSwitchBrake, INPUT_PULLUP);
  pinMode(pinPowerOn , INPUT_PULLUP);
  pinMode(pinSpeedMeter, INPUT_PULLUP);
  pinMode(pinDirL, INPUT_PULLUP);
  pinMode(pinDirR, INPUT_PULLUP);

  lcd.createChar(0, START_DIV_0_OF_1);
  lcd.createChar(1, START_DIV_1_OF_1);
  lcd.createChar(2, DIV_0_OF_1);
  lcd.createChar(3, DIV_1_OF_1);
  lcd.createChar(4, END_DIV_0_OF_1);
  lcd.createChar(5, END_DIV_1_OF_1);

}


void loop() {


readPin();
dashPrint();


switch (status) {

    //macchina alimentata ferma in OFF
    case 0:
      if (statePowerOn) stepStatus = 1;
      printPrintStatus(6);//Display "STATO OFF"
      break;

    //Macchina in ON check generale
    case 1:
      if (!statePowerOn) stepStatus = 0;
      printPrintStatus(1);//Display "START OK"

      /*
      if (stateSwitchThrottle and SelectorDirection != 0) {
        stepStatus = 2;
        */
      }
      break;



    //Macchina pronta a partire
    case 2:
      //Display "PRONTI A PARTIRE"

      //Condizoni di funzionamenti con interblocco freno. Ripristino con throttle 0 e freno a R
      //Se viene azionato il freno con l'acceleratore in posizione maggiore di 0, interrompi la potenza e ripristina solo se il freno e lacceleratore vengono riportati in posizione 0
      if (stateSwitchThrottle and SelectorDirection() != 0) {
        enableThrottle = true;
      } else{
        enableThrottle = false;
      }

      
      analogWrite(pinPWM_Throttle, Throttle(enableThrottle));

      break;




}





void dashPrint(){
  voltage = (float)analogRead(4)/4095*30.00*(27000/27000);
  current = (float)analogRead(2)/4095*45.00*(27000/27000);
  speed = map(analogRead(2),0,4095,0,20);
  tempMotor = map(analogRead(15),0,4095,0,100);

    //Stampa su LCD
  drawProgressBar(voltage, 5, 0, 10);
  printPercentage(16,0,voltage);
  printVoltage(0,3,voltage);
  printCurrent(8,3,current);
  printPower(15,3, voltage, current);
  printTemperature(0,0,tempMotor);

  bool buttonState = digitalRead(5);
  printDirection(0,2,buttonState,0);

  printSpeed(7,2,speed);
}



void printPrintStatus (int value){
  const int xpos = 0;
  const int ypos = 1;
  switch(value){
   case 0:
    lcd.setCursor(xpos,ypos);
    lcd.print("     STATO OFF      ");
    break;

  case 1:
    lcd.setCursor(xpos,ypos);
    lcd.print("      START OK      ");
    break;

  case 2:
    lcd.setCursor(xpos,ypos);
    lcd.print("  BATTERIA SCARICA! ");
    break;

  case 3:
    lcd.setCursor(xpos,ypos);
    lcd.print("ALLARME TEMPERATURA!");
    break;

  case 4:
    lcd.setCursor(xpos,ypos);
    lcd.print("   GUASTO DRIVER!  ");
    break;
    
  case 5:
    lcd.setCursor(xpos,ypos);
    lcd.print("BATTERIA IN CARICA ");
    break;
    }

  case 6:
    lcd.setCursor(xpos,ypos);
    lcd.print("     STATO OFF     ");
    break;
}

void printSpeed (int xpos, int ypos, float value){
  lcd.setCursor(xpos,ypos);
  dtostrf (value, 2,0,buffer);
  lcd.print (String(buffer) + "km/h");
}

void printVoltage (int xpos, int ypos, float value){
  lcd.setCursor(xpos,ypos);
  dtostrf (value, 4,1,buffer);
  lcd.print (String(buffer) + "V");
}
void printCurrent (int xpos, int ypos, float value){
  if(value<maxCurrent){
    lcd.setCursor(xpos,ypos);
    dtostrf (value, 4,1, buffer);
    lcd.print (String(buffer) + "A");
  }
}

void printPower (int xpos, int ypos, float voltage, float current){
  float power = voltage*current;
  lcd.setCursor(xpos,ypos);
  dtostrf (power, 4,0,buffer);
  lcd.print (String(buffer) + "W");
}
void printPercentage (int xpos, int ypos, float value){
  int valueVoltPerc = constrain(map(value*1000,battMinVolt*1000,battMaxVolt*1000,0,100),0,100);
  lcd.setCursor(xpos,ypos);
  dtostrf (valueVoltPerc, 3,0, buffer);
  lcd.print (String(buffer) + "%");
}
void drawProgressBar(float value, int xpos, int ypos, int lengthBar ) {
 
  int percent = constrain(map(value*1000,battMinVolt*1000,battMaxVolt*1000,0,100),0,100);
  lcd.setCursor(xpos, ypos);
 
  byte nb_columns = map(percent, 0, 100, 0, lengthBar);

  for (byte i = 0; i < lengthBar; ++i) {

    if (i == 0) {
      if (nb_columns > 0) {
        lcd.write(1);
        nb_columns -= 1;
      } else {
        lcd.write(0); 
      }

    } else if (i == lengthBar -1) {
      if (nb_columns > 0) {
        lcd.write(5); 
      } else {
        lcd.write(4); 
      }

    } else {
      if (nb_columns >= 1) {
        lcd.write(3); 
        nb_columns -= 1;
      } else {
        lcd.write(2);
      }
    }
  }
}
void printTemperature (int xpos, int ypos, float value){
  lcd.setCursor(xpos,ypos);
  dtostrf (value, 2,0,buffer);
  lcd.print (String(buffer) + (char)223 + "C");
}

void printDirection (int xpos, int ypos, bool dir, bool enable){
  lcd.setCursor(xpos,ypos);
  if(enable){
    if(dir){
      lcd.print ("FWD"); 
    }else{
     lcd.print ("REV");
    }
  }else{
    lcd.print ("OFF");
  }
}


void checkStatus() {
  stateSwitchThrottle = !digitalRead(pinSwitchThrottle);
  stateSwitchBrake = !digitalRead(pinSwitchBrake);
  statePowerOn = !digitalRead(pinPowerOn);
  stateDirL = !digitalRead(pinDirL);
  stateDirR = !digitalRead(pinDirR);
  potThrottle = analogRead(pinPotThrottle);
}


uint8_t SelectorDirection() {
  if (!stateDirL and !stateDirR) {
    return 0;
  } else if (stateDirL) {
    return 1;
  } else if (stateDirR) {
    return 2;
  }
}



bool ready;
void Throttle (int valuePot, int dir, bool switchThrottle) {
      
      byte speed = constrain(map(valuePot, 0, 4095, 0, 255), 0, 255);
      if(!switchThrottle and dir == 0){
         ready = true;
      }
      if(switchThrottle and dir == 0){
        ready = false;
      }


      if(dir == 1 and ready){
        motorController.Enable();
        motorController.TurnLeft(speed);

      }else if(dir == 2 and ready){
        motorController.Enable();
       motorController.TurnRight(speed);

      }else{
      motorController.Disable();
    }

         
}