#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <WiFiManager.h>
#include <Preferences.h>
#include <string.h>

#include <MicroOcpp.h>
#include <MicroOcppMongooseClient.h>
#include <MicroOcpp/Core/Context.h>
#include <MicroOcpp/Core/Configuration.h>

#include <Adafruit_SSD1306.h>
#include <SmartLeds.h>
#include <MFRC522.h>

#include "esp_system.h"
#include "config.h"
#include "IM1281C.h"
#include "pins.h"

/* LED State Enum */
enum LEDState
{
  LED_INITIALIZING,   // Blue blinking - Initialization and WiFi connecting
  LED_WIFI_CONNECTED, // Blue solid - WiFi connected, connecting to OCPP server
  LED_OCPP_CONNECTED, // Green solid - Successfully connected to OCPP server
  LED_ERROR,          // Red solid - Error state
  LED_RESET_TX,       // Yellow solid - 3s BOOT button hold (Ready to clear transaction)
  LED_FACTORY_RESET   // Magenta fast blink - 7s BOOT button hold (Ready to factory reset)
};

static int s_retry_num = 0;
static volatile bool s_ocpp_connected = false;
static volatile LEDState s_led_state = LED_INITIALIZING;
static volatile unsigned long s_blink_last_time = 0;
static volatile bool s_blink_on = false;
static const unsigned long BLINK_INTERVAL = 200; // 200ms blink interval
static const unsigned long AUTH_WINDOW_MS = 30000;
static const unsigned long IM1281C_POLL_INTERVAL_MS = 5000;
static const unsigned long OLED_REFRESH_INTERVAL_MS = 500;
static const unsigned long CARD_ID_DISPLAY_MS = 5000;
static const unsigned long WAIT_HINT_BLINK_MS = 300;
static const int CC_FILTER_MIN = 0;
static const int CC_FILTER_MAX = 100;
static const int CC_FILTER_ON_THRESHOLD = 75;
static const int CC_FILTER_OFF_THRESHOLD = 25;
static const int CC_FILTER_DELTA_UP = 6;
static const int CC_FILTER_DELTA_DOWN_IDLE = -8;
static const int CC_FILTER_DELTA_DOWN_ACTIVE = -20;

static bool s_cc1_plugged = false;
static bool s_cc2_plugged = false;
static bool s_cc1_raw_last = false;
static bool s_cc2_raw_last = false;
static unsigned long s_cc1_last_change_ms = 0;
static unsigned long s_cc2_last_change_ms = 0;
static bool s_cc1_prev_plugged = false;
static bool s_cc2_prev_plugged = false;
static bool s_cc1_filter_inited = false;
static int s_cc1_filter_score = 0;
static bool s_cc2_filter_inited = false;
static int s_cc2_filter_score = 0;

static bool s_auth_in_progress = false;
static bool s_auth_ok = false;
static unsigned long s_auth_ok_at_ms = 0;
static String s_auth_id_tag;
static String s_last_swipe_id;
static unsigned long s_last_swipe_at_ms = 0;

static bool s_remote_start_accepted = false;

static bool authWindowValid();
static bool isConnectorIdle(unsigned int connectorId);
static void clearAuthWait(const char *reason)
{
  if (s_auth_ok || s_auth_id_tag.length() > 0)
  {
    Serial.printf("[main] Clear pending authorization: %s\n", reason);
  }
  s_auth_ok = false;
  s_auth_ok_at_ms = 0;
  s_auth_id_tag = "";
}

uint8_t mac[6];
char cpSerial[13];

// OCPP Configuration Variables
char ocpp_backend[128];
char cp_identifier[64];
char ocpp_password[64];
bool shouldSaveConfig = false;

// callback notifying us of the need to save config
void saveConfigCallback()
{
  Serial.println("Should save config");
  shouldSaveConfig = true;
}

struct mg_mgr mgr;

Adafruit_SSD1306 display(128, 64, &Wire, -1);

/**
 * WS2812B LED Pin
 * - GPIO 17 - RYMCU ESP32-DevKitC
 * - GPIO 16 - YD-ESP32-A
 */
#define LED_PIN 17
#define LED_COUNT 1

SmartLed leds(LED_WS2812B, LED_COUNT, LED_PIN, 0, DoubleBuffer);
MFRC522 rfid(PIN_RC_CS, PIN_RC_RST);
IM1281C im1281c;

static IM1281CAData s_meter_a;
static IM1281CBData s_meter_b;
static bool s_meter_data_ready = false;
static bool s_oled_ready = false;
static String s_oled_card_id;
static unsigned long s_oled_card_started_at_ms = 0;
static unsigned long s_oled_card_duration_ms = 0;
static String s_oled_msg_title;
static String s_oled_msg_line1;
static String s_oled_msg_line2;
static unsigned long s_oled_msg_started_at_ms = 0;
static unsigned long s_oled_msg_duration_ms = 0;
static bool s_oled_msg_alert = false;

static const char *ledStageText(LEDState state)
{
  switch (state)
  {
  case LED_INITIALIZING:
    return "INIT";
  case LED_WIFI_CONNECTED:
    return "WIFI";
  case LED_OCPP_CONNECTED:
    return "OCPP";
  case LED_ERROR:
    return "ERROR";
  case LED_RESET_TX:
    return "RST-TX";
  case LED_FACTORY_RESET:
    return "FACTORY";
  default:
    return "UNKNOWN";
  }
}

