//sim https://wokwi.com/projects/334742399832031827
#include <LiquidCrystal_I2C.h>
#include "Arduino.h"

LiquidCrystal_I2C lcd(0x27,20,4);//LCD  (Address,Columbs,Rows)

//config 
  //physical inputs 
    const int BATT_SENS[10] = {A0,A1,A2,A3,A4,A5,A6,A7,A8,A9}; //voltage sensor pinout
    const int IN_SENS = A15;//input current sensor pinout
    const int OUT_SENS = A14;//output current sensor pinout
    const int RELAY[4] = {2,3,4,5};// relay pin out
  //system setup
    const int TOTAL_CELL = 4; //how many individual batt
    const int TOTAL_Relay = 4; //how many Relays
    const int  CELL_IN_PACK= 2; //Number of Cells in a Battery (number of sensores)
    const float CURRENT_FACTOR = 0.04; //ACS758LCB-050U current sensing sensativity step
    const float VOLT_FACTOR = 0.02443792; //RCmall FZ0430 reading factor to Volt
  //system parmeters
    const int COUNT_Avg_Volt =100; //numbers of avarages
    const float MIN_VOLT = 11.9; // min charge 11.9
    const float MAX_VOLT = 13.8; // max charge 12.7
    const float MIN_ALLOW = 10; // lower limit of avaliable cell 
    const float MIN_CURRENT = 0.1; // min charging threshold 
    const float HYSTERESIS = 0.2; // bat charge HYSTERESIS
    const int CYCLE_TO_FULL = 10;// wait how many sycle to declare full
    const int DELAY_READ = 1; // delay of each read in microsecond
    const int SYSTEM_DELAY = 1000;//delay betwenn cycles in milisecond 
//calculations    
const int NUMBER_OF_BAT_PACK=TOTAL_CELL/CELL_IN_PACK;
const float Second_to_wait = (((TOTAL_CELL + 2)*COUNT_Avg_Volt*DELAY_READ/1000)+SYSTEM_DELAY/1000)*CYCLE_TO_FULL;

class Class_Bat_Icon{
    
  public:
    byte CharBatState[8] = {// battery battery icon int as empty
      0x0E,
      0x1F,
      0x11,
      0x11,
      0x11,
      0x11,
      0x11,
      0x1F
    };

    byte Skull[8] = {
      0b00000,
      0b01110,
      0b10101,
      0b11011,
      0b01110,
      0b01110,
      0b00000,
      0b00000
    };

  Class_Bat_Icon(float Avg_Batt_Volt) {
    float Batt_Charge_status=0;
    int i=0;
    // Get the value of the charge and reture the charge state 1-5
    if (Avg_Batt_Volt > MAX_VOLT) // check if max volt
        Batt_Charge_status = 5; // set max volt icon
    else
      if (Avg_Batt_Volt < MIN_VOLT) //check if min volt 
        Batt_Charge_status = 0; //set min volt icon
      else
        Batt_Charge_status = (Avg_Batt_Volt - MIN_VOLT)/(MAX_VOLT-MIN_VOLT)*5;//set current charge

    //update the icon accurding to "Batt_Charge_status"
    for (i = 0 ; i <= 5 ; i ++ ){
      if( i <= Batt_Charge_status)
        CharBatState [7-i] = 0x1F; // draw "full" line
      else
        CharBatState [7-i] = 0x11; // draw "empty" line
    }
  }
}; Class_Bat_Icon Batt_Icon[TOTAL_CELL] = Class_Bat_Icon(0);

