#include <LiquidCrystal_I2C.h>
#include <SPI.h>
LiquidCrystal_I2C lcd(0x3F, 16, 2);
#define M1 5
#define M2 4
#define M3 3
#define D1 10
#define D2 9
#define D3 8
#define BUZZER 7
#define B1_high A0
#define B2_high A1
#define B3_high A2
#define Button 2

int buttonState = 0;
int printStart = 0;
int interval = 5000;  // Measurement Interwal (ms)
float battLow = 2.8;   // End of measurement - voltage level
float voltRef = 4.37;  // Reference voltage (pin 5V)

int l=0;
boolean finished1 = false;
boolean finished2 = false;
boolean finished3 = false;

float mAh1 = 0;
float mAh2 = 0;
float mAh3 = 0;
int poj1 = 0;
int poj2 = 0;
int poj3 = 0;
float R1 = 4;  // Resistor R1 value [Ohm]
float RB1 = 4.1; // Measured B1 circuit resistance [Ohm]
float R2 = 4;  // Resistor R1 value [Ohm]
float RB2 = 4.1; // Measured B2 circuit resistance [Ohm]
float R3 = 4;  // Resistor R1 value [Ohm]
float RB3 = 4.1; // Measured B3 circuit resistance [Ohm]
float current1 = 0.0;
float current2 = 0.0;
float current3 = 0.0;
float B1start = 0.0;
float B2start = 0.0;
float B3start = 0.0;
float B1V1 = 0.0;  // voltage before R1
float B2V1 = 0.0;  // voltage before R2
float B3V1 = 0.0;  // voltage before R3
float Rw1 = 0;
float Rw2 = 0;
float Rw3 = 0;
float roznica1 = 0;
float roznica2 = 0;
float roznica3 = 0;
float suma1 = 0;
float suma2 = 0;
float suma3 = 0;
float X1 = 2; // voltage divider 1 ratio
float X2 = 2; // voltage divider 2 ratio
float X3 = 2; // voltage divider 3 ratio

int pomiar = 0;
int k;
boolean menu1_end = false;
boolean menu_end = false;
int mode = 0;

unsigned long previousMillis1 = 0;
unsigned long previousMillis2 = 0;
unsigned long previousMillis3 = 0;
unsigned long millisPassed1 = 0;
unsigned long millisPassed2 = 0;
unsigned long millisPassed3 = 0;

