#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "PID_v1.h" // https://github.com/br3ttb/Arduino-PID-Library

LiquidCrystal_I2C lcd(0x27, 20, 4);

#define AVG_NUM 10
#define offsetVoltage  2.5 // for ACS712 sensor
#define Sensitivity 0.066 // 185mV/A for ACS712-5A variant  

#define solar_volt_sense 0        // defining the analog pin A0 to read solar panel Voltage
#define bat_volt_sense 1          //defining the analog pin A1 to read battery voltage
#define solar_current_sense 2     //defining the Analog pin A2 to measure load current
#define load_current_sense 3
#define temp_sense 6

#define pwm_pin 3                 // defining digital pin D9 to drive the main MOSFET Q1 @ 1KHz
#define load_pin 2
#define gradDetect 26.3157894373 // Gradient for SoC Detect 
#define Vb_FULL_SCALE 13.8 //SoC 100% Baterai 13.8 Volt
#define BULK_CHRARGE_SP 14.4
#define FLOAT_CHARGE_SP 13.5
#define Charge_Restart_SP 13.2
#define MIN_SOLAR_VOLT  10
#define LVD 11.5
#define LVR 12.5
#define ABSORPTION_LIMIT 3600000    // 1 hour in milliseconds
#define NIGHT_TIME 3600000          // 1 hour in milliseconds
#define CHARGER_RESTART_TIME 600000 // 10 mins in milliseconds
#define EVENING_TIME 300000         // 5mins in milliseconds
#define MORNING_TIME 180000         // 3mins in milliseconds

float pwm_duty=0;                          // PWM Duty Cycle (0 to 255)
float bulk_charge_sp=0;                  // Bulk charging set point
float error=0;                           // calculate the difference between battery voltage and bulk charge set point
float float_charge_sp=0;                 // Float charging set point
int load_status; // 0-off, 1- on 
const int bat_type = 1;                  //Flooded=0,AGM=1 

const float BETA = 3950;

float solar_volt=0; // solar panel voltage 
float solar_current=0;
float bat_volt=0; // solar panel voltage 
float load_current=0;
int celsius;
int temp_change=0;
float temp_val=0;
float solar_watts=0;
float load_watts=0;
float load_wattHours=0;
float solar_wattHours=0;
float bat_volt_k_1 =0;

float SoC_Detect = 0;
float temp_SoC_Detect =0;
float SoCCorr_k_1 =0;
float SoC_VB=0;
int SOC_VB_Temp =0;


enum charger_state {off, bulk,absorption,Float} charger_state ; 

unsigned long last_time=0;
unsigned long current_time=0;

unsigned long absorption_time;         // to keep track of today's time in absorption state
unsigned long charger_millis;           // to keep track of time for charger
unsigned long restart_time;
unsigned long morn_timer;
unsigned long even_timer;
unsigned long load_millis;

int mode =1;

double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT); // Here Kp= 2 , Ki= 5 and Kd=1 //  aggKp=4, aggKi=0.2, aggKd=1;

///////////////////////DECLARATION OF ALL BIT MAP ARRAY FOR FONTS////////////////////////////////////////////////////////////////
//--------------------------------------------------------------------------------------------------------------------------

byte solar[8] = //icon for solar panel
{
  0b11111,0b10101,0b11111,0b10101,0b11111,0b10101,0b11111,0b00000
};
byte battery[8] =  //icon for battery
{
  0b01110,0b11011,0b10001,0b10001,0b10001,0b10001,0b10001,0b11111
};
byte charge[8] = // icon for battery charge
{
  0b01010,0b11111,0b10001,0b10001,0b10001,0b01110,0b00100,0b00100,
};
byte not_charge[8]=
{
  0b00000,0b10001,0b01010,0b00100,0b01010,0b10001,0b00000,0b00000,
};
byte temp[8] = //icon for termometer
{
  0b00100,0b01010,0b01010,0b01110,0b01110,0b11111,0b11111,0b01110
};
byte energy[8] =  // icon for power
{
  0b00010,0b00100,0b01000,0b11111,0b00010,0b00100,0b01000,0b00000
};