//all sensors read
class Sens_Read {
  private:
    //Get the sensor location, return avarage "COUNT_Avg_Volt" reads of the analog input 
    float Avarage_read(int sensor_pin){
      double Avg_Volt = 0;
      int i=0;
      for (i = 0 ; i <= COUNT_Avg_Volt; i++){
        Avg_Volt= Avg_Volt + analogRead (sensor_pin);
        delayMicroseconds(DELAY_READ); // delay between reads
      }
      return Avg_Volt/(COUNT_Avg_Volt+1);
    }
  public:
    float Bat_Volt[TOTAL_CELL] = {0};//value of the read voltage of each cell sensor
    float Bat_Avg_charge = 0; //avarage charge
    float In_current = 0; //current into the system 
    float Out_current = 0; // current out of the system

    //constructor for sens
    Sens_Read() {
        int i=0;
        //Read all battery voltage sensor
        for( i = 0 ; i < TOTAL_CELL ; i++){
            Bat_Volt[i] = Avarage_read(BATT_SENS[i])*VOLT_FACTOR;
            if(Bat_Volt[i]>MAX_VOLT) // check if seensor is above max 
              Bat_Avg_charge = Bat_Avg_charge + MAX_VOLT; // set max value
            else
              if(Bat_Volt[i]<MIN_VOLT) // check if sensor is below low 
              Bat_Avg_charge = Bat_Avg_charge + MIN_VOLT;// set low value 
              else
                Bat_Avg_charge = Bat_Avg_charge + Bat_Volt[i]; // set actual value 
        }
        //read currents sensors
        In_current = Avarage_read(IN_SENS)*CURRENT_FACTOR;
        Out_current = Avarage_read(OUT_SENS)*CURRENT_FACTOR;
    }
};Sens_Read Sens_status;



class Class_Batt{
  private:

  public:
    int Batt_Num=0;//index of the battery
    float Avg_Volt=0; // Avarage voltage of the pack
    float Cell[CELL_IN_PACK] = {0};//cell voltage value in the pack
    Class_Batt(int batt_num) { // populate cell values and Avg_Volt volt  
        int i=0;
        int temp=0;
        for( i = 0 ; i < CELL_IN_PACK ; i++) {
            Cell[i]=Sens_status.Bat_Volt[batt_num*CELL_IN_PACK+i];
            temp = temp + Cell[i];
            }
        Avg_Volt = temp/CELL_IN_PACK;
    }
    
};Class_Batt Battery[NUMBER_OF_BAT_PACK] = Class_Batt(0);

//global VAR
int Cur_Bat = 0; // Batt selector
int cycle_count = 0; //cycle counter while suspected full


int ReqBat(bool Req);
void SelectBat(int ReqBat);
void LcdBatUpdate();
int SystemMod(int Min,int Max);

// put your setup code here, to run once:
void setup() {
  int i=0;
  Serial.begin(9600);
  lcd.init();//initialize the lcd
  lcd.backlight();// backlight on
  lcd.createChar(8,Batt_Icon[0].Skull);
  //set all output pins
  for (i = 0; i < TOTAL_Relay; i++)
    pinMode(RELAY[i], OUTPUT);
  Sens_status=Sens_Read();
  for (i = 0; i < NUMBER_OF_BAT_PACK; i++)
    Battery[i]=Class_Batt(i);
  SelectBat(ReqBat(1));
}

