// Pin Definitions
const int b1r1 = 2;  // Relay for B1 positive
const int b1r2 = 3;  // Relay for B1 negative
const int b2r1 = 4;  // Relay for B2 positive
const int b2r2 = 5;  // Relay for B2 negative
const int b3r1 = 6;  // Relay for B3 positive
const int b3r2 = 7;  // Relay for B3 negative
const int b4r1 = 8;  // Relay for B4 positive
const int b4r2 = 9;  // Relay for B4 negative

// Analog pins for reading voltages from battery packs (through voltage dividers)
const int v_b1 = A0;  // Voltage reading for B1
const int v_b2 = A1;  // Voltage reading for B2
const int v_b3 = A2;  // Voltage reading for B3
const int v_b4 = A3;  // Voltage reading for B4

// Thresholds for switching
const float voltageSwitchThreshold = 10.0; // 10% as per the description
const float fullyChargedVoltage = 40.0;    // Voltage when fully charged (24V for 12V*2 pack)
float voltageB1, voltageB2, voltageB3, voltageB4; // Battery voltages

// State to track which pack is discharging
bool isPack1Discharging = false;

void setup() {
  // Set up relay pins as OUTPUT
  pinMode(b1r1, OUTPUT);
  pinMode(b1r2, OUTPUT);
  pinMode(b2r1, OUTPUT);
  pinMode(b2r2, OUTPUT);
  pinMode(b3r1, OUTPUT);
  pinMode(b3r2, OUTPUT);
  pinMode(b4r1, OUTPUT);
  pinMode(b4r2, OUTPUT);

  // Start serial communication
  Serial.begin(9600);

  // Initialize all relays in off (charging) state
  resetRelays();

  // Initial check to see which pack has more charge when the system powers on
  checkInitialPackState();
}

void loop() {
  // Read the voltages of all batteries
  voltageB1 = readBatteryVoltage(v_b1);
  voltageB2 = readBatteryVoltage(v_b2);
  voltageB3 = readBatteryVoltage(v_b3);
  voltageB4 = readBatteryVoltage(v_b4);

  // Print voltmeter readings
  Serial.print("B1 Voltage: "); Serial.println(voltageB1);
  Serial.print("B2 Voltage: "); Serial.println(voltageB2);
  Serial.print("B3 Voltage: "); Serial.println(voltageB3);
  Serial.print("B4 Voltage: "); Serial.println(voltageB4);

  // Calculate pack voltages (B1 + B2 for P1 and B3 + B4 for P2)
  float voltageP1 = voltageB1 + voltageB2;
  float voltageP2 = voltageB3 + voltageB4;

  Serial.print("Pack 1 Voltage: "); Serial.println(voltageP1);
  Serial.print("Pack 2 Voltage: "); Serial.println(voltageP2);

  // Switch packs based on the given conditions
  manageBatteryPacks(voltageP1, voltageP2);

  delay(10000);  // Delay for stability and easy reading in serial monitor
}

// Function to read the voltage of a battery (use appropriate voltage divider)
float readBatteryVoltage(int analogPin) {
  int sensorValue = analogRead(analogPin);
  float voltage = sensorValue * (5.0 / 1023.0); // Convert analog reading to voltage
  return voltage * 4;  // Assuming a 1:4 ratio in voltage divider for 12V
}

// Set all relays to the default state (charging)
void resetRelays() {
  digitalWrite(b1r1, LOW);
  digitalWrite(b1r2, LOW);
  digitalWrite(b2r1, LOW);
  digitalWrite(b2r2, LOW);
  digitalWrite(b3r1, LOW);
  digitalWrite(b3r2, LOW);
  digitalWrite(b4r1, LOW);
  digitalWrite(b4r2, LOW);
}

// Initial check to determine which pack to discharge and which to charge
void checkInitialPackState() {
  float voltageP1 = readBatteryVoltage(v_b1) + readBatteryVoltage(v_b2);
  float voltageP2 = readBatteryVoltage(v_b3) + readBatteryVoltage(v_b4);

  if (voltageP1 > voltageP2) {
    Serial.println("Pack 1 has higher voltage, starting P1 to discharge, P2 to charge...");
    dischargePack1();
    chargePack2();
    isPack1Discharging = true;
  } else {
    Serial.println("Pack 2 has higher voltage, starting P2 to discharge, P1 to charge...");
    dischargePack2();
    chargePack1();
    isPack1Discharging = false;
  }
}

// Manage switching between the packs based on voltage and thresholds
void manageBatteryPacks(float voltageP1, float voltageP2) {
  if (isPack1Discharging) {
    // If Pack 1 is discharging, check if it should switch to charging
    if (voltageP1 < voltageSwitchThreshold) {
      Serial.println("Pack 1 voltage too low, switching P1 to charge, P2 to discharge...");
      chargePack1();
      dischargePack2();
      isPack1Discharging = false;
    } else if (voltageP2 >= fullyChargedVoltage) {
      Serial.println("Pack 2 is fully charged, switching P2 to discharge, P1 to charge...");
      chargePack1();
      dischargePack2();
      isPack1Discharging = false;
    }
  } else {
    // If Pack 2 is discharging, check if it should switch to charging
    if (voltageP2 < voltageSwitchThreshold) {
      Serial.println("Pack 2 voltage too low, switching P2 to charge, P1 to discharge...");
      chargePack2();
      dischargePack1();
      isPack1Discharging = true;
    } else if (voltageP1 >= fullyChargedVoltage) {
      Serial.println("Pack 1 is fully charged, switching P1 to discharge, P2 to charge...");
      chargePack2();
      dischargePack1();
      isPack1Discharging = true;
    }
  }
}

// Charging Pack 1 (B1 + B2)
void chargePack1() {
  digitalWrite(b1r1, LOW); // B1 in charging state
  digitalWrite(b1r2, LOW);
  digitalWrite(b2r1, LOW); // B2 in charging state
  digitalWrite(b2r2, LOW);
}

// Discharging Pack 1 (B1 + B2)
void dischargePack1() {
  digitalWrite(b1r1, HIGH); // B1 in discharging state
  digitalWrite(b1r2, HIGH);
  digitalWrite(b2r1, HIGH); // B2 in discharging state
  digitalWrite(b2r2, HIGH);
}

// Charging Pack 2 (B3 + B4)
void chargePack2() {
  digitalWrite(b3r1, LOW); // B3 in charging state
  digitalWrite(b3r2, LOW);
  digitalWrite(b4r1, LOW); // B4 in charging state
  digitalWrite(b4r2, LOW);
}

// Discharging Pack 2 (B3 + B4)
void dischargePack2() {
  digitalWrite(b3r1, HIGH); // B3 in discharging state
  digitalWrite(b3r2, HIGH);
  digitalWrite(b4r1, HIGH); // B4 in discharging state
  digitalWrite(b4r2, HIGH);
}