//**************SoC Detect*************************************************
float SOC_Detect()
{
  SoC_Detect = (gradDetect*(bat_volt - 10.0));
  return SoC_Detect;
}

void SoC_Estimator(float SOCCOOR_K_1, float BAT_VOLT_K_1, float BAT_VOLT,float *SOC_VB)
{
  *SOC_VB = SOCCOOR_K_1 + ((BAT_VOLT - BAT_VOLT_K_1)/(Vb_FULL_SCALE - BAT_VOLT_K_1 )*(100 - SOCCOOR_K_1));
}
void INIT()
{
  //**********************Raw 0 (SOLAR INDICATOR)***********************
  lcd.createChar(0, solar);
  lcd.setCursor(0, 0); //(Kolom, Baris)
  lcd.write(0);

  lcd.setCursor(2, 0); //(Kolom, Baris)
  lcd.print(solar_volt,1);

  lcd.setCursor(6, 0); //(Kolom, Baris)
  lcd.print('V');

  lcd.setCursor(8, 0); //(Kolom, Baris)
  lcd.print(solar_current,1);

  lcd.setCursor(12, 0); //(Kolom, Baris)
  lcd.print('A');

  lcd.setCursor(14, 0); //(Kolom, Baris)
  lcd.print(solar_watts,1);

  lcd.setCursor(19, 0); //(Kolom, Baris)
  lcd.print('W');

//*******************Raw 1 (BATTERAI INDICATOR)********************************************
  lcd.createChar(1, battery);
  lcd.setCursor(0, 1); //(Kolom, Baris)
  lcd.write(1);

  lcd.setCursor(2, 1); //(Kolom, Baris)
  lcd.print(bat_volt,1);

  lcd.setCursor(6, 1); //(Kolom, Baris)
  lcd.print('V');


  lcd.createChar(4, temp);
  lcd.setCursor(8, 1); //(Kolom, Baris)
  lcd.write(4);

  lcd.setCursor(9, 1); //(Kolom, Baris)
  lcd.print(celsius,1);
  lcd.write(0b11011111);

  lcd.setCursor(12, 1); //(Kolom, Baris)
  lcd.print('C');


  lcd.createChar(2, charge);
  lcd.setCursor(14, 1); //(Kolom, Baris)
  lcd.write(2);


  lcd.createChar(3, not_charge);
  lcd.setCursor(15, 1); //(Kolom, Baris)
  lcd.write(3);

  lcd.setCursor(17, 1); //(Kolom, Baris)
  lcd.print(SOC_VB_Temp);

  lcd.setCursor(19, 1); //(Kolom, Baris)
  lcd.print('%');
//**********************RAW 3 (LOAD INDICAtOR)***********************************
  lcd.setCursor(0, 2); //(Kolom, Baris)
  lcd.print('L');
  lcd.setCursor(2, 2); //(Kolom, Baris)
  lcd.print(load_current,1);
  lcd.setCursor(6, 2); //(Kolom, Baris)
  lcd.print('A');
  lcd.setCursor(8, 2); //(Kolom, Baris)
  lcd.print(load_watts,1);
  lcd.setCursor(13, 2); //(Kolom, Baris)
  lcd.print('W');
  lcd.setCursor(15, 2); //(Kolom, Baris)
  lcd.print("OFF"); //Load State
//**************Raw 3 (Listrik Output-Solar Wh & Load Wh)********************
  lcd.createChar(5, energy);
  lcd.setCursor(0, 3); //(Kolom, Baris)
  lcd.write(5);
  lcd.setCursor(2, 3); //(Kolom, Baris)
  lcd.print(solar_wattHours,1);
  lcd.setCursor(6, 3); //(Kolom, Baris)
  lcd.print("Wh");


  lcd.setCursor(13, 3); //(Kolom, Baris)
  lcd.print(load_wattHours,1);
  lcd.setCursor(17, 3); //(Kolom, Baris)
  lcd.print("Wh"); //Load State


  delay(500);
}


int read_adc(int channel){
  
  int sum = 0;
  int temp;
  int i;

  
  for (i=0; i<AVG_NUM; i++) {          // loop through reading raw adc values AVG_NUM number of times  
    temp = analogRead(channel);        // read the input pin  
    sum += temp;                       // store sum for averaging
    delayMicroseconds(50);             // pauses for 50 microseconds  
  }
  return(sum / AVG_NUM);               // divide sum by AVG_NUM to get average and return it
}

void Initial_setpoint()
{
  solar_volt= read_adc(solar_volt_sense)*0.0048828*(37.5/7.5);
  solar_current = ((read_adc(solar_current_sense)*0.0048828 - offsetVoltage)/ Sensitivity );
  bat_volt= read_adc(bat_volt_sense)*0.0048828*(37.5/7.5);
  load_current = ((read_adc(load_current_sense)*0.0048828 - offsetVoltage)/ Sensitivity );
  solar_watts = solar_current * solar_volt;
  load_watts = load_current * bat_volt; //load Watts now

  temp_change =celsius-25.0;      // 25deg cel is taken as standard room temperature ( STC)
  // for floaded battery
  if (bat_type == 0)
  {
    bulk_charge_sp =BULK_CHRARGE_SP-(0.020*temp_change) ;
    float_charge_sp=FLOAT_CHARGE_SP-(0.020*temp_change) ;
  }
  // for AGM battery // set point is lowered to avoid excess gassing
  else
  {
    bulk_charge_sp =(BULK_CHRARGE_SP-0.2)-(0.020*temp_change) ;
    float_charge_sp=(FLOAT_CHARGE_SP-0.2)-(0.020*temp_change) ;
  }

}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  TCCR2B = TCCR2B & B11111000 | 0x03;    // pin 3 PWM frequency of 980.39 Hz
  lcd.init();
  lcd.backlight();

  lcd.setCursor(0,0); lcd.print("*******************");
  lcd.setCursor(0,1); lcd.print("    SOLAR CHARGE  "); 
  lcd.setCursor(0,2); lcd.print("     CONTROLLER  "); 
  lcd.setCursor(0,3); lcd.print("*******************");

  delay(1000);
  lcd.clear();
  Initial_setpoint();
  temp_val = analogRead(temp_sense);
  celsius = 1 / (log(1 / (1023. / temp_val - 1)) / BETA + 1.0 / 298.15) - 273.15;
  while ((celsius > 50 ) || (celsius < -20 ) || (bat_volt < 9)) // If the temperature is beyond the permisible limit or battery is damaged/disconnected 
  {
    lcd.setCursor(2,9); 
    lcd.print("    ERROR  "); 
    Serial.println(" Error "); 
  } 
  charger_state = off;          // start the charger from off
  charger_millis = millis() ;   // initialise the local clock
  load_millis =millis();
  lcd.clear();
  INIT();
  Serial.print("absorption_time: ");  
  Serial.print(absorption_time);   
  Serial.print("\n");

}

void loop() {
  // put your main code here, to run repeatedly:
  solar_volt= read_adc(solar_volt_sense)*0.0048828*(37.5/7.5);
  solar_current = ((read_adc(solar_current_sense)*0.0048828 - offsetVoltage)/ Sensitivity );
  bat_volt= read_adc(bat_volt_sense)*0.0048828*(37.5/7.5);
  load_current = ((read_adc(load_current_sense)*0.0048828 - offsetVoltage)/ Sensitivity );
  solar_watts = solar_current * solar_volt;
  load_watts = load_current * bat_volt; //load Watts now

  last_time = current_time;
  current_time = millis();
  load_wattHours = load_wattHours +  load_watts*(( current_time -last_time) /3600000.0) ; // calculating energy in Watt-Hour
  solar_wattHours = solar_wattHours+  solar_watts*(( current_time -last_time) /3600000.0) ; // calculating energy in Watt-Hour

  if (solar_current <0)
  {
    solar_current = 0;
  }

  if(load_current < 0)
  {
    load_current = 0;
  }

  temp_SoC_Detect = SOC_Detect();
  if(temp_SoC_Detect < 0)
  {
    temp_SoC_Detect = 0;
  }
  current_time = millis();
  if(current_time - last_time >= 1000)
  {
    bat_volt_k_1 = bat_volt;
    SoCCorr_k_1 = temp_SoC_Detect;
    last_time = current_time;
    
  }
  SoC_Estimator(SoCCorr_k_1,bat_volt_k_1,bat_volt,&SoC_VB);
  SOC_VB_Temp = (int) SoC_VB;

  if(SOC_VB_Temp > 99)
  {
    SOC_VB_Temp = 99;
  }
  if(SOC_VB_Temp < 0)
  {
    SOC_VB_Temp = 0;
  }
  
  temp_val = analogRead(temp_sense);
  celsius = 1 / (log(1 / (1023. / temp_val - 1)) / BETA + 1.0 / 298.15) - 273.15;
  while ((celsius > 50 ) || (celsius < -20 ) || (bat_volt < 9)) // If the temperature is beyond the permisible limit or battery is damaged/disconnected 
  {
    bat_volt= read_adc(bat_volt_sense)*0.0048828*(37.5/7.5);
    lcd.createChar(1, battery);
    lcd.setCursor(0, 1); //(Kolom, Baris)
    lcd.write(1);

    lcd.setCursor(2, 1); //(Kolom, Baris)
    lcd.print(bat_volt,1);

    lcd.setCursor(6, 1); //(Kolom, Baris)
    lcd.print('V');

    Serial.println(" Error ");   
    digitalWrite(2, LOW);
    pwm_duty =0;
    analogWrite(pwm_pin, pwm_duty);
  } 

  temp_change =celsius-25.0;      // 25deg cel is taken as standard room temperature ( STC)
  // for floaded battery
  if (bat_type == 0)
  {
    bulk_charge_sp =BULK_CHRARGE_SP-(0.020*temp_change) ;
    float_charge_sp=FLOAT_CHARGE_SP-(0.020*temp_change) ;
  }
  // for AGM battery // set point is lowered to avoid excess gassing
  else
  {
    bulk_charge_sp =(BULK_CHRARGE_SP-0.2)-(0.020*temp_change) ;
    float_charge_sp=(FLOAT_CHARGE_SP-0.2)-(0.020*temp_change) ;
  }

  switch(charger_state)
  {
    case off:
      if ((bat_volt < float_charge_sp) && (solar_volt > (bat_volt + 0.5)))
      {
        charger_millis = millis() ;
        charger_state = bulk ; 
      }
      else if((bat_volt > float_charge_sp) && (solar_volt > (bat_volt + 0.5)) && (absorption_time > ABSORPTION_LIMIT))
      {
        charger_millis = millis() ;
        restart_time = 0 ;
        charger_state = Float ; 
      }
      else
      {
        if ((millis() - charger_millis > NIGHT_TIME))
        {
          absorption_time = 0; 
          pwm_duty = 0;
          analogWrite(pwm_pin, pwm_duty);  //generate PWM from D3 @ 0% duty // Shut down the charger
        }
      }
      break; // end of case off condition

      ///////////////////////////////////  STAGE-1 (BULK CHARGING)//////////////////////////////////////////////////////
      // During this stage the MOSFET is fully on by setting the duty cycle to 100%
      // Constant Current Charging
      /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    case bulk:
      if (solar_volt < bat_volt)
      {
        charger_millis = millis() ;
        charger_state = off ; 
      }
      else if ((bat_volt > bulk_charge_sp) && ( solar_volt > ( bat_volt + 0.5)) && (absorption_time < ABSORPTION_LIMIT))
      {
        charger_millis = millis() ;
        charger_state = absorption ; 
      }
      else if ((bat_volt > float_charge_sp) && (solar_volt > (bat_volt + 0.5)) && (absorption_time > ABSORPTION_LIMIT))
      {
        charger_millis = millis() ;
        restart_time = 0 ;
        charger_state = Float ; 
      }
      else 
      {
        pwm_duty = 163.3451957294; //Full Scale PWM Duty for Bulk volt charge
        analogWrite(pwm_pin,pwm_duty);   //generate PWM from D3 @ 100% duty // MOSFET Q1 is ON 
        if( bat_volt > bulk_charge_sp )
        {
          charger_state = absorption ;  
        }
      }
      break; // end of case bulk condition
      ///////////////////////////////////  STAGE-2 (ABSORPTION CHARGING)//////////////////////////////////////////////////////
      // During this stage the MOSFET is partially on by setting the duty cycle to between 0% and 100%
      // Constant voltage
      /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
      case absorption:
        if(solar_volt < bat_volt)
        {
          charger_millis = millis() ;
          charger_state = off ; 
        }
        else if(( bat_volt > float_charge_sp) && (solar_volt > (bat_volt + 0.5)) && (absorption_time > ABSORPTION_LIMIT))
        {
          charger_millis = millis() ;
          charger_state = Float ;
        }
        else
        {
          //  increment absorption timer and test for duration in absorption state
          absorption_time = absorption_time + millis() - charger_millis ;
          charger_millis = millis();

          Input = bat_volt;
          Setpoint = bulk_charge_sp;
          myPID.Compute();  // Compute PID Output 
          pwm_duty = Output*5 ; // Output = kp * error + ki * errSum + kd * dErr  
          if(pwm_duty < 0)
          {
            pwm_duty=0;
          }
          if(pwm_duty >255)
          {
            pwm_duty=255;
          }
          analogWrite(pwm_pin, pwm_duty);
        }
        break; // end of case absorption condition

        ///////////////////////////////////  STAGE-3 (FLOAT CHARGING)//////////////////////////////////////////////////////
        // During this stage the MOSFET is partially on by setting the duty cycle between 0% and 100%
        // Constant Voltage Charging
        /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        case Float:
           if (solar_volt < bat_volt)
           {
            charger_millis = millis() ;
            charger_state = off ;
           }
           else if((bat_volt < Charge_Restart_SP) && (solar_volt > (bat_volt + 0.5)) && (restart_time > CHARGER_RESTART_TIME))
           {
            charger_millis = millis() ;
            charger_state = bulk ; 
           }
           else if((bat_volt >float_charge_sp)  && (solar_volt > (bat_volt + 0.5)) && (absorption_time < ABSORPTION_LIMIT))
           {
            charger_millis = millis() ;
            charger_state = absorption ;
           }
           else
           {
            if (bat_volt > float_charge_sp)
            {
              pwm_duty--;
              if(pwm_duty <0)
              {
                pwm_duty=0;
              }
              analogWrite(pwm_pin, pwm_duty);
            } 
            else
            {
              pwm_duty = 12.75; // setting duty cycle = 5% for trickle charge
              analogWrite(pwm_pin, pwm_duty);  //generate PWM from D3 @ 5% duty // Q1 is driving @ 5% duty cycle
            }
            if(bat_volt < Charge_Restart_SP)
            {
              restart_time = restart_time + millis() - charger_millis ;
              charger_millis = millis();  
            }
           }
           break; // end of case float condition
  }

  Serial.print(solar_volt);
  Serial.print("; ");
  Serial.print(solar_current);
  Serial.print("; ");
  Serial.print(bat_volt);
  Serial.print("; ");
  Serial.print(load_current);
  Serial.print("; ");
  Serial.print(celsius);
  Serial.print("; ");
  Serial.print(bat_volt_k_1);
  Serial.print("; ");
  Serial.print(temp_SoC_Detect);
  Serial.print("; ");
  Serial.print(SoCCorr_k_1);
  Serial.print("; ");
  Serial.print(SOC_VB_Temp);    
  Serial.print("; ");
  Serial.print(absorption_time);
  Serial.print("; ");
  Serial.print(charger_state); 
  Serial.print("; ");
  Serial.print(Output);  
  Serial.print("; ");
  Serial.print(pwm_duty); 
  Serial.print("\n");
 
  INIT();
  
  

}