void loop() {
  int Min = 0; // Whos the battery with the min voltage 
  int Max = 0; // Whos the battery with the max voltage 
  int i=0;

  //read sensors 
  Sens_status=Sens_Read();
  for (i = 0; i < NUMBER_OF_BAT_PACK; i++)
    Battery[i]=Class_Batt(i);

  //getting battery voltage 
  //return Min (0) or Max (1) Bat position
  Min = ReqBat(0);
  Max = ReqBat(1);

  //reset state massage
  //charging cycle
  switch (SystemMod(Min,Max)){
  case 1: //Charging
    lcd.setCursor(14,0);
    lcd.print("Chargi");
    lcd.setCursor(14,1);
    lcd.print("ng     ");
    break;

  case 2://Verify
    lcd.setCursor(14,0);
    lcd.print("Verify");
    lcd.setCursor(14,1);
    lcd.print("FullBt");
    break;

  case 3://Switching to Low Bat
    Cur_Bat = Min;
    lcd.setCursor(14,1);
    lcd.print("      ");
    break;

  case 4://Float
    lcd.setCursor(14,0);
    lcd.print("Float ");
    lcd.setCursor(14,1);
    lcd.print("      ");
    break;

  case 5://Pack is Full
    lcd.setCursor(14,0);
    lcd.print(" Full ");
    lcd.setCursor(14,1);
    lcd.print(" Pack "); 
    break;

  case 6://Discharge
    lcd.setCursor(14,0);
    lcd.print("Discha");
    lcd.setCursor(14,1);
    lcd.print("rge   ");
    break;

  case 7://Switching to next High Batt
    Cur_Bat = Max;
    Serial.print(Cur_Bat); 
    break;

  case 8:// pack is empty 
    lcd.setCursor(14,0);
    lcd.print(" pack ");
    lcd.setCursor(14,1);
    lcd.print(" Empty");
    break;

  default://?
    while(true){
    lcd.setCursor(0,0);
    lcd.print("somthing went trebly wrong");}
  }

  //Set requested Bat
  //SelectBat (Cur_Bat);
  /*
  for (i=0; i<TOTAL_CELL; i++){
      Serial.print(" cell");
      Serial.print(i);
      Serial.print(" ");
      Serial.print(Sens_status.Bat_Volt[i]);
  }
  */
  for (i=0; i<NUMBER_OF_BAT_PACK; i++){
    Serial.print(" Bat");
    Serial.print(i);
    Serial.print(" ");
    Serial.print(Battery[i].Avg_Volt);
  }
  
  Serial.print("InCur");
  Serial.print(Sens_status.In_current);
  Serial.print(" OutCur");
  Serial.print(Sens_status.Out_current);
  /*
  Serial.print(" Bat_1");
  Serial.print(Battery[0].Avg_Volt);
  Serial.print(" Bat_2");
  Serial.print(Battery[0].Avg_Volt);
  Serial.print(" Bat_3");
  Serial.print(Battery[0].Avg_Volt);
  Serial.print(" Bat_4");
  Serial.print(Battery[0].Avg_Volt);
  Serial.print(" min");
  Serial.print(Min);
  Serial.print(" max");
  Serial.print(Max);
  */
  Serial.print(" Cur_Bat");
  Serial.print(Cur_Bat);
  Serial.println();

  LcdBatUpdate();
  delay(SYSTEM_DELAY);
}

//return Min (0) or Max (1) Bat position
int ReqBat(bool Req){
  int temp = 0;
  int i=0;
  int Batt_High =0;
  int Batt_Low=-1;
  for (i = 0; i < NUMBER_OF_BAT_PACK; i++){
    //Compare for Max
     if (temp < Battery[i].Avg_Volt ){
      temp = Battery[i].Avg_Volt;
      Batt_High = i;
    }
  }
  temp = Battery[Batt_High].Avg_Volt;
  //find the Min/Max voltage in array and return the index
  for (i = 0; i < NUMBER_OF_BAT_PACK; i++){
    //Compare for Min
    if (temp >= Battery[i].Avg_Volt && Battery[i].Avg_Volt > MIN_ALLOW){
      temp = Battery[i].Avg_Volt;
      Batt_Low = i;
    //Compare for Max
    } 
  }
  if (Req==1)
    return (Batt_High);
  return (Batt_Low);
}

/*Detect system Mod:
 * Charging:
 * 1 = Charging, 2 = Verify, 3 = Switching to Low Bat, 4 = Flaot, 5 = Pack is Full
 * Discharging
 * 6 = Discharge, 7 = //Switching to next High Batt, 8 = Pack is Empty
 */
