#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED display resolution
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Create an SSD1306 display object
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Constants
const int numCells = 4; // Example with 4 cells
int cellPins[numCells] = {A0, A1, A2, A3}; // Analog pins for voltage measurement
int mosfetPins[numCells] = {2, 3, 4, 5};  // Digital pins for MOSFET control

// Voltage thresholds
float maxVoltage = 3.65;  // Voltage at full charge
float minVoltage = 3.2;   // Approximate voltage at 50% charge
float voltageDividerRatio = 0.75; // Adjust based on your voltage divider
float voltageTolerance = 0.008;   // Tolerance for considering voltages "the same"

void setup() {
  // Initialize serial communication
    Serial.begin(9600);

      // Initialize the display
        if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
            Serial.println(F("SSD1306 allocation failed"));
                for(;;); // Don't proceed, loop forever
                  }
                    display.clearDisplay();
                      display.setTextSize(1);
                        display.setTextColor(WHITE);
                          
                            for (int i = 0; i < numCells; i++) {
                                pinMode(mosfetPins[i], OUTPUT);
                                    digitalWrite(mosfetPins[i], LOW); // Ensure all MOSFETs are off at start
                                      }
                                      }

                                      void loop() {
                                        float cellVoltages[numCells];
                                          float totalVoltage = 0;

                                            // Read cell voltages and calculate the total voltage
                                              for (int i = 0; i < numCells; i++) {
                                                  cellVoltages[i] = analogRead(cellPins[i]) * (5.0 / 1023.0) * voltageDividerRatio; // Convert analog reading to voltage
                                                      totalVoltage += cellVoltages[i];
                                                        }

                                                          // Calculate average voltage
                                                            float averageVoltage = totalVoltage / numCells;

                                                              // Display the voltages on the OLED
                                                                display.clearDisplay(); // Clear the display
                                                                  display.setCursor(0, 0); // Set cursor to the top-left corner
                                                                    display.print("Cell Voltages:");
                                                                      
                                                                        for (int i = 0; i < numCells; i++) {
                                                                            display.setCursor(0, 16 + i * 10);
                                                                                display.print("Cell ");
                                                                                    display.print(i + 1);
                                                                                        display.print(": ");
                                                                                            display.print(cellVoltages[i], 4); // Print voltage with 2 decimal places
                                                                                                display.print(" V");
                                                                                                  }
                                                                                                    
                                                                                                      display.display(); // Update the display with the new data

                                                                                                        // Balancing logic
                                                                                                          for (int i = 0; i < numCells; i++) {
                                                                                                              // Check if the cell voltage is above the 50% threshold (minVoltage) and the difference is significant
                                                                                                                  if (cellVoltages[i] > minVoltage && (cellVoltages[i] - averageVoltage) > voltageTolerance) {
                                                                                                                        digitalWrite(mosfetPins[i], HIGH); // Turn on MOSFET to start balancing
                                                                                                                              delay(100); // Energy transfer time (adjust based on your design)
                                                                                                                                    digitalWrite(mosfetPins[i], LOW); // Turn off MOSFET
                                                                                                                                        } else {
                                                                                                                                              digitalWrite(mosfetPins[i], LOW); // Ensure MOSFET is off if voltages are similar or below the threshold
                                                                                                                                                  }
                                                                                                                                                    }

                                                                                                                                                      delay(500); // Update the display every 500ms
                                                                                                                                                      }