#include <EEPROM.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);  // LCD I2C address

// Variables for voltage, current, power calculations
float vo = 0, vi = 0, currentBat = 0, currentPV = 0;
float cutoff = 0, pBat = 0, pPV = 0, efficiency = 0;
int pwmValue = 0, batteryType = 0, readCounter = 0;

// Button debounce timing
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

// Pins
const int pwmPin = 9;
const int buttonPin1 = 10;  // Mode toggle
const int buttonPin2 = 11;  // Increase battery type
const int buttonPin3 = 12;  // Decrease battery type

void setup() {
  // LCD initialization
  lcd.init();
  lcd.backlight();

  // Set PWM frequency for pin 9
  TCCR1B = TCCR1B & B11111000 | B00000001;  // Set PWM frequency for D9

  // Serial communication
  Serial.begin(115200);

  // Pin setup
  pinMode(pwmPin, OUTPUT);
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonPin3, INPUT_PULLUP);

  // Read saved battery type from EEPROM
  batteryType = EEPROM.read(0);
  if (batteryType > 2) batteryType = 0;

  // Set cutoff based on battery type
  setBatteryCutoff();
}

void loop() {
  readCounter++;

  // Read sensor values and calculate power if not in configuration mode
  if (readCounter > 30) {
    readCounter = 0;
    readSensorValues();
    calculatePowerAndEfficiency();
    controlPWM();
    displayDataOnLCD();
    printDataToSerial();
  }

  // Handle button presses for toggling modes and battery selection
  handleButtonPresses();
  
  delay(10);  // Prevent CPU overuse
}

void setBatteryCutoff() {
  switch (batteryType) {
    case 0:
      cutoff = 15.4;  // Flooded
      break;
    case 1:
      cutoff = 14.8;  // AGM
      break;
    case 2:
      cutoff = 14.0;  // Gel
      break;
  }
}

void readSensorValues() {
  // Reset values
  currentBat = 0;
  currentPV = 0;
  vo = 0;
  vi = 0;

  // Take multiple readings to average
  for (int i = 0; i < 100; i++) {
    currentBat += (.049 * analogRead(A2) - 25);  // Battery current
    currentPV += (.049 * analogRead(A3) - 25);   // PV current
    vo += analogRead(A0);  // Battery voltage
    vi += analogRead(A1);  // PV voltage
    delayMicroseconds(100);
  }

  // Normalize readings
  currentBat = max(currentBat / 60, 0.0);  // Prevent negative readings
  currentPV = max(currentPV / 70, 0.0);
  vo = (vo * 0.03) / 100;
  vi = (vi * 0.3) / 80;

  // Adjust PV input voltage
  vi = (vo * 10) - vi;
}

void calculatePowerAndEfficiency() {
  // Calculate power
  pBat = currentBat * vo;
  pPV = currentPV * vi;

  // Calculate efficiency
  if (pPV != 0) {
    efficiency = (pBat / pPV) * 100;
  } else {
    efficiency = 0;
  }
}

void controlPWM() {
  // PWM adjustment based on voltage
  if (vo > cutoff) {
    pwmValue = max(pwmValue - 1, 0);  // Decrease PWM if voltage exceeds cutoff
  }
  if (vo < cutoff + 1) {
    pwmValue = min(pwmValue + 1, 200);  // Increase PWM if below cutoff
  }

  analogWrite(pwmPin, pwmValue);  // Set PWM output
}

void displayDataOnLCD() {
  lcd.clear();

  // Display PV (input) voltage, current, and power
  lcd.setCursor(0, 0);
  lcd.print("P ");
  lcd.print((int)vi);
  lcd.print("V ");
  lcd.print(currentPV, 1);
  lcd.print("A ");
  lcd.print((int)pPV);
  lcd.print("W");

  // Display Battery (output) voltage, current, and power
  lcd.setCursor(0, 1);
  lcd.print("B ");
  lcd.print((int)vo);
  lcd.print("V ");
  lcd.print(currentBat, 1);
  lcd.print("A ");
  lcd.print((int)pBat);
  lcd.print("W");
}

void printDataToSerial() {
  // Print data to serial monitor for debugging
  Serial.print("Input Voltage: ");
  Serial.print(vi, 1);
  Serial.print("V, Input Current: ");
  Serial.print(currentPV, 1);
  Serial.print("A, Input Power: ");
  Serial.print(pPV, 1);
  Serial.println("W");

  Serial.print("Battery Voltage: ");
  Serial.print(vo, 1);
  Serial.print("V, Battery Current: ");
  Serial.print(currentBat, 1);
  Serial.print("A, Battery Power: ");
  Serial.print(pBat, 1);
  Serial.println("W");
}

void handleButtonPresses() {
  if (millis() - lastDebounceTime > debounceDelay) {
    if (!digitalRead(buttonPin1)) {
      lastDebounceTime = millis();
      if (!m) EEPROM.update(0, batteryType);  // Save battery type to EEPROM
      m = !m;  // Toggle mode
    }

    if (m) {  // Battery type selection mode
      if (!digitalRead(buttonPin2)) {
        lastDebounceTime = millis();
        batteryType = (batteryType + 1) % 3;
        setBatteryCutoff();
      }

      if (!digitalRead(buttonPin3)) {
        lastDebounceTime = millis();
        batteryType = (batteryType - 1 + 3) % 3;  // Wrap-around
        setBatteryCutoff();
      }

      // Display battery type on LCD
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("  Battery Type  ");
      lcd.setCursor(0, 1);
      switch (batteryType) {
        case 0:
          lcd.print("    Flooded     ");
          break;
        case 1:
          lcd.print("      AGM       ");
          break;
        case 2:
          lcd.print("      Gel       ");
          break;
      }
    }
  }
}