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

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

// Buttons
#define BUTTON_SELECT 13
#define BUTTON_START 14

// MCP41010 (Digital Potentiometer)
#define POT_CS 5 // Chip Select
#define POT_MOSI 23
#define POT_CLK 18

// ACS712 analog pin
#define ACS_PIN 34

// Voltage Divider analog pin
#define VBAT_PIN 35

// Global Variables
int presetIndex = 0;
bool charging = false;
unsigned long lastDebounce = 0;

// Voltage presets (Absorption V, Float V, Bulk Cutoff V)
struct BatteryProfile {
  float absorbV;
    float floatV;
      float bulkCutoff;
      };

      BatteryProfile profiles[] = {
        {14.6, 13.6, 13.2},
          {29.2, 27.2, 26.4},
            {43.8, 40.8, 39.6},
              {58.4, 54.4, 52.8}
              };

              const char* presetLabels[] = {"12V", "24V", "36V", "48V"};

              // Helper Functions
              void setOutputVoltage(float voltage) {
                // Map target voltage (e.g., 14.6V) to digital pot value (0-255)
                  // Adjust this mapping for your buck converter
                    int potValue = map((int)(voltage * 10), 0, 600, 0, 255); 
                      digitalWrite(POT_CS, LOW);
                        SPI.transfer(0x11); // Command byte (write to pot 0)
                          SPI.transfer(potValue);
                            digitalWrite(POT_CS, HIGH);
                            }

                            float readBatteryVoltage() {
                              int raw = analogRead(VBAT_PIN);
                                float voltage = (raw / 4095.0) * 3.3 * 2 * 11; // Adjust for your divider
                                  return voltage;
                                  }

                                  float readCurrent() {
                                    int raw = analogRead(ACS_PIN);
                                      float voltage = (raw / 4095.0) * 3.3;
                                        float current = (voltage - 2.5) / 0.185; // ACS712-5A (adjust if different)
                                          return current;
                                          }

                                          void displayInfo(float vbatt, float ibatt, const char* stage) {
                                            display.clearDisplay();
                                              display.setTextSize(1);
                                                display.setTextColor(WHITE);
                                                  display.setCursor(0, 0);
                                                    display.print("Battery: ");
                                                      display.println(presetLabels[presetIndex]);
                                                        display.print("Stage: ");
                                                          display.println(stage);
                                                            display.print("Vbat: ");
                                                              display.print(vbatt, 1);
                                                                display.println("V");
                                                                  display.print("Ibat: ");
                                                                    display.print(ibatt, 1);
                                                                      display.println("A");
                                                                        display.display();
                                                                        }

                                                                        void setup() {
                                                                          pinMode(BUTTON_SELECT, INPUT_PULLUP);
                                                                            pinMode(BUTTON_START, INPUT_PULLUP);
                                                                              pinMode(POT_CS, OUTPUT);

                                                                                SPI.begin(POT_CLK, -1, POT_MOSI);
                                                                                  digitalWrite(POT_CS, HIGH);

                                                                                    Serial.begin(115200);
                                                                                      if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
                                                                                          Serial.println("SSD1306 allocation failed");
                                                                                              while (1);
                                                                                                }
                                                                                                  display.display();
                                                                                                    delay(1000);
                                                                                                      display.clearDisplay();
                                                                                                      }

                                                                                                      void loop() {
                                                                                                        static String stage = "Idle";
                                                                                                          float vbatt = readBatteryVoltage();
                                                                                                            float ibatt = readCurrent();

                                                                                                              // Button debounce
                                                                                                                if (millis() - lastDebounce > 300) {
                                                                                                                    if (!digitalRead(BUTTON_SELECT)) {
                                                                                                                          presetIndex = (presetIndex + 1) % 4;
                                                                                                                                lastDebounce = millis();
                                                                                                                                    }
                                                                                                                                        if (!digitalRead(BUTTON_START)) {
                                                                                                                                              charging = !charging;
                                                                                                                                                    lastDebounce = millis();
                                                                                                                                                          if (charging) {
                                                                                                                                                                  setOutputVoltage(profiles[presetIndex].absorbV);
                                                                                                                                                                          stage = "Bulk";
                                                                                                                                                                                } else {
                                                                                                                                                                                        setOutputVoltage(0);
                                                                                                                                                                                                stage = "Idle";
                                                                                                                                                                                                      }
                                                                                                                                                                                                          }
                                                                                                                                                                                                            }

                                                                                                                                                                                                              if (charging) {
                                                                                                                                                                                                                  if (stage == "Bulk" && vbatt >= profiles[presetIndex].absorbV - 0.2) {
                                                                                                                                                                                                                        stage = "Absorb";
                                                                                                                                                                                                                              setOutputVoltage(profiles[presetIndex].absorbV);
                                                                                                                                                                                                                                  } else if (stage == "Absorb" && ibatt < 1.0) {
                                                                                                                                                                                                                                        stage = "Float";
                                                                                                                                                                                                                                              setOutputVoltage(profiles[presetIndex].floatV);
                                                                                                                                                                                                                                                  }
                                                                                                                                                                                                                                                    }

                                                                                                                                                                                                                                                      displayInfo(vbatt, ibatt, stage.c_str());
                                                                                                                                                                                                                                                        delay(500);
                                                                                                                                                                                                                                                        }