static const char *cpStatusShort(ChargePointStatus status)
{
  switch (status)
  {
  case ChargePointStatus_Available:
    return "AVL";
  case ChargePointStatus_Preparing:
    return "PRE";
  case ChargePointStatus_Charging:
    return "CHG";
  case ChargePointStatus_SuspendedEVSE:
    return "SEVSE";
  case ChargePointStatus_SuspendedEV:
    return "SEV";
  case ChargePointStatus_Finishing:
    return "FIN";
  case ChargePointStatus_Reserved:
    return "RSV";
  case ChargePointStatus_Unavailable:
    return "UNAV";
  case ChargePointStatus_Faulted:
    return "FLT";
  default:
    return "N/A";
  }
}

static unsigned int getWaitHintConnectorId()
{
  if (!authWindowValid())
  {
    return 0;
  }

  const bool c1_active = isTransactionActive(1) || isTransactionRunning(1) || ocppPermitsCharge(1);
  const bool c2_active = isTransactionActive(2) || isTransactionRunning(2) || ocppPermitsCharge(2);

  if (c1_active && !c2_active && isConnectorIdle(2))
  {
    return 2;
  }

  if (c2_active && !c1_active && isConnectorIdle(1))
  {
    return 1;
  }

  if (!s_cc1_plugged && !s_cc2_plugged)
  {
    if (isConnectorIdle(1) && isOperative(1))
    {
      return 1;
    }
    if (isConnectorIdle(2) && isOperative(2))
    {
      return 2;
    }
  }

  return 0;
}

static void showOledCard(const String &idTag, unsigned long durationMs = CARD_ID_DISPLAY_MS)
{
  s_oled_card_id = idTag;
  s_oled_card_started_at_ms = millis();
  s_oled_card_duration_ms = durationMs;
}

static bool isOledCardVisible()
{
  return s_oled_card_id.length() > 0 && (millis() - s_oled_card_started_at_ms) <= s_oled_card_duration_ms;
}

static void showOledMessage(const String &title, const String &line1 = String(), const String &line2 = String(), unsigned long durationMs = 2500, bool alert = false)
{
  s_oled_msg_title = title;
  s_oled_msg_line1 = line1;
  s_oled_msg_line2 = line2;
  s_oled_msg_started_at_ms = millis();
  s_oled_msg_duration_ms = durationMs;
  s_oled_msg_alert = alert;
}

static bool isOledMessageVisible()
{
  return s_oled_msg_title.length() > 0 && (millis() - s_oled_msg_started_at_ms) <= s_oled_msg_duration_ms;
}

static int energyKwhToWh(float energyKwh)
{
  if (energyKwh <= 0.0f)
  {
    return 0;
  }

  float energyWh = energyKwh * 1000.0f;
  if (energyWh > 2147483000.0f)
  {
    energyWh = 2147483000.0f;
  }
  return static_cast<int>(energyWh + 0.5f);
}

static void drawCenteredText(const String &text, int16_t y, uint8_t textSize)
{
  int16_t x1 = 0;
  int16_t y1 = 0;
  uint16_t w = 0;
  uint16_t h = 0;

  display.setTextSize(textSize);
  display.getTextBounds(text.c_str(), 0, y, &x1, &y1, &w, &h);

  int16_t x = (128 - static_cast<int16_t>(w)) / 2;
  if (x < 0)
  {
    x = 0;
  }

  display.setCursor(x, y);
  display.print(text);
}

static void refreshOled()
{
  if (!s_oled_ready)
  {
    return;
  }

  static unsigned long s_last_refresh_ms = 0;
  const unsigned long now = millis();
  if ((now - s_last_refresh_ms) < OLED_REFRESH_INTERVAL_MS)
  {
    return;
  }
  s_last_refresh_ms = now;

  const bool c1_chg = ocppPermitsCharge(1);
  const bool c2_chg = ocppPermitsCharge(2);
  const ChargePointStatus st1 = getChargePointStatus(1);
  const ChargePointStatus st2 = getChargePointStatus(2);
  const bool show_message = isOledMessageVisible();
  const bool show_card = !show_message && isOledCardVisible();

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextWrap(false);

  if (show_message)
  {
    String title = s_oled_msg_title;
    String line1 = s_oled_msg_line1;
    String line2 = s_oled_msg_line2;

    if (s_oled_msg_alert)
    {
      const uint8_t alertTitleSize = (title.length() <= 10) ? 2 : 1;
      const int16_t alertTitleY = (alertTitleSize == 2) ? 6 : 10;
      if (title.length() > 21)
      {
        title = title.substring(0, 21);
      }
      if (line1.length() > 14)
      {
        line1 = line1.substring(0, 14);
      }
      if (line2.length() > 14)
      {
        line2 = line2.substring(0, 14);
      }

      drawCenteredText(title, alertTitleY, alertTitleSize);
      if (line1.length() > 0)
      {
        drawCenteredText(line1, 30, 1);
      }
      if (line2.length() > 0)
      {
        drawCenteredText(line2, 44, 1);
      }
      display.display();
      return;
    }

    if (title.length() > 14)
    {
      title = title.substring(0, 14);
    }
    if (line1.length() > 16)
    {
      line1 = line1.substring(0, 16);
    }
    if (line2.length() > 16)
    {
      line2 = line2.substring(0, 16);
    }

    drawCenteredText(title, 6, 2);
    if (line1.length() > 0)
    {
      drawCenteredText(line1, 30, 1);
    }
    if (line2.length() > 0)
    {
      drawCenteredText(line2, 44, 1);
    }
    display.display();
    return;
  }

  if (show_card)
  {
    String shownId = s_oled_card_id;
    if (shownId.length() > 12)
    {
      shownId = shownId.substring(shownId.length() - 12);
    }

    // Card display mode: full-screen centered content
    drawCenteredText(String("CARD"), 16, 1);
    drawCenteredText(shownId, 34, 2);
    display.display();
    return;
  }

  display.setTextSize(1);
  display.setCursor(0, 0);
  display.printf("ST:%s AUTH:%s", ledStageText(s_led_state), authWindowValid() ? "WAIT" : "IDLE");

  display.setCursor(0, 8);
  display.printf("C1 P%d CH%d %s", s_cc1_plugged ? 1 : 0, c1_chg ? 1 : 0, cpStatusShort(st1));

  display.setCursor(0, 16);
  display.printf("C2 P%d CH%d %s", s_cc2_plugged ? 1 : 0, c2_chg ? 1 : 0, cpStatusShort(st2));

  if (!s_meter_data_ready)
  {
    display.setCursor(0, 28);
    display.print("Meter: waiting IM1281C");
  }
  else
  {
    display.setCursor(0, 24);
    display.printf("A U%.1f I%.2f", s_meter_a.voltage, s_meter_a.current);

    display.setCursor(0, 32);
    display.printf("A P%.0f E%.3f", s_meter_a.power, s_meter_a.energy);

    display.setCursor(0, 40);
    display.printf("B U%.1f I%.2f", s_meter_b.voltage, s_meter_b.current);

    display.setCursor(0, 48);
    display.printf("B P%.0f E%.3f", s_meter_b.power, s_meter_b.energy);

    display.setCursor(0, 56);
    display.printf("T%.1fC F%.1fHz", s_meter_a.temperature, s_meter_a.frequency);
  }

  display.display();
}

