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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define VOLTAGE_PIN A0    // Pin for voltage measurement
#define CURRENT_PIN A1    // Pin for current measurement

#define BUTTON_PIN_START 2
#define BATTERY_TYPE_PIN 4 // Switch for battery type selection

bool isTesting = false;
unsigned long previousMillis = 0;
unsigned long startTime = 0;
float totalAh = 0.0;
float voltage = 0.0;
float current = 0.0;
float V_full = 0.0;
float V_last = 0.0;
float fullAh = 0.0;
int batteryType = 0; // 0 = Li-ion, 1 = LiFePO4
const unsigned long testDuration = 60000; // 1 minute in milliseconds

void setup() {
  pinMode(BUTTON_PIN_START, INPUT_PULLUP);
    pinMode(BATTERY_TYPE_PIN, INPUT_PULLUP);

      if(!display.begin(SSD1306_PAGEADDR, 0x3C)) {
          Serial.println(F("SSD1306 allocation failed"));
              for(;;);
                }
                  display.clearDisplay();
                    display.display();

                      display.setTextSize(1);
                        display.setTextColor(SSD1306_WHITE);
                          display.setCursor(0, 0);
                            display.println(F("Battery Ah Tester"));
                              display.display();
                                delay(2000);
                                }

                                void loop() {
                                  // Toggle battery type selection
                                    if (digitalRead(BATTERY_TYPE_PIN) == LOW) {
                                        batteryType = !batteryType;
                                            delay(200); // Debounce
                                              }

                                                // Start the test when the button is pressed
                                                  if (digitalRead(BUTTON_PIN_START) == LOW && !isTesting) {
                                                      isTesting = true;
                                                          previousMillis = millis();
                                                              startTime = millis();
                                                                  totalAh = 0.0;
                                                                      V_full = measureVoltage(); // Record the full voltage at the start of the test
                                                                        }

                                                                          // If testing, continue to measure voltage, current, and calculate Ah
                                                                            if (isTesting) {
                                                                                measureCurrentAndVoltage();
                                                                                    calculateAh();
                                                                                        displayData();

                                                                                            // Stop the test automatically after 1 minute
                                                                                                if (millis() - startTime >= testDuration) {
                                                                                                      isTesting = false;
                                                                                                            V_last = voltage; // Record the last voltage at the end of the test
                                                                                                                  calculateFullAh(); // Calculate the full battery capacity
                                                                                                                        displayResult();
                                                                                                                            }
                                                                                                                              }
                                                                                                                              }

                                                                                                                              float measureVoltage() {
                                                                                                                                int rawVoltage = analogRead(VOLTAGE_PIN);
                                                                                                                                  float voltageMultiplier = (batteryType == 0) ? 4.2 / 1023.0 : 3.65 / 1023.0;
                                                                                                                                    return rawVoltage * voltageMultiplier;
                                                                                                                                    }

                                                                                                                                    float measureCurrent() {
                                                                                                                                      int rawCurrent = analogRead(CURRENT_PIN);
                                                                                                                                        // Assuming the current sensor has a range of 0-5V for 0-5A and a 10-bit ADC
                                                                                                                                          return (rawCurrent / 1023.0) * 5.0; // Converts to current in amps
                                                                                                                                          }

                                                                                                                                          void measureCurrentAndVoltage() {
                                                                                                                                            voltage = measureVoltage();
                                                                                                                                              current = measureCurrent();

                                                                                                                                                // Add protection to stop the test if the voltage drops too low
                                                                                                                                                  float lowVoltageThreshold = (batteryType == 0) ? 3.0 : 2.5;
                                                                                                                                                    if (voltage < lowVoltageThreshold) {
                                                                                                                                                        isTesting = false;
                                                                                                                                                            V_last = voltage;
                                                                                                                                                                calculateFullAh();
                                                                                                                                                                    displayResult();
                                                                                                                                                                      }
                                                                                                                                                                      }

                                                                                                                                                                      void calculateAh() {
                                                                                                                                                                        unsigned long currentMillis = millis();
                                                                                                                                                                          float timeElapsed = (currentMillis - previousMillis) / 3600000.0; // Convert to hours
                                                                                                                                                                            previousMillis = currentMillis;
                                                                                                                                                                              totalAh += current * timeElapsed;
                                                                                                                                                                              }

                                                                                                                                                                              void calculateFullAh() {
                                                                                                                                                                                float voltageDrop = V_full - V_last;
                                                                                                                                                                                  fullAh = totalAh / voltageDrop * V_full;
                                                                                                                                                                                  }

                                                                                                                                                                                  void displayData() {
                                                                                                                                                                                    display.clearDisplay();
                                                                                                                                                                                      display.setCursor(0, 0);
                                                                                                                                                                                        display.print(F("Type: "));
                                                                                                                                                                                          display.println(batteryType == 0 ? "Li-ion" : "LiFePO4");
                                                                                                                                                                                            display.print(F("Voltage: "));
                                                                                                                                                                                              display.print(voltage,4);
                                                                                                                                                                                                display.println(F(" V"));
                                                                                                                                                                                                  display.print(F("Current: "));
                                                                                                                                                                                                    display.print(current);
                                                                                                                                                                                                      display.println(F(" A"));
                                                                                                                                                                                                        display.print(F("Total Ah: "));
                                                                                                                                                                                                          display.print(totalAh);
                                                                                                                                                                                                            display.display();
                                                                                                                                                                                                            }

                                                                                                                                                                                                            void displayResult() {
                                                                                                                                                                                                              display.clearDisplay();
                                                                                                                                                                                                                display.setCursor(0, 0);
                                                                                                                                                                                                                  display.println(F("Test Completed"));
                                                                                                                                                                                                                    display.print(F("Full Volt: "));
                                                                                                                                                                                                                      display.print(V_full,4);
                                                                                                                                                                                                                        display.println(F(" V"));
                                                                                                                                                                                                                          display.print(F("Last Volt: "));
                                                                                                                                                                                                                            display.print(V_last,4);
                                                                                                                                                                                                                              display.println(F(" V"));
                                                                                                                                                                                                                                display.print(F("Full Ah: "));
                                                                                                                                                                                                                                  display.print(fullAh);
                                                                                                                                                                                                                                    display.println(F(" Ah"));
                                                                                                                                                                                                                                      display.display();
                                                                                                                                                                                                                                      }