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

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

// PWM Output to Buck Converter
#define PWM_PIN 25
#define PWM_CHANNEL 0
#define PWM_FREQ 5000
#define PWM_RESOLUTION 8

// Button pins
#define BTN_UP     32
#define BTN_DOWN   33
#define BTN_START  25

// Current sensor
#define ACS_PIN 35
float offset = 2.5; // Midpoint voltage for ACS712
float sensitivity = 0.066; // 66mV/A for ACS712-30A

// Battery settings
int selectedVoltage = 12; // Default 12V
bool charging = false;

// Stage control
enum Stage {IDLE, BULK, ABSORB, FLOAT};
Stage chargeStage = IDLE;

unsigned long stageStartTime = 0;
float batteryVoltage = 0;
float currentAmps = 0;

// PWM voltage control function
void setOutputVoltage(float voltage) {
  float maxVoltage = 60.0; // Max buck voltage
    int pwmVal = map((int)(voltage * 10), 0, maxVoltage * 10, 0, 255);
      pwmVal = constrain(pwmVal, 0, 255);
        ledcWrite(PWM_CHANNEL, pwmVal);
        }

        // Read battery voltage (replace with actual divider if needed)
        float readBatteryVoltage() {
          int adc = analogRead(34); // e.g., via voltage divider
            float voltage = (adc / 4095.0) * 3.3 * 11.0; // Adjust divider ratio
              return voltage;
              }

              float readCurrent() {
                float voltage = analogRead(ACS_PIN) * 3.3 / 4095.0;
                  float amps = (voltage - offset) / sensitivity;
                    return amps;
                    }

                    void updateDisplay() {
                      display.clearDisplay();
                        display.setTextSize(1);
                          display.setTextColor(SSD1306_WHITE);

                            display.setCursor(0, 0);
                              display.print("Voltage: ");
                                display.print(batteryVoltage);
                                  display.println(" V");

                                    display.setCursor(0, 10);
                                      display.print("Current: ");
                                        display.print(currentAmps);
                                          display.println(" A");

                                            display.setCursor(0, 20);
                                              display.print("Target: ");
                                                display.print(selectedVoltage);
                                                  display.println("V");

                                                    display.setCursor(0, 30);
                                                      display.print("Stage: ");
                                                        switch (chargeStage) {
                                                            case BULK: display.print("Bulk"); break;
                                                                case ABSORB: display.print("Absorb"); break;
                                                                    case FLOAT: display.print("Float"); break;
                                                                        default: display.print("Idle"); break;
                                                                          }

                                                                            display.display();
                                                                            }

                                                                            void handleButtons() {
                                                                              if (digitalRead(BTN_UP) == LOW) {
                                                                                  if (selectedVoltage < 48) selectedVoltage += 12;
                                                                                      delay(300);
                                                                                        }
                                                                                          if (digitalRead(BTN_DOWN) == LOW) {
                                                                                              if (selectedVoltage > 12) selectedVoltage -= 12;
                                                                                                  delay(300);
                                                                                                    }
                                                                                                      if (digitalRead(BTN_START) == LOW) {
                                                                                                          charging = !charging;
                                                                                                              chargeStage = charging ? BULK : IDLE;
                                                                                                                  stageStartTime = millis();
                                                                                                                      delay(500);
                                                                                                                        }
                                                                                                                        }

                                                                                                                        void updateChargingLogic() {
                                                                                                                          if (!charging) {
                                                                                                                              setOutputVoltage(0);
                                                                                                                                  return;
                                                                                                                                    }

                                                                                                                                      switch (chargeStage) {
                                                                                                                                          case BULK:
                                                                                                                                                setOutputVoltage(selectedVoltage * 1.15); // push hard current
                                                                                                                                                      if (batteryVoltage >= selectedVoltage * 0.8) {
                                                                                                                                                              chargeStage = ABSORB;
                                                                                                                                                                      stageStartTime = millis();
                                                                                                                                                                            }
                                                                                                                                                                                  break;

                                                                                                                                                                                      case ABSORB:
                                                                                                                                                                                            setOutputVoltage(selectedVoltage * 1.05); // hold voltage
                                                                                                                                                                                                  if (millis() - stageStartTime > 5 * 60 * 1000UL) { // 5 min
                                                                                                                                                                                                          chargeStage = FLOAT;
                                                                                                                                                                                                                }
                                                                                                                                                                                                                      break;

                                                                                                                                                                                                                          case FLOAT:
                                                                                                                                                                                                                                setOutputVoltage(selectedVoltage * 1.00); // maintain
                                                                                                                                                                                                                                      break;

                                                                                                                                                                                                                                          default:
                                                                                                                                                                                                                                                setOutputVoltage(0);
                                                                                                                                                                                                                                                      break;
                                                                                                                                                                                                                                                        }
                                                                                                                                                                                                                                                        }

                                                                                                                                                                                                                                                        void setup() {
                                                                                                                                                                                                                                                          Serial.begin(115200);
                                                                                                                                                                                                                                                            pinMode(BTN_UP, INPUT_PULLUP);
                                                                                                                                                                                                                                                              pinMode(BTN_DOWN, INPUT_PULLUP);
                                                                                                                                                                                                                                                                pinMode(BTN_START, INPUT_PULLUP);

                                                                                                                                                                                                                                                                  ledcSetup(PWM_CHANNEL, PWM_FREQ, PWM_RESOLUTION);
                                                                                                                                                                                                                                                                    ledcAttachPin(PWM_PIN, PWM_CHANNEL);

                                                                                                                                                                                                                                                                      if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
                                                                                                                                                                                                                                                                          Serial.println(F("SSD1306 allocation failed"));
                                                                                                                                                                                                                                                                              while (true);
                                                                                                                                                                                                                                                                                }

                                                                                                                                                                                                                                                                                  display.clearDisplay();
                                                                                                                                                                                                                                                                                    display.setTextSize(2);
                                                                                                                                                                                                                                                                                      display.setTextColor(SSD1306_WHITE);
                                                                                                                                                                                                                                                                                        display.setCursor(0, 10);
                                                                                                                                                                                                                                                                                          display.print("LiFePO4");
                                                                                                                                                                                                                                                                                            display.display();
                                                                                                                                                                                                                                                                                              delay(1000);
                                                                                                                                                                                                                                                                                              }

                                                                                                                                                                                                                                                                                              void loop() {
                                                                                                                                                                                                                                                                                                batteryVoltage = readBatteryVoltage();
                                                                                                                                                                                                                                                                                                  currentAmps = readCurrent();

                                                                                                                                                                                                                                                                                                    handleButtons();
                                                                                                                                                                                                                                                                                                      updateChargingLogic();
                                                                                                                                                                                                                                                                                                        updateDisplay();

                                                                                                                                                                                                                                                                                                          delay(500);
                                                                                                                                                                                                                                                                                                          }