static bool isConnectorPlugged(unsigned int connectorId)
{
  if (connectorId == 1)
    return s_cc1_plugged;
  if (connectorId == 2)
    return s_cc2_plugged;
  return false;
}

static bool isConnectorIdle(unsigned int connectorId)
{
  return !isTransactionActive(connectorId) && !isTransactionRunning(connectorId) && !getTransaction(connectorId);
}

static bool isConnectorStartReady(unsigned int connectorId)
{
  return connectorId >= 1 && connectorId <= 2 && isOperative(connectorId) && isConnectorIdle(connectorId) && isConnectorPlugged(connectorId);
}

static void updateConnectorPluggedState()
{
  const bool cc1_raw = (digitalRead(PIN_CC1) == HIGH);

  if (!s_cc1_filter_inited)
  {
    s_cc1_filter_inited = true;
    s_cc1_filter_score = cc1_raw ? CC_FILTER_MAX : CC_FILTER_MIN;
    s_cc1_plugged = cc1_raw;
  }

  int cc1_delta = cc1_raw ? CC_FILTER_DELTA_UP : CC_FILTER_DELTA_DOWN_IDLE;
  if ((isTransactionActive(1) || isTransactionRunning(1)) && !cc1_raw)
  {
    cc1_delta = CC_FILTER_DELTA_DOWN_ACTIVE;
  }

  s_cc1_filter_score += cc1_delta;
  if (s_cc1_filter_score > CC_FILTER_MAX)
  {
    s_cc1_filter_score = CC_FILTER_MAX;
  }
  else if (s_cc1_filter_score < CC_FILTER_MIN)
  {
    s_cc1_filter_score = CC_FILTER_MIN;
  }

  bool cc1_candidate = s_cc1_plugged;
  if (s_cc1_filter_score >= CC_FILTER_ON_THRESHOLD)
  {
    cc1_candidate = true;
  }
  else if (s_cc1_filter_score <= CC_FILTER_OFF_THRESHOLD)
  {
    cc1_candidate = false;
  }

  if (cc1_candidate != s_cc1_plugged)
  {
    bool cc1_plugged_old = s_cc1_plugged;
    s_cc1_plugged = cc1_candidate;

    // If connector1 just connected and auth window is valid, try to start
    if (!cc1_plugged_old && s_cc1_plugged && s_auth_ok && (millis() - s_auth_ok_at_ms) <= AUTH_WINDOW_MS && s_auth_id_tag.length() > 0 && isConnectorIdle(1))
    {
      Serial.printf("[main] Connector 1 plugged in, auto-starting with idTag %s\n", s_auth_id_tag.c_str());
      auto tx = beginTransaction_authorized(s_auth_id_tag.c_str(), nullptr, 1);
      if (tx != nullptr)
      {
        s_auth_ok = false;
        s_auth_id_tag = "";
      }
    }
  }

  const bool cc2_raw = (digitalRead(PIN_CC2) == HIGH);

  if (!s_cc2_filter_inited)
  {
    s_cc2_filter_inited = true;
    s_cc2_filter_score = cc2_raw ? CC_FILTER_MAX : CC_FILTER_MIN;
    s_cc2_plugged = cc2_raw;
  }

  int delta = cc2_raw ? CC_FILTER_DELTA_UP : CC_FILTER_DELTA_DOWN_IDLE;
  if ((isTransactionActive(2) || isTransactionRunning(2)) && !cc2_raw)
  {
    delta = CC_FILTER_DELTA_DOWN_ACTIVE;
  }

  s_cc2_filter_score += delta;
  if (s_cc2_filter_score > CC_FILTER_MAX)
  {
    s_cc2_filter_score = CC_FILTER_MAX;
  }
  else if (s_cc2_filter_score < CC_FILTER_MIN)
  {
    s_cc2_filter_score = CC_FILTER_MIN;
  }

  bool cc2_candidate = s_cc2_plugged;
  if (s_cc2_filter_score >= CC_FILTER_ON_THRESHOLD)
  {
    cc2_candidate = true;
  }
  else if (s_cc2_filter_score <= CC_FILTER_OFF_THRESHOLD)
  {
    cc2_candidate = false;
  }

  if (cc2_candidate != s_cc2_plugged)
  {
    bool cc2_plugged_old = s_cc2_plugged;
    s_cc2_plugged = cc2_candidate;

    // If connector2 just connected and auth window is valid, try to start
    if (!cc2_plugged_old && s_cc2_plugged && s_auth_ok && (millis() - s_auth_ok_at_ms) <= AUTH_WINDOW_MS && s_auth_id_tag.length() > 0 && isConnectorIdle(2))
    {
      Serial.printf("[main] Connector 2 plugged in, auto-starting with idTag %s\n", s_auth_id_tag.c_str());
      auto tx = beginTransaction_authorized(s_auth_id_tag.c_str(), nullptr, 2);
      if (tx != nullptr)
      {
        s_auth_ok = false;
        s_auth_id_tag = "";
      }
    }
  }
}

