// Relay Block 6 Channel
  int chargeLipo_1 = 53;
  int dischargeLipo_1 = 51;
  int chargeLipo_2 = 49;
  int dischargeLipo_2 = 47;
  int voltageDividerRelay1 = 45;
  int voltageDividerRelay2 = 43;


// Analog Sensors
  const int lipoCurrent_1 = A0; // ACS712 for Lipo pack 1
  const int lipoCurrent_2 = A1; // ACS712 for Lipo pack 2

  const int lipoVoltage_1 = A3; // Voltage Divider for Lipo pack 1
  const int lipoVoltage_2 = A4; // Voltage Divider for Lipo pack 2


// Battery Parameters
  float batteryCapacity = 360.0; // Battery capacity in mAh
  float totalCharge_1 = 0; // Store total charge Pack 1 in amp-second
  float totalCharge_2 = 0; // Store total charge Pack 2 in amp-second
  float initialSoC_1 = 0; // Initial SoC Pack 1 based on voltage measurement
  float initialSoC_2 = 0; // Initial SoC Pack 2 based on voltage measurement 


// ACS712 Parameters
  float ACS712_sensitivity = 0.66; // 66mV per amp for 30A version
  float ACS712_offset = 2.5; // Output at 0A current is 2.5V


// Voltage divider parameters
  float voltageDividerRatio = 0.392; // Example ratio (R1 = 10k, R2 = 6.8k)


//Time Tracking 
  unsigned long lastTime = 0;


// Battery State
  float lipoOneLive = false;
  float lipoTwoLive = false;


void setup() {
  // Serial Monitor 
  Serial.begin(9600);

  // Define Pin mode
  pinMode(chargeLipo_1, OUTPUT);
  pinMode(dischargeLipo_1, OUTPUT);
  pinMode(chargeLipo_2, OUTPUT);
  pinMode(dischargeLipo_2, OUTPUT);
  pinMode(voltageDividerRelay1, OUTPUT);
  pinMode(voltageDividerRelay2, OUTPUT);

  //Initial SoC using Voltage Divider
  initialSoC_1 = estimateInitialSoC_1();
  initialSoC_2 = estimateInitialSoC_2();

}

void loop() {

  // Measure current from ACS712
  float current_1 = measureCurrent_1();
  float current_2 = measureCurrent_2();

  // Calculate the elapsed time in seconds
  unsigned long currentTime = millis();
  float timeElapsed = (currentTime - lastTime) / 1000.0; // Time in seconds
  lastTime = currentTime;

  //Calculate the charge added or removed (Coulomb Counting)
  totalCharge_1 += current_1 * timeElapsed; // In Amp-Seconds
  totalCharge_2 += current_2 * timeElapsed; // in Amp-Seconds

  //Convert total charge to mAh
  float chargeUsedOne_mAh = totalCharge_1 / 3600.0 * 1000.0; //mAh
  float chargeUsedTwo_mAh = totalCharge_2 / 3600.0 * 1000.0; //mAh

  //Update the SoC (Based on charge / discharge current)
  float SoC_current1 = 100 * (initialSoC_1 / 100.0 - chargeUsedOne_mAh / batteryCapacity);
  float SoC_current2 = 100 * (initialSoC_2 / 100.0 - chargeUsedTwo_mAh / batteryCapacity);

  //Read voltage to cross-check SoC
  float batteryVoltage_1 = measureVoltage_1();
  float batteryVoltage_2 = measureVoltage_2();

  float SoC_voltageOne = estimateSoCFromVoltageOne(batteryVoltage_1);
  float SoC_voltageTwo = estimateSoCFromVoltageTwo(batteryVoltage_2);


  // Status of battery in use
  if (dischargeLipo_1 == HIGH) {
    lipoOneLive == true;
    lipoTwoLive == false;
  } else if (dischargeLipo_2 == HIGH) {
    lipoOneLive == false;
    lipoTwoLive == true;
  } else if (chargeLipo_1 == HIGH && chargeLipo_2 == HIGH) {
    lipoOneLive == false;
    lipoTwoLive == false;
  }

}


// Discharging Lipo 1
void useLipo_1 () {

  digitalWrite(chargeLipo_1, LOW);
  digitalWrite(dischargeLipo_1, HIGH);
  digitalWrite(chargeLipo_2, HIGH);
  digitalWrite(dischargeLipo_2, LOW);
}

// Discharge Lipo 2
void useLipo_2 () {

  digitalWrite(chargeLipo_1, HIGH);
  digitalWrite(dischargeLipo_1, LOW);
  digitalWrite(chargeLipo_2, LOW);
  digitalWrite(dischargeLipo_2, HIGH);
}

// Function to measure current from ACS712
float measureCurrent_1() {
  int currentAnalogValue_1 = analogRead(lipoCurrent_1);
  float voltagePack_1 = (currentAnalogValue_1 / 1024.0) * 5.0; // Lipo 1 convert to voltage
  float current_1 = (voltagePack_1 - ACS712_offset) / ACS712_sensitivity;
  return current_1;
}

float measureCurrent_2() {
  int currentAnalogValue_2 = analogRead(lipoCurrent_2);
  float voltagePack_2 = (currentAnalogValue_2 / 1024.0) * 5.0; // Lipo 2 convert to voltage
  float current_2 = (voltagePack_2 - ACS712_offset) / ACS712_sensitivity;
  return current_2;
}


// Function to measure voltage using Voltage Divider
float measureVoltage_1() {
  int voltageAnalogValue_1 = analogRead(lipoVoltage_1);
  float measuredVoltage_1 = (voltageAnalogValue_1 / 1024.0) * 5.0 / voltageDividerRatio;
  return measuredVoltage_1;
}

float measureVoltage_2() {
  int voltageAnalogValue_2 = analogRead(lipoVoltage_2);
  float measuredVoltage_2 = (voltageAnalogValue_2 / 1024.0) * 5.0 / voltageDividerRatio;
  return measuredVoltage_2;
}


// Function to estimate initial SoC from battery voltage 
float estimateInitialSoC_1() {
  float voltage_1 = measureVoltage_1();
  return estimateSoCFromVoltageOne(voltage_1);
}

float estimateInitialSoC_2() {
  float voltage_2 = measureVoltage_2();
  return estimateSoCFromVoltageTwo(voltage_2);
}


// Function to estimate SoC from voltage (Basic Mapping)
float estimateSoCFromVoltageOne(float voltage_1) {
  if (voltage_1 >= 12.6) return 100.0;  // Fully charged (3S LiPo)
  if (voltage_1 >= 11.1) return 50.0;   // Mid SoC
  if (voltage_1 >= 9.9) return 10.0;    // Low SoC
  return 0.0;                         // Fully discharged
}

float estimateSoCFromVoltageTwo(float voltage_2) {
  if (voltage_2 >= 12.6) return 100.0;  // Fully charged (3S LiPo)
  if (voltage_2 >= 11.1) return 50.0;   // Mid SoC
  if (voltage_2 >= 9.9) return 10.0;    // Low SoC
  return 0.0;                         // Fully discharged
}