int SystemMod(int Min,int Max){ 
  int i=0;
  if (Min < 0)
    return 99;//no availible batt.

  if ( Sens_status.In_current > Sens_status.Out_current ){
    //check the battery pack status
    if (Battery[Min].Avg_Volt <= (MAX_VOLT - HYSTERESIS)){
    //check if Battery is sinking current
      if (( Sens_status.In_current- Sens_status.Out_current) > MIN_CURRENT){
        cycle_count=0;
        return 1; //charging       
      //check if battery is charged up 
      } else if(Battery[Cur_Bat].Avg_Volt > MAX_VOLT){
        if(cycle_count>CYCLE_TO_FULL)
          return 3; //verification complete Switch to Low Bat
        cycle_count++;
        return 2; // verify if full 
      } else
        return 4;// float, not enouth current
    } else 
        return 5;// pack is full
  // Discharging cycle
  } else 
    //check the battery pack status
    if(Battery[Max].Avg_Volt >= (MIN_VOLT + HYSTERESIS)){
      //check if battery is empty
      if(Battery[Cur_Bat].Avg_Volt < MIN_VOLT)
      //request next highest capacity battery 
       return 7; //current batt is empty, switch to next one 
      else 
        return 6; // discharging
    } else 
      return 8; // pack is empty
}

  void SelectBat(int ReqBat){
    switch (ReqBat){
    case 0:
      digitalWrite(RELAY[0],LOW);
      digitalWrite(RELAY[1],LOW);
    break;
    case 1:
      digitalWrite(RELAY[0],HIGH);
      digitalWrite(RELAY[1],LOW);
    break;
    case 2:
      digitalWrite(RELAY[0],HIGH);
      digitalWrite(RELAY[1],HIGH);
      break;
    case 3:
      break;
  }
}


void LcdBatUpdate(){
  int i =0;
  int ii=0;
  int setRow = 0;
  int setColm = 0;
  float Total_Precentage =0;
  for(i = 0 ; i < TOTAL_CELL ; i++){
    Batt_Icon[i] = Class_Bat_Icon(Sens_status.Bat_Volt[i]);
    if (Sens_status.Bat_Volt[i]>=MIN_ALLOW)
      lcd.createChar(i, Batt_Icon[i].CharBatState);
    else
      lcd.createChar(i, Batt_Icon[i].Skull);
    //print to LCD + calculated volt
    if (i < 4){
      setColm = 0;
      setRow = i;
      //2nd raw
    }else{
      setColm = 7;
      setRow = i%4;
    }
    //draw the BAT icon
    lcd.setCursor(setColm+1,setRow);
    lcd.write(i);
    //draw the voltage 
    lcd.setCursor(setColm+2,setRow);
    lcd.print( Sens_status.Bat_Volt[i],1);
    //draw the V symble
    lcd.setCursor(setColm+6,setRow);
    lcd.print("V");
    //marke in use battery 
    if ( 0 == i % CELL_IN_PACK  )
    for (ii = 0; ii < CELL_IN_PACK ; ii++){
      lcd.setCursor(setColm,setRow+ii);
      if ( (i == Cur_Bat*CELL_IN_PACK) )
        lcd.write(62);
      else 
        lcd.print(" ");
    }
  }
  //show precentage
  Total_Precentage = (Sens_status.Bat_Avg_charge/TOTAL_CELL-MIN_VOLT)/(MAX_VOLT-MIN_VOLT)*100;
   lcd.setCursor(16,2);
   if (Total_Precentage >= 100){
    lcd.print("100");
    lcd.setCursor(19,2);
    } else{
      if (Total_Precentage <= 0)
        lcd.print("0");
      else 
        if(Total_Precentage<10){
          lcd.setCursor(17,2); //clean the % mark 
          lcd.print(" ");
          lcd.setCursor(18,2);
          lcd.print(Total_Precentage,0);
        }else
          lcd.print(Total_Precentage,0);
      lcd.setCursor(19,2); //clean the % mark 
      lcd.print(" ");
      lcd.setCursor(18,2);
    }
  lcd.print("%");
  return;
  
}