void setup() {

  lcd.init();
  lcd.backlight();
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(D1, OUTPUT);
  pinMode(D2, OUTPUT);
  pinMode(D3, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  digitalWrite(M1, LOW);
  digitalWrite(M2, LOW);
  digitalWrite(M3, LOW);
  digitalWrite(D1, LOW);
  digitalWrite(D2, LOW);
  digitalWrite(D3, LOW);
  menu();  // enter the menu
  lcd.print("Battery test");
  lcd.setCursor(0, 1);
  lcd.print("Please wait");
  delay(2000);
  B1start = X1 * analogRead(B1_high) * voltRef / 1024.0; // B1 voltage - no load applied
  B2start = X2 * analogRead(B2_high) * voltRef / 1024.0; // B2 voltage - no load applied
  B3start = X3 * analogRead(B3_high) * voltRef / 1024.0; // B3 voltage - no load applied
  lcd.clear();

  finished1 = false;
  finished2 = false;
  finished3 = false;
  digitalWrite(BUZZER, HIGH);
  delay(500);
  digitalWrite(BUZZER, LOW);

}

void loop() {

  if(mode == 1){
    test();
  }
  if(mode == 2){
    discharge();
  }
}
////////////////////////////////////////////////////////////////////////////////////////////////
void discharge() {
  B1V1 = X1 * analogRead(B1_high) * voltRef / 1024.0; // B1 voltage
  B2V1 = X2 * analogRead(B2_high) * voltRef / 1024.0; // B2 voltage
  B3V1 = X3 * analogRead(B3_high) * voltRef / 1024.0; // B3 voltage

  if ((finished1 == true) && (finished2 == true) && (finished3 == true)) {
    if(l < 3){
      for (k = 0; k < 3; k++) {
        digitalWrite(BUZZER, HIGH);
        delay(200);
        digitalWrite(BUZZER, LOW);
        delay(200);
    }
      l++;
    }
  }

  if (B1V1 >= battLow && finished1 == false)
  {
    digitalWrite(M1, HIGH);
    digitalWrite(D1, HIGH);

    lcd.setCursor(0, 0);
    lcd.print("1:");
    lcd.print(B1V1);
    lcd.print("V");
  }
  if ((B1V1 < battLow) || (finished1 == true)) // Disconnects load, prints the result
  {
    digitalWrite(M1, LOW);

    finished1 = true;

    digitalWrite(D1, LOW);
    lcd.setCursor(0, 0);
    lcd.print("1:END");
  }

  if (B2V1 >= battLow && finished2 == false)
  {
    digitalWrite(M2, HIGH);
    digitalWrite(D2, HIGH);
 
    lcd.setCursor(9, 0);
    lcd.print("2:");
    lcd.print(B2V1);
    lcd.print("V");
  }
  if ((B2V1 < battLow) || (finished2 == true)) // Disconnects load, prints the result
  {
    digitalWrite(M2, LOW);

    finished2 = true;

    digitalWrite(D2, LOW);
    lcd.setCursor(9, 0);
    lcd.print("2:END");
  }

  if (B3V1 >= battLow && finished3 == false)
  {
    digitalWrite(M3, HIGH);
    digitalWrite(D3, HIGH);
 
    lcd.setCursor(0, 1);
    lcd.print("3:");
    lcd.print(B3V1);
    lcd.print("V");
  }
  if ((B3V1 < battLow) || (finished3 == true)) // Disconnects load, prints the result
  {
    digitalWrite(M3, LOW);

    finished3 = true;

    digitalWrite(D3, LOW);
    lcd.setCursor(0, 1);
    lcd.print("3:END     ");
  }
  delay(interval/4);
  digitalWrite(D1, LOW);
  digitalWrite(D2, LOW);
  digitalWrite(D3, LOW);
  delay(interval/4);
  lcd.clear();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////
void test(){
  B1V1 = X1 * analogRead(B1_high) * voltRef / 1024.0; // B1 voltage
  B2V1 = X2 * analogRead(B2_high) * voltRef / 1024.0; // B2 voltage
  B3V1 = X3 * analogRead(B3_high) * voltRef / 1024.0; // B3 voltage

  if ((finished1 == true) && (finished2 == true) && (finished3 == true)) {
    if(l < 3){
      for (k = 0; k < 3; k++) {
        digitalWrite(BUZZER, HIGH);
        delay(200);
        digitalWrite(BUZZER, LOW);
        delay(200);
    }
      l++;
    }
  }

  if (B1V1 >= battLow && finished1 == false)
  {
    digitalWrite(M1, HIGH);
    millisPassed1 = millis() - previousMillis1;
    current1 = (B1V1) / R1; // Current calculation
    mAh1 = mAh1 + (current1 * 1000.0) * (millisPassed1 / 3600000.0); // Capacity sum
    previousMillis1 = millis();


    if (pomiar > 0 && pomiar < 5) // 4 x voltage measurement
    {
      suma1 = suma1 + B1V1;
    }
    if (pomiar == 6)
    {
      suma1 = suma1 / 4;
      roznica1 = B1start / suma1;
      Rw1 = (roznica1 - 1) * RB1; // Internal resistance
    }

    digitalWrite(D1, HIGH);
    lcd.clear();
    lcd.home();
    lcd.print("U:");
    lcd.print(B1V1);
    lcd.print("V");
    lcd.setCursor(9, 0);
    lcd.print("I:");
    lcd.print(current1);
    lcd.print("A");
    lcd.setCursor(0, 1);
    poj1 = mAh1;
    lcd.print(poj1);
    lcd.print("mAh");
    if (pomiar > 6)
    {
      lcd.setCursor(9, 1);
      lcd.print("Rw:");
      lcd.print(Rw1);
    }
    pomiar++;
    delay(interval);
    digitalWrite(D1, LOW);
  }
  if ((B1V1 < battLow) || (finished1 == true)) // Disconnects load, prints the result
  {
    digitalWrite(M1, LOW);

    finished1 = true;

    digitalWrite(D1, HIGH);
    lcd.clear();
    lcd.home();
    lcd.print("1 - Koniec");
    lcd.setCursor(0, 1);
    poj1 = mAh1;
    lcd.print(poj1);
    lcd.print("mAh ");
    lcd.setCursor(8, 1);
    lcd.print("Rw1:");
    lcd.print(Rw1);

    delay(interval * 2);
    digitalWrite(D1, LOW);
  }

  if (B2V1 >= battLow && finished2 == false)
  {
    digitalWrite(M2, HIGH);
    millisPassed2 = millis() - previousMillis2;
    current2 = (B2V1) / R2; // Current calculation
    mAh2 = mAh2 + (current2 * 1000.0) * (millisPassed2 / 3600000.0); // Capacity sum
    previousMillis2 = millis();


    if (pomiar > 0 && pomiar < 5) // 4 x voltage measurement
    {
      suma2 = suma2 + B2V1;
    }
    if (pomiar == 6)
    {
      suma2 = suma2 / 4;
      roznica2 = B2start / suma2;
      Rw2 = (roznica2 - 1) * RB2; // Internal resistance
    }

    digitalWrite(D2, HIGH);
    lcd.clear();
    lcd.home();
    lcd.print("U:");
    lcd.print(B2V1);
    lcd.print("V");
    lcd.setCursor(9, 0);
    lcd.print("I:");
    lcd.print(current2);
    lcd.print("A");
    lcd.setCursor(0, 1);
    poj2 = mAh2;
    lcd.print(poj2);
    lcd.print("mAh");
    if (pomiar > 6)
    {
      lcd.setCursor(9, 1);
      lcd.print("Rw:");
      lcd.print(Rw2);
    }
    delay(interval);
    digitalWrite(D2, LOW);
  }
  if ((B2V1 < battLow) || (finished2 == true)) // Disconnects load, prints the result
  {
    digitalWrite(M2, LOW);

    finished2 = true;

    digitalWrite(D2, HIGH);
    lcd.clear();
    lcd.home();
    lcd.print("2 - Koniec");
    lcd.setCursor(0, 1);
    poj2 = mAh2;
    lcd.print(poj2);
    lcd.print("mAh ");
    lcd.setCursor(8, 1);
    lcd.print("Rw2:");
    lcd.print(Rw2);

    delay(interval * 2);
    digitalWrite(D2, LOW);
  }

  if (B3V1 >= battLow && finished3 == false)
  {
    digitalWrite(M3, HIGH);
    millisPassed3 = millis() - previousMillis3;
    current3 = (B3V1) / R3; // Current calculation
    mAh3 = mAh3 + (current3 * 1000.0) * (millisPassed3 / 3600000.0); // Capacity sum
    previousMillis3 = millis();


    if (pomiar > 0 && pomiar < 5) // 4 x voltage measurement
    {
      suma3 = suma3 + B3V1;
    }
    if (pomiar == 6)
    {
      suma3 = suma3 / 4;
      roznica3 = B3start / suma3;
      Rw3 = (roznica3 - 1) * RB3; // Internal resistance
    }

    digitalWrite(D3, HIGH);
    lcd.clear();
    lcd.home();
    lcd.print("U:");
    lcd.print(B3V1);
    lcd.print("V");
    lcd.setCursor(9, 0);
    lcd.print("I:");
    lcd.print(current3);
    lcd.print("A");
    lcd.setCursor(0, 1);
    poj3 = mAh3;
    lcd.print(poj3);
    lcd.print("mAh");
    if (pomiar > 6)
    {
      lcd.setCursor(9, 1);
      lcd.print("Rw:");
      lcd.print(Rw3);
    }
    delay(interval);
    digitalWrite(D3, LOW);
  }
  if ((B3V1 < battLow) || (finished3 == true)) // Disconnects load, prints the result
  {
    digitalWrite(M3, LOW);

    finished3 = true;

    digitalWrite(D3, HIGH);
    lcd.clear();
    lcd.home();
    lcd.print("3 - Koniec");
    lcd.setCursor(0, 1);
    poj3 = mAh3;
    lcd.print(poj3);
    lcd.print("mAh ");
    lcd.setCursor(8, 1);
    lcd.print("Rw3:");
    lcd.print(Rw3);

    delay(interval * 2);
    digitalWrite(D3, LOW);
  }
}
////////////////////////////////////////////////////////////////////////////////////////////

void menu(){
lcd.clear();
lcd.setCursor(0,0);
lcd.print("SELECT MODE");
lcd.setCursor(0,1);
lcd.print("<--CLICK");
while(menu1_end==false){
   while(digitalRead(Button) == LOW){}
      delay(800);
      if(digitalRead(Button) == HIGH){
        menu1_end = true;
        lcd.setCursor(0,0);
        if(mode == 1){lcd.print(">Mode: CAP TEST");}
        if(mode == 2){lcd.print(">Mode: DISCHARGE");} 
      }
      else{
         if(mode==2){
          mode = 1;
        }
        else{
          mode++;
        }       
        lcd.clear();
        lcd.setCursor(0,0);
        if(mode == 1){lcd.print("Mode: CAP TEST");}
        if(mode == 2){lcd.print("Mode: DISCHARGE");}       
      }
    digitalWrite(D1, HIGH);
    delay(100);
    digitalWrite(D1, LOW);
}
while(menu_end==false){
   while(digitalRead(Button) == LOW){}
      delay(800);
      if(digitalRead(Button) == HIGH){
        menu_end = true;
        lcd.setCursor(0,1);
        lcd.print(">Low V: "); lcd.print(battLow); lcd.print("V");
      }
      else{
         if(battLow > 3.8){
          battLow = 2.9;
        }
        else{
          battLow = battLow+0.1;
        }       
        lcd.setCursor(0,1);
        lcd.print("Low V: "); lcd.print(battLow); lcd.print("V");     
      }
    digitalWrite(D2, HIGH);
    delay(100);
    digitalWrite(D2, LOW);
    }
delay(2000);
lcd.clear();
}