static void updatePanelLedsFromPlugState()
{
  // Reserved for startup sync; runtime LED behavior is tied to charging permission.
  digitalWrite(PIN_LED1, HIGH);
  digitalWrite(PIN_LED2, HIGH);
}

static void updateChargeActuators()
{
  const bool chg1_on = ocppPermitsCharge(1);
  const bool chg2_on = ocppPermitsCharge(2);
  bool led1_on = chg1_on;
  bool led2_on = chg2_on;

  // During local authorization wait, blink the suggested idle connector LED.
  const unsigned int hintConnector = getWaitHintConnectorId();
  if (hintConnector > 0)
  {
    const bool blink_on = ((millis() / WAIT_HINT_BLINK_MS) % 2) == 0;
    if (hintConnector == 1 && !chg1_on)
    {
      led1_on = blink_on;
    }
    else if (hintConnector == 2 && !chg2_on)
    {
      led2_on = blink_on;
    }
  }

  // LEDs and relays are low-active
  digitalWrite(PIN_LED1, led1_on ? LOW : HIGH);
  digitalWrite(PIN_LED2, led2_on ? LOW : HIGH);
  digitalWrite(PIN_RELAY1, chg1_on ? LOW : HIGH);
  digitalWrite(PIN_RELAY2, chg2_on ? LOW : HIGH);
}

static void stopIfUnplugged()
{
  if (s_cc1_prev_plugged && !s_cc1_plugged && (isTransactionActive(1) || isTransactionRunning(1)))
  {
    Serial.println("[main] Connector 1 unplugged. Stop transaction immediately.");
    endTransaction(nullptr, "EVDisconnected", 1);
  }

  if (s_cc2_prev_plugged && !s_cc2_plugged && (isTransactionActive(2) || isTransactionRunning(2)))
  {
    Serial.println("[main] Connector 2 unplugged. Stop transaction immediately.");
    endTransaction(nullptr, "EVDisconnected", 2);
  }

  s_cc1_prev_plugged = s_cc1_plugged;
  s_cc2_prev_plugged = s_cc2_plugged;
}

static bool authWindowValid()
{
  return s_auth_ok && (millis() - s_auth_ok_at_ms) <= AUTH_WINDOW_MS && s_auth_id_tag.length() > 0;
}

static void requestAuthorizeByCard(const String &idTag)
{
  if (s_auth_in_progress)
  {
    return;
  }

  clearAuthWait("new card swiped");
  showOledCard(idTag);
  s_auth_in_progress = true;

  Serial.printf("[main] Authorize idTag: %s\n", idTag.c_str());
  authorize(
      idTag.c_str(),
      [idTag](JsonObject payload)
      {
        s_auth_in_progress = false;
        const char *status = payload["idTagInfo"]["status"] | "";
        if (!strcmp(status, "Accepted"))
        {
          s_auth_ok = true;
          s_auth_ok_at_ms = millis();
          s_auth_id_tag = idTag;
          Serial.printf("[main] Authorize accepted for idTag %s\n", idTag.c_str());
          showOledMessage("AUTH OK", "Swipe ready", String("ID ") + idTag.substring(max(0, (int)idTag.length() - 10)), 2200, false);

          // Check if there's already a connector plugged in; if so, start immediately
          unsigned int targetConnector = 0;
          if (s_cc1_plugged && isConnectorIdle(1) && isOperative(1))
          {
            targetConnector = 1;
          }
          else if (s_cc2_plugged && isConnectorIdle(2) && isOperative(2))
          {
            targetConnector = 2;
          }

          if (targetConnector > 0)
          {
            // Immediately start the transaction on the plugged connector
            Serial.printf("[main] Connector %u is plugged in, auto-starting transaction\n", targetConnector);
            auto tx = beginTransaction_authorized(idTag.c_str(), nullptr, targetConnector);
            if (tx != nullptr)
            {
              clearAuthWait("transaction started");
              showOledMessage("START OK", String("C") + String(targetConnector), "Charging", 2200, false);
            }
            else
            {
              showOledMessage("START FAIL", String("C") + String(targetConnector), "Busy/Not Ready", 2500, true);
            }
          }
          // Otherwise, wait for a connector to be plugged in
        }
        else
        {
          clearAuthWait("authorize rejected");
          Serial.printf("[main] Authorize rejected, status=%s\n", status);
          showOledMessage("AUTH FAIL", status, "Try again", 2500, true);
        }
      },
      []()
      {
        s_auth_in_progress = false;
        clearAuthWait("authorize aborted");
        Serial.println("[main] Authorize aborted");
        showOledMessage("AUTH ABORT", "Swipe again", String(), 1800, true);
      });
}

static void pollRfidCard()
{
  if (!rfid.PICC_IsNewCardPresent() || !rfid.PICC_ReadCardSerial())
  {
    return;
  }

  String idTag;
  for (byte i = 0; i < rfid.uid.size; i++)
  {
    if (rfid.uid.uidByte[i] < 0x10)
    {
      idTag += '0';
    }
    idTag += String(rfid.uid.uidByte[i], HEX);
  }
  idTag.toUpperCase();

  s_last_swipe_id = idTag;
  s_last_swipe_at_ms = millis();

  rfid.PICC_HaltA();
  rfid.PCD_StopCrypto1();

  requestAuthorizeByCard(idTag);
}

