/**
   SoC - SoH

   EruP(c)12-2023
*/

#include <LiquidCrystal.h>
#include <Keypad.h>
#include <Servo.h>

/* Display */
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

/* Keypad setup */
const byte KEYPAD_ROWS = 4;
const byte KEYPAD_COLS = 4;
byte rowPins[KEYPAD_ROWS] = {5, 4, 3, 2};
byte colPins[KEYPAD_COLS] = {A3, A2, A1, A0};
char keys[KEYPAD_ROWS][KEYPAD_COLS] = {
  {'1', '2', '3', '+'},
  {'4', '5', '6', '-'},
  {'7', '8', '9', '*'},
  {'.', '0', '=', '/'}
};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, KEYPAD_ROWS, KEYPAD_COLS);

uint64_t value = 0;

#define AH      10        // kapasitas awal baterai
#define minVoL  10.5      // Tegangan minimal baterai (SoC=0%)
#define maxVoL  12.70     // Tegangan minimal baterai (SoC=100%)

float Tabel[12][2]=
{
  {100,12.7},
  {95,12.6},
  {90,12.5},
  {80,12.42},
  {70,12.32},
  {60,12.2},
  {50,12.06},
  {40,11.9},
  {30,11.75},
  {20,11.58},
  {10,11.31},
  {0,10.5}
};

float HitungSoC(float VoL)
{
  for(short int i=1;i++;i<=11)
  {
    if(VoL>=Tabel[i][1])
    {
      
      float x2=Tabel[i-1][1];
      float x1=Tabel[i][1];
      float y2=Tabel[i-1][0];
      float y1=Tabel[i][0];
      float SoC=(y2-y1)/(x2-x1)*(VoL-x1)+y1;
      return SoC;
      continue;
    }
  }
}

float xHitungVoL(float SoC)
{
  for(short int i=1;i++;i<=11)
  {
    if(SoC>=Tabel[i][0])
    {
      
      float x2=Tabel[i-1][0];
      float x1=Tabel[i][0];
      float y2=Tabel[i-1][1];
      float y1=Tabel[i][1];
      float VoL=(y2-y1)/(x2-x1)*(SoC-x1)+y1;
      return VoL;
      continue;
    }
  }
}

float xHitungSoC(float VoL)
{
  //return (VoL-minVoL)/(maxVoL-minVoL)*100;
  //float S=-11.577*pow(VoL,4) + 535.87*pow(VoL,3) - 9263*VoL*VoL + 70912*VoL - 202944;
  double S=-11.57677316721360*pow(VoL,4) + 535.86600856623000*pow(VoL,3) + -9263.02898113957000*pow(VoL,2) + 70912.12858742210000*VoL + -202943.92722954200000;
  //https://agrimetsoft.com/regressions/Polynomial#toolSection
  //double S=-90.258314061228500*pow(VoL,6) + 6360.458436326890000*pow(VoL,5) + -186627.314513787000000*pow(VoL,4) + 2918445.556913080000000*pow(VoL,3) + -25652797.055340600000000*VoL*VoL + 120169321.089649000000000*VoL + -234374671.718637000000000;
  return S;
  //return S>100?100:S<0?0:S;
}

float HitungVoL(float SoC)
{
  // return (maxVoL-minVoL)*SoC/100+minVoL;
  //float V = 3E-09*pow(SoC,5) - 9E-07*pow(SoC,4) + 9E-05*pow(SoC,3) - 0.0044*SoC*SoC + 0.1133*SoC + 10.508;
  //float V=0.000004*pow(SoC,3) - 0.0008*pow(SoC,2) + 0.0611*SoC + 10.61;
  //float V = -4E-11*pow(SoC,6) + 1E-08*pow(SoC,5) - 2E-06*pow(SoC,4) + 0.0002*pow(SoC,3) - 0.0059*SoC*SoC + 0.1258*SoC + 10.502;
  //float V=1E-11*pow(SoC,6) - 3E-09*pow(SoC,5) + 2E-07*pow(SoC,4) + 5E-06*pow(SoC,3) - 0.0016*pow(SoC,2) + 0.0812*SoC + 10.524;
  double V= -0.000000000035400*pow(SoC,6) + 0.000000013941310*pow(SoC,5) + -0.000002111161922*pow(SoC,4) + 0.000156174617911*pow(SoC,3) + -0.005945345569180*SoC*SoC + 0.125803689165569*SoC + 10.501726897959300;
  return V>maxVoL?maxVoL:V<minVoL?minVoL:V;
}

float Baterai(float Arus)
{
  const float SoH=100;//45;               // % Kesehatan baterai saat ini
  const float Isi=100;
  static float KapBatt=AH*SoH/100;  // Kapasitas real baterai
  static float Kap=KapBatt*Isi/100; // Kapasitas Isi
  const float rd=0.05;              // R Dalam baterai
  static unsigned long WaktuLalu=micros();
  
  unsigned long WaktuSekarang=micros();
  //Serial.println("wL="+String(WaktuLalu)+"us, ws="+String(WaktuSekarang)+"us, dw="+String(((float)WaktuSekarang-(float)WaktuLalu)/1000000.0/10.0)+"h");
  //float dWaktu=WaktuSekarang;
  //Serial.println(dWaktu);
  //dWaktu-=WaktuLalu;
  //Serial.println(dWaktu);
  //dWaktu/=1000000.0;
  //Serial.println(dWaktu);
  //dWaktu/=3600.0;
  //dWaktu*=3600.0;
  //Serial.println(dWaktu);

  float dWaktu=((float)WaktuSekarang-WaktuLalu)/1000000/3600;

  WaktuLalu=WaktuSekarang;
  Kap-=(Arus*dWaktu);
  if(Kap<=0) Kap=0;
  float SoC=Kap/KapBatt*100;
  float VL=HitungVoL(SoC)-rd*Arus;
  Serial.println("dT="+String(dWaktu*360)+"s, Arus="+String(Arus)+"A, SoC="+String(SoC)+"%, Isi="+String(Kap)+"Ah, VL="+String(VL)+"V");
  return VL;
}

void setup() {
  Serial.begin(115200);
  lcd.begin(20, 4);
}

void xloop()
{
  for(float SoC=0;SoC<=100;SoC+=5)
  {
    float VoL=HitungVoL(SoC);
    Serial.println("SoC="+String(SoC)+"%, VoL="+String(VoL)+"V");
    delay(1000);
  }
}
void loop()
{
  static float Arus=0;
  char key=keypad.getKey();
  if(key)
  {
    Arus=(key-'0')*10;
    if(Arus<0) Arus=0;
    //Serial.println("Arus="+String(Arus)+"A");
  }
  lcd.setCursor(0,0);
  lcd.print("IL="+String(Arus)+"A ");
  
  float Vin=Baterai(Arus);
  lcd.setCursor(0,1);
  lcd.print("VL="+String(Vin)+"V ");

  static float VoL;
  float VL;
  static short int ArusMin=0;
  const float Imin=0;
  static float rd=0;

  if(Arus<=Imin)
  {
    ArusMin=1;
    VoL=Vin;
    //Serial.println("VoL="+String(VoL)+"V");
  }
  else
  {
    VL=Vin;
    //Serial.println("VoL="+String(VoL)+"V VL="+String(VL)+"V");
    if(ArusMin==1)
    {
      ArusMin=0;
      rd=(VoL-VL)/Arus;
      lcd.setCursor(0,3);
      lcd.print("rd="+String(rd)+"R ");
      //Serial.println("rd="+String(rd)+"R");
    }
    VoL=VL+rd*Arus;
    //Serial.println("VoL="+String(VoL)+"V");
  }

  lcd.setCursor(10,0);
  lcd.print("VoL="+String(VoL)+"V ");
  float SoC=HitungSoC(VoL);
  lcd.setCursor(10,1);
  lcd.print("SoC="+String((int)ceil(SoC))+"% ");

  static float SoCL=-1;
  if(SoCL==-1) SoCL=SoC;
  static unsigned long WaktuLalu=micros();
  unsigned long Waktu=micros();
  float dWaktu=((float)Waktu-WaktuLalu)/1000000/3600;
  WaktuLalu=Waktu;
  static float Q=0;
  Q+=Arus*dWaktu;
  float dSoC=SoCL-SoC;

Serial.println("SoCL="+String(SoCL)+"%, SoC="+String(SoC)+"%, dSoC="+String(dSoC)+"%, Arus="+String(Arus)+"A, dWaktu="+String(dWaktu*3600)+"s, Q="+String(Q)+"Ah");

  if(dSoC>0)
  {
    float KapBatt=Q/dSoC*100;
    float SoH=KapBatt/AH*100;
    if(SoH>100) SoH=100;
    lcd.setCursor(10,3);
    lcd.print("SoH="+String((int)SoH)+"% ");
  }
  delay(1000);
}