static void expireAuthWaitIfNeeded()
{
  if (s_auth_ok && s_auth_id_tag.length() > 0 && (millis() - s_auth_ok_at_ms) > AUTH_WINDOW_MS)
  {
    clearAuthWait("authorization timeout");
  }
}

static void pollIm1281c()
{
  static unsigned long s_last_poll_ms = 0;
  const unsigned long now = millis();
  if ((now - s_last_poll_ms) < IM1281C_POLL_INTERVAL_MS)
  {
    return;
  }
  s_last_poll_ms = now;

  if (!im1281c.readAll())
  {
    Serial.printf("[IM1281C] read failed: A=%u B=%u\n", im1281c.lastAResult(), im1281c.lastBResult());
    return;
  }

  const IM1281CAData &a = im1281c.a();
  const IM1281CBData &b = im1281c.b();
  s_meter_a = a;
  s_meter_b = b;
  s_meter_data_ready = true;

  Serial.printf("[IM1281C] A: U=%.4fV I=%.4fA P=%.4fW E=%.4fkWh PF=%.3f CO2=%.4fkg T=%.2fC F=%.2fHz\n",
                a.voltage,
                a.current,
                a.power,
                a.energy,
                a.powerFactor,
                a.co2,
                a.temperature,
                a.frequency);
  Serial.printf("[IM1281C] B: U=%.4fV I=%.4fA P=%.4fW E=%.4fkWh PF=%.3f CO2=%.4fkg\n",
                b.voltage,
                b.current,
                b.power,
                b.energy,
                b.powerFactor,
                b.co2);
}

/* LED Control Functions */
void updateLED()
{
  unsigned long current_time = millis();

  switch (s_led_state)
  {
  case LED_INITIALIZING:
    // Blue blinking during initialization
    if (current_time - s_blink_last_time >= BLINK_INTERVAL)
    {
      s_blink_last_time = current_time;
      s_blink_on = !s_blink_on;

      if (s_blink_on)
      {
        leds[0] = Rgb{0, 0, 255}; // Blue on
      }
      else
      {
        leds[0] = Rgb{0, 0, 0}; // Off
      }
      leds.show();
    }
    break;

  case LED_WIFI_CONNECTED:
    // Blue solid - WiFi connected, OCPP connecting
    leds[0] = Rgb{0, 0, 255}; // Blue solid
    leds.show();
    break;

  case LED_OCPP_CONNECTED:
    // Green solid - OCPP connected
    leds[0] = Rgb{0, 255, 0}; // Green solid
    leds.show();
    break;

  case LED_ERROR:
    // Red solid - Error state
    leds[0] = Rgb{255, 0, 0}; // Red solid
    leds.show();
    break;

  case LED_RESET_TX:
    // Yellow fast blink - Ready to clear transaction
    if (current_time - s_blink_last_time >= 100)
    {
      s_blink_last_time = current_time;
      s_blink_on = !s_blink_on;

      if (s_blink_on)
        leds[0] = Rgb{150, 150, 0}; // Yellow
      else
        leds[0] = Rgb{0, 0, 0};

      leds.show();
    }
    break;

  case LED_FACTORY_RESET:
    // Magenta fast blink - Ready to factory reset
    if (current_time - s_blink_last_time >= 100)
    {
      s_blink_last_time = current_time;
      s_blink_on = !s_blink_on;

      if (s_blink_on)
        leds[0] = Rgb{255, 0, 255}; // Magenta
      else
        leds[0] = Rgb{0, 0, 0};

      leds.show();
    }
    break;
  }
}

void setup()
{
  // Get MAC address and set as Charge Point Serial Number
  esp_efuse_mac_get_default(mac);
  snprintf(cpSerial, sizeof(cpSerial),
           "%02X%02X%02X%02X%02X%02X",
           mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);

  if (strlen(CFG_CP_IDENTIFIER) > 0)
  {
    strncpy(cp_identifier, CFG_CP_IDENTIFIER, sizeof(cp_identifier) - 1);
    cp_identifier[sizeof(cp_identifier) - 1] = '\0';
  }
  else
  {
    // Auto-generate Charge Point Identifier based on MAC (e.g. HLCP_A1B2C3)
    snprintf(cp_identifier, sizeof(cp_identifier), "HLCP_%s", cpSerial + 6);
  }

  // reset LED
  leds[0] = Rgb{0, 0, 0};
  leds.show();
  // initialize Serial
  Serial.begin(115200);
  delay(1000);
  Serial.printf("\n\n%s(%s) made by %s\n", CFG_CP_MODAL, cpSerial, CFG_CP_VENDOR);
  Serial.printf("Charge Point Identifier: %s\n", cp_identifier);
  Serial.println("Initializing firmware...\n");

  // Initialize LED
  s_led_state = LED_INITIALIZING;
  s_blink_last_time = 0;
  s_blink_on = false;

  // Initialize CC switches (input) and panel LEDs (low-active output)
  pinMode(PIN_CC1, INPUT);
  pinMode(PIN_CC2, INPUT);
  pinMode(PIN_LED1, OUTPUT);
  pinMode(PIN_LED2, OUTPUT);
  pinMode(PIN_RELAY1, OUTPUT);
  pinMode(PIN_RELAY2, OUTPUT);
  digitalWrite(PIN_LED1, HIGH);   // Off by default (low-active)
  digitalWrite(PIN_LED2, HIGH);   // Off by default (low-active)
  digitalWrite(PIN_RELAY1, HIGH); // Off by default (low-active)
  digitalWrite(PIN_RELAY2, HIGH); // Off by default (low-active)
  updateConnectorPluggedState();
  updatePanelLedsFromPlugState();

  leds[0] = Rgb{255, 255, 0};
  leds.show();

  // Initialize I2C for OLED (from schematic pin map)
  Wire.begin(PIN_OLED_SDA, PIN_OLED_SCL);

  // Initialize SSD1306 OLED over I2C, try 0x3C then 0x3D
  if (display.begin(SSD1306_SWITCHCAPVCC, 0x3C))
  {
    s_oled_ready = true;
  }
  else if (display.begin(SSD1306_SWITCHCAPVCC, 0x3D))
  {
    s_oled_ready = true;
  }
  else
  {
    s_oled_ready = false;
    Serial.println("[OLED] SSD1306 init failed");
  }

  if (s_oled_ready)
  {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("Helios EVCS");
    display.setCursor(0, 12);
    display.println("Booting...");
    display.display();
  }

  // Initialize IM1281C over UART2 (default: address 1, 4800bps, 8N1)
  im1281c.begin(Serial2, PIN_U2RXD, PIN_U2TXD);

  // Initialize SPI bus for RC522
  SPI.begin(PIN_RC_SCK, PIN_RC_MISO, PIN_RC_MOSI, PIN_RC_CS);
  pinMode(PIN_RC_CS, OUTPUT);
  digitalWrite(PIN_RC_CS, HIGH);
  pinMode(PIN_RC_RST, OUTPUT);
  digitalWrite(PIN_RC_RST, HIGH);
  rfid.PCD_Init();

  // Load configuration from Preferences
  Preferences preferences;
  preferences.begin("ocpp-config", false);
  String b = preferences.getString("backend", CFG_OCPP_BACKEND);
  String p = preferences.getString("ocpp_password", CFG_OCPP_PASSWORD ? CFG_OCPP_PASSWORD : "");

  Serial.printf("\n[OCPP] Loaded Backend URL: %s\n", b.c_str());
  Serial.printf("[OCPP] Loaded Password length: %d\n", p.length());

  strncpy(ocpp_backend, b.c_str(), sizeof(ocpp_backend) - 1);
  ocpp_backend[sizeof(ocpp_backend) - 1] = '\0';
  strncpy(ocpp_password, p.c_str(), sizeof(ocpp_password) - 1);
  ocpp_password[sizeof(ocpp_password) - 1] = '\0';

  WiFiManager wm;
  wm.setSaveConfigCallback(saveConfigCallback);
  wm.setSaveParamsCallback(saveConfigCallback);
  wm.setParamsPage(true);

  // Use autocomplete=off to prevent browsers from autofilling old URLs after a reset
  WiFiManagerParameter custom_ocpp_backend("backend", "OCPP Backend URL", ocpp_backend, 128, "autocomplete=\"off\"");
  WiFiManagerParameter custom_ocpp_password("ocpp_password", "OCPP Basic AuthKey", ocpp_password, 64, "autocomplete=\"off\" type=\"password\"");

  wm.addParameter(&custom_ocpp_backend);
  wm.addParameter(&custom_ocpp_password);

  const char *customHeadElement = R"rawliteral(
    <style>
      :root {
        font-family: -apple-system, BlinkMacSystemFont, "Segoe UI", sans-serif;
        color: #1f2a37;
        --primarycolor: #2563eb;
      }

      body,
      body.invert {
        margin: 0;
        background: #f4f6fb;
        min-height: 100vh;
        display: flex;
        justify-content: center;
        align-items: center;
        padding: 16px;
        color: #1f2a37;
      }

      .wrap {
        width: min(360px, 100%);
        background: #fff;
        border-radius: 16px;
        padding: 24px;
        box-shadow: 0 8px 24px rgba(15, 23, 42, 0.12);
        text-align: left;
      }

      h1,
      h2,
      h3,
      h4 {
        color: #111827;
        margin-top: 0;
        text-align: center;
      }

      label {
        display: block;
        margin-bottom: 6px;
        font-weight: 600;
      }

      input,
      select,
      button {
        border-radius: 10px;
        width: 100%;
        font-size: 1rem;
        box-sizing: border-box;
      }

      input[type="text"],
      input[type="password"],
      input[type="number"],
      input[type="email"],
      select {
        padding: 12px;
        margin: 0 0 16px 0;
        border: 1px solid #d0d7e2;
        background: #f9fafc;
        transition: border 0.2s, box-shadow 0.2s, background 0.2s;
      }

      input:focus,
      select:focus {
        outline: none;
        border-color: #2563eb;
        background: #fff;
        box-shadow: 0 0 0 3px rgba(37, 99, 235, 0.15);
      }

      button,
      input[type="submit"],
      button.D {
        padding: 12px;
        border: none;
        background: #2563eb;
        color: #fff;
        font-weight: 600;
        cursor: pointer;
        transition: background 0.2s, transform 0.1s;
      }

      button.D {
        background: #dc2626;
      }

      button:hover,
      input[type="submit"]:hover {
        background: #1d4ed8;
      }

      button:active,
      input[type="submit"]:active {
        transform: scale(0.99);
      }

      .msg {
        border-radius: 12px;
        border: 1px solid #e5e7eb;
        background: #f9fafb;
        padding: 16px;
        color: #374151;
      }

      .msg.P {
        border-color: #2563eb;
        background: rgba(37, 99, 235, 0.08);
        color: #1f2a37;
      }

      .msg.S {
        border-color: #16a34a;
        background: rgba(22, 163, 74, 0.08);
      }

      .msg.D {
        border-color: #dc2626;
        background: rgba(220, 38, 38, 0.08);
      }

      .q {
        display: none;
      }

      a {
        color: #2563eb;
        font-weight: 600;
      }

      hr {
        border: none;
        height: 1px;
        background: #e5e7eb;
        margin: 24px 0;
      }
    </style>
  )rawliteral";
  wm.setCustomHeadElement(customHeadElement);
  bool autoConnectRet = wm.autoConnect(cp_identifier, cpSerial);

  if (shouldSaveConfig)
  {
    strncpy(ocpp_backend, custom_ocpp_backend.getValue(), sizeof(ocpp_backend) - 1);
    ocpp_backend[sizeof(ocpp_backend) - 1] = '\0';
    strncpy(ocpp_password, custom_ocpp_password.getValue(), sizeof(ocpp_password) - 1);
    ocpp_password[sizeof(ocpp_password) - 1] = '\0';

    preferences.putString("backend", ocpp_backend);
    preferences.putString("ocpp_password", ocpp_password);
    Serial.println("Saved new OCPP config to Preferences");
  }
  preferences.end();

  if (!autoConnectRet)
  {
    Serial.println("Failed to connect and hit timeout");
    s_led_state = LED_ERROR;
  }
  else
  {
    Serial.println("WiFi connected successfully");
    s_led_state = LED_WIFI_CONNECTED;

    mg_mgr_init(&mgr);
    const char *basic_auth_password = (strlen(ocpp_password) > 0) ? ocpp_password : nullptr;
    unsigned char *basic_auth_password_bytes = nullptr;
    size_t basic_auth_password_len = 0;

    if (basic_auth_password)
    {
      basic_auth_password_bytes = reinterpret_cast<unsigned char *>(const_cast<char *>(basic_auth_password));
      basic_auth_password_len = strlen(basic_auth_password);
    }

    MicroOcpp::MOcppMongooseClient *client = new MicroOcpp::MOcppMongooseClient(
        &mgr,
        ocpp_backend,
        cp_identifier,
        nullptr,
        0,
        "",
        MicroOcpp::makeDefaultFilesystemAdapter(MicroOcpp::FilesystemOpt::Use_Mount_FormatOnFail),
        MicroOcpp::ProtocolVersion(1, 6));

    // Preferences and firmware config are the source of truth. Override any stale
    // values cached in MicroOcpp's ws-conn storage before the first reconnect cycle.
    client->setBackendUrl(ocpp_backend);
    client->setChargeBoxId(cp_identifier);
    if (basic_auth_password_bytes)
    {
      client->setAuthKey(basic_auth_password_bytes, basic_auth_password_len);
    }
    else
    {
      client->setAuthKey(reinterpret_cast<const unsigned char *>(""), 0);
    }
    client->reloadConfigs();

    mocpp_initialize(*client, ChargerCredentials(CFG_CP_MODAL, CFG_CP_VENDOR, CFG_CP_FW_VERSION, cpSerial, nullptr, nullptr, CFG_CB_SERIAL, nullptr, nullptr), MicroOcpp::makeDefaultFilesystemAdapter(MicroOcpp::FilesystemOpt::Use_Mount_FormatOnFail));

    // Expose both physical connectors to CSMS and feed live plug-state from CC switches.
    // connectorId 1 <-> CC1, connectorId 2 <-> CC2
    setConnectorPluggedInput([]()
                             { return s_cc1_plugged; },
                             1);
    setConnectorPluggedInput([]()
                             { return s_cc2_plugged; },
                             2);

    // Occupied state drives StatusNotification (Available <-> Preparing/Finishing)
    // to report plug-in / unplug events even without an active transaction.
    setOccupiedInput([]()
                     { return s_cc1_plugged; },
                     1);
    setOccupiedInput([]()
                     { return s_cc2_plugged; },
                     2);

    // Bind IM1281C metering values to OCPP connector 1 / 2
    setEnergyMeterInput([]()
                        { return s_meter_data_ready ? energyKwhToWh(s_meter_a.energy) : 0; },
                        1);
    setPowerMeterInput([]()
                       { return s_meter_data_ready ? s_meter_a.power : 0.0f; },
                       1);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_a.voltage : 0.0f; },
                       "Voltage", "V", nullptr, nullptr, 1);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_a.current : 0.0f; },
                       "Current.Import", "A", nullptr, nullptr, 1);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_a.powerFactor : 0.0f; },
                       "Power.Factor", nullptr, nullptr, nullptr, 1);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_a.frequency : 0.0f; },
                       "Frequency", "Hz", nullptr, nullptr, 1);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_a.temperature : 0.0f; },
                       "Temperature", "Celsius", nullptr, nullptr, 1);

    setEnergyMeterInput([]()
                        { return s_meter_data_ready ? energyKwhToWh(s_meter_b.energy) : 0; },
                        2);
    setPowerMeterInput([]()
                       { return s_meter_data_ready ? s_meter_b.power : 0.0f; },
                       2);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_b.voltage : 0.0f; },
                       "Voltage", "V", nullptr, nullptr, 2);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_b.current : 0.0f; },
                       "Current.Import", "A", nullptr, nullptr, 2);
    addMeterValueInput([]()
                       { return s_meter_data_ready ? s_meter_b.powerFactor : 0.0f; },
                       "Power.Factor", nullptr, nullptr, nullptr, 2);

    // MicroOcpp defaults MeterValuesSampledData to Energy + Power only.
    // Expand sampled measurands so CSMS can receive voltage/current/PF/frequency/temperature.
    static const char *kMeterValuesSampledData =
        "Energy.Active.Import.Register,Power.Active.Import,Voltage,Current.Import,Power.Factor,Frequency,Temperature";
    if (auto *cfg = MicroOcpp::getConfigurationPublic("MeterValuesSampledData"))
    {
      cfg->setString(kMeterValuesSampledData);
    }
    if (auto *cfg = MicroOcpp::getConfigurationPublic("MeterValuesAlignedData"))
    {
      cfg->setString(kMeterValuesSampledData);
    }
    MicroOcpp::configuration_save();

    // Custom RemoteStartTransaction policy:
    // accept only when target connector is idle + operative + plugged.
    setRequestHandler(
        "RemoteStartTransaction",
        [](JsonObject payload)
        {
          s_remote_start_accepted = false;

          const char *idTag = payload["idTag"] | "";
          if (!idTag || !*idTag)
          {
            return;
          }

          int reqConnectorId = payload["connectorId"] | -1;
          unsigned int targetConnector = 0;

          if (reqConnectorId >= 1 && reqConnectorId <= 2)
          {
            if (isConnectorStartReady((unsigned int)reqConnectorId))
            {
              targetConnector = (unsigned int)reqConnectorId;
            }
          }
          else
          {
            for (unsigned int cid = 1; cid <= 2; cid++)
            {
              if (isConnectorStartReady(cid))
              {
                targetConnector = cid;
                break;
              }
            }
          }

          if (targetConnector == 0)
          {
            showOledMessage("REMOTE REJ", "No idle plug", "or not ready", 2500, true);
            return;
          }

          auto tx = beginTransaction_authorized(idTag, nullptr, targetConnector);
          s_remote_start_accepted = (tx != nullptr);
          if (s_remote_start_accepted)
          {
            Serial.printf("[main] Remote start accepted on connector %u\n", targetConnector);
            showOledMessage("REMOTE OK", String("C") + String(targetConnector), "Charging", 2200, false);
          }
          else
          {
            showOledMessage("REMOTE REJ", String("C") + String(targetConnector), "Busy/Not Ready", 2500, true);
          }
        },
        []() -> std::unique_ptr<MicroOcpp::JsonDoc>
        {
          auto doc = std::unique_ptr<MicroOcpp::JsonDoc>(new MicroOcpp::JsonDoc(JSON_OBJECT_SIZE(1)));
          JsonObject payload = doc->to<JsonObject>();
          payload["status"] = s_remote_start_accepted ? "Accepted" : "Rejected";
          return doc;
        });

    // For development/recovery: Set up BOOT button (GPIO 0)
    pinMode(0, INPUT_PULLUP);

    // Forcefully accept rejected RemoteStopTransaction (if hardware goes out of sync with CSMS)
    setOnSendConf("RemoteStopTransaction", [](JsonObject payload)
                  {
        if (!strcmp(payload["status"], "Rejected")) {
          unsigned int connectorId = payload["connectorId"] | 1;
          if (connectorId < 1 || connectorId > 2)
          {
            connectorId = 1;
          }
          Serial.printf("[main] MicroOcpp rejected RemoteStopTransaction on connector %u. Force overriding and stopping charging...\n", connectorId);
          endTransaction(nullptr, "Remote", connectorId);
        } });
  }
}

void loop()
{
  updateConnectorPluggedState();
  stopIfUnplugged();
  pollRfidCard();
  expireAuthWaitIfNeeded();
  pollIm1281c();

  mg_mgr_poll(&mgr, 10);
  mocpp_loop();
  updateChargeActuators();

  // Handle BOOT button (GPIO 0) interactions for recovery
  bool is_btn_pressed = (digitalRead(0) == LOW);
  static unsigned long boot_press_time = 0;
  static bool boot_was_pressed = false;

  if (is_btn_pressed)
  {
    if (!boot_was_pressed)
    {
      boot_was_pressed = true;
      boot_press_time = millis();
    }

    unsigned long held_time = millis() - boot_press_time;
    if (held_time >= 7000)
    {
      s_led_state = LED_FACTORY_RESET;
    }
    else if (held_time >= 3000)
    {
      s_led_state = LED_RESET_TX;
    }
  }
  else
  {
    if (boot_was_pressed)
    {
      unsigned long held_time = millis() - boot_press_time;
      if (held_time >= 7000)
      {
        Serial.println("BOOT button held for > 7s! Clearing WiFi and OCPP settings, then restarting...");

        // Clear WiFi completely
        WiFi.disconnect(true, true);
        WiFiManager wm;
        wm.resetSettings();

        // Clear Preferences explicitely
        Preferences preferences;
        preferences.begin("ocpp-config", false);
        preferences.remove("backend");
        preferences.remove("ocpp_password");
        preferences.clear();
        preferences.end();
        Serial.println("NVS ocpp-config cleared.");

        // Clear MicroOcpp FS configs (this removes MO's cached URL)
        auto fs = MicroOcpp::makeDefaultFilesystemAdapter(MicroOcpp::FilesystemOpt::Use_Mount_FormatOnFail);
        fs->remove(MO_WSCONN_FN);
        Serial.println("MicroOcpp config cache cleared.");

        // Give time for serial to print and NVS to sync
        delay(1000);
        ESP.restart();
      }
      else if (held_time >= 3000)
      {
        Serial.println("BOOT button held for > 3s! Forcefully ending dangling transactions on connector 1 and 2...");
        endTransaction(nullptr, "PowerLoss", 1);
        endTransaction(nullptr, "PowerLoss", 2);
      }
      boot_was_pressed = false;
      // Temporarily set to init so the logic below restores the actual network state accurately
      s_led_state = LED_INITIALIZING;
    }
  }

  // Only update default LED states if button is not overriding them
  if (!is_btn_pressed)
  {
    auto ctx = getOcppContext();
    if (ctx && ctx->getConnection().isConnected())
    {
      if (s_led_state != LED_OCPP_CONNECTED)
      {
        s_led_state = LED_OCPP_CONNECTED;
      }
    }
    else
    {
      if (s_led_state != LED_WIFI_CONNECTED)
      {
        s_led_state = LED_WIFI_CONNECTED;
      }
    }
  }

  updateLED();
  refreshOled();

  delay(10);
}