#include <Arduino.h>
#include "Core.h"

WiFiUDP udp;
Preferences prefs;
int udpPort = listenPort;

float currentDuty[NUM_CHANNELS] = {0.0f};
float targetDuty[NUM_CHANNELS]  = {0.0f};
float slewStartDuty[NUM_CHANNELS] = {0.0f};

unsigned long slewStartTime = 0;
unsigned long lastPacketTime = 0;
unsigned long lastFadeTime = 0;

float logicalPoints[5] = {0, 25, 50, 75, 100};

float calibratedPoints[NUM_CHANNELS][5] = {
  {0.0f, 25.0f, 50.0f, 75.0f, 99.0f},
  {0.0f, 24.0f, 49.0f, 74.0f, 98.0f},
  {0.0f, 26.0f, 51.0f, 76.0f, 99.0f},
  {0.0f, 25.0f, 50.0f, 75.0f, 97.0f},
  {0.0f, 25.0f, 50.0f, 75.0f, 99.0f},
  {0.0f, 24.0f, 50.0f, 74.0f, 98.0f},
  {0.0f, 25.0f, 49.0f, 75.0f, 97.0f},
  {0.0f, 26.0f, 50.0f, 76.0f, 99.0f}
};

bool overrideActive[NUM_CHANNELS] = {false};

ConnectionState connectionState = STATE_DISCONNECTED;

// UI IDs (defined here, used also by UI.cpp)
uint16_t connectionStatusLabel;
uint16_t calChannelDropdown;
uint16_t calInputs[5];
uint16_t calTestValueInput;
uint16_t calSaveButton;
uint16_t calOverrideSwitch;
uint8_t selectedCalChannel = 0;

// -------------------------------
// Calibration interpolation
// -------------------------------
float applyCalibration(uint8_t ch, float logicalDuty) {
  if (logicalDuty <= 0.0f) return calibratedPoints[ch][0];
  if (logicalDuty >= 100.0f) return calibratedPoints[ch][4];

  for (int i = 0; i < 4; i++) {
    if (logicalDuty >= logicalPoints[i] && logicalDuty <= logicalPoints[i+1]) {
      float x0 = logicalPoints[i];
      float x1 = logicalPoints[i+1];
      float y0 = calibratedPoints[ch][i];
      float y1 = calibratedPoints[ch][i+1];
      float t = (logicalDuty - x0) / (x1 - x0);
      return y0 + t * (y1 - y0);
    }
  }
  return calibratedPoints[ch][4];
}

void updateConnectionStatusUI(ConnectionState state) {
    const char* text = "Unknown";

    switch (state) {
        case STATE_DISCONNECTED:
            text = "Disconnected";
            break;
        case STATE_CONNECTING:
            text = "Connecting...";
            break;
        case STATE_WAIT_FOR_FIRST_PACKET:
            text = "Waiting for first UDP packet";
            break;
        case STATE_CONNECTED:
            text = "Connected";
            break;
    }

    ESPUI.updateControlValue(connectionStatusLabel, text);
}

// -------------------------------
// Core init (called from setup)
// -------------------------------
void coreInit() {
  // LEDC PWM init
  for (int ch = 0; ch < NUM_CHANNELS; ch++) {
    bool ok = ledcAttach(pwmPins[ch], pwmFrequency, pwmResolution);
    if (!ok) {
      Serial.print("LEDC attach failed on pin ");
      Serial.println(pwmPins[ch]);
    }
    ledcWrite(pwmPins[ch], 0);
  }

  // Preferences: load UDP port (default = listenPort)
  prefs.begin("analogmon", false);
  udpPort = prefs.getInt("udpPort", listenPort);

  // Load calibration from flash (if present)
  for (int ch = 0; ch < NUM_CHANNELS; ch++) {
    for (int i = 0; i < 5; i++) {
      String key = "cal_" + String(ch) + "_" + String(i);
      float val = prefs.getFloat(key.c_str(), calibratedPoints[ch][i]);
      calibratedPoints[ch][i] = val;
    }
  }

  // Start UDP with runtime port
  udp.begin(udpPort);
  Serial.print("Listening on UDP port ");
  Serial.println(udpPort);

  lastPacketTime = millis();
  connectionState = STATE_DISCONNECTED;
  updateConnectionStatusUI(connectionState);
}

// -------------------------------
// UDP parsing
// -------------------------------
void coreHandleUDP() {
  int packetSize = udp.parsePacket();
  if (packetSize <= 0) return;

  char buf[256];
  int len = udp.read(buf, sizeof(buf) - 1);
  buf[len] = '\0';

  float values[NUM_CHANNELS] = {0};
  int idx = 0;

  char* token = strtok(buf, ",");
  while (token != nullptr && idx < NUM_CHANNELS) {
    values[idx] = atof(token);
    idx++;
    token = strtok(nullptr, ",");
  }

  if (idx != NUM_CHANNELS) return;

  if (connectionState == STATE_DISCONNECTED) {
      // First valid packet after being disconnected → start CONNECTING
      Serial.println("STATE CHANGE: DISCONNECTED → CONNECTING (UDP connection established)");
      connectionState = STATE_CONNECTING;
      updateConnectionStatusUI(connectionState);

      // Initialize fade-in: start from 0 on all non-override channels
      for (int ch = 0; ch < NUM_CHANNELS; ch++) {
          if (!overrideActive[ch]) {
              currentDuty[ch] = 0.0f;
          }
      }

      lastPacketTime = millis();  // prevent watchdog during fade-in
  }
  else if (connectionState == STATE_CONNECTING) {
      // Ignore UDP values during fade-in, just keep watchdog alive
      lastPacketTime = millis();
  }
  else if (connectionState == STATE_WAIT_FOR_FIRST_PACKET) {
      // First real packet after fade-in completes
      Serial.println("STATE CHANGE: WAIT_FOR_FIRST_PACKET → CONNECTED (first UDP packet received)");

      for (int ch = 0; ch < NUM_CHANNELS; ch++) {
          if (!overrideActive[ch]) {
              targetDuty[ch] = values[ch];
              slewStartDuty[ch] = currentDuty[ch]; // currently ~100%
          }
      }

      slewStartTime = millis();
      lastPacketTime = millis();
      connectionState = STATE_CONNECTED;
      updateConnectionStatusUI(connectionState);

      // Debug output
      Serial.println("Received UDP packet (first after fade-in):");
      for (int i = 0; i < NUM_CHANNELS; i++) {
        Serial.print("  CH");
        Serial.print(i);
        Serial.print(" (");
        Serial.print(channelLabels[i]);
        Serial.print("): ");
        Serial.print(values[i], 2);
        if (channelUnits[i][0] != '\0') {
          Serial.print(" ");
          Serial.print(channelUnits[i]);
        }
        Serial.println();
      }
      Serial.println();
  }
  else if (connectionState == STATE_CONNECTED) {
      // Normal UDP-driven update
      for (int ch = 0; ch < NUM_CHANNELS; ch++) {
          if (!overrideActive[ch]) {
              targetDuty[ch] = values[ch];
              slewStartDuty[ch] = currentDuty[ch];
          }
      }
      slewStartTime = millis();
      lastPacketTime = millis();

      // Debug output
      Serial.println("Received UDP packet:");
      for (int i = 0; i < NUM_CHANNELS; i++) {
        Serial.print("  CH");
        Serial.print(i);
        Serial.print(" (");
        Serial.print(channelLabels[i]);
        Serial.print("): ");
        Serial.print(values[i], 2);
        if (channelUnits[i][0] != '\0') {
          Serial.print(" ");
          Serial.print(channelUnits[i]);
        }
        Serial.println();
      }
      Serial.println();
  }
}

// -------------------------------
// Connection state machine
// -------------------------------
void coreUpdateState() {
  unsigned long now = millis();

  switch (connectionState) {

      case STATE_CONNECTED: {
          // Check for lost connection
          if (now - lastPacketTime > watchdogTimeout) {
              Serial.println("STATE CHANGE: CONNECTED → DISCONNECTED (UDP connection lost)");
              connectionState = STATE_DISCONNECTED;
              updateConnectionStatusUI(connectionState);
              break;
          }

          // Normal slew-rate limiting
          float progress = (float)(now - slewStartTime) / (float)slewDuration;
          if (progress > 1.0f) progress = 1.0f;

          for (int ch = 0; ch < NUM_CHANNELS; ch++) {
              if (!overrideActive[ch]) {
                  float newDuty = slewStartDuty[ch] + (targetDuty[ch] - slewStartDuty[ch]) * progress;
                  currentDuty[ch] = newDuty;

                  float calibratedDuty = applyCalibration(ch, newDuty);
                  int duty = (int)((calibratedDuty / 100.0f) * ((1 << pwmResolution) - 1));
                  ledcWrite(pwmPins[ch], duty);
              }
          }
      } break;

      case STATE_CONNECTING: {
          // If we lose packets even while connecting, fall back to DISCONNECTED
          if (now - lastPacketTime > watchdogTimeout) {
              Serial.println("STATE CHANGE: CONNECTING → DISCONNECTED (no packets during fade-in)");
              connectionState = STATE_DISCONNECTED;
              updateConnectionStatusUI(connectionState);
              break;
          }

          // Fade-in animation: 0 → 100% on all non-override channels
          if (now - lastFadeTime >= FADE_INTERVAL) {
              lastFadeTime = now;

              bool allReached = true;

              for (int ch = 0; ch < NUM_CHANNELS; ch++) {
                  if (!overrideActive[ch]) {
                      float cd = currentDuty[ch];

                      // Exponential approach to 100%
                      cd = cd * FADE_IN_FACTOR + 100.0f * (1.0f - FADE_IN_FACTOR);
                      currentDuty[ch] = cd;

                      float calibratedDuty = applyCalibration(ch, cd);
                      int duty = (int)((calibratedDuty / 100.0f) * ((1 << pwmResolution) - 1));
                      ledcWrite(pwmPins[ch], duty);

                      // Check if we're close enough to 100%
                      if (cd < 99.0f) {
                          allReached = false;
                      }
                  }
              }

              if (allReached) {
                  Serial.println("STATE CHANGE: CONNECTING → STATE_WAIT_FOR_FIRST_PACKET (fade-in complete)");
                  connectionState = STATE_WAIT_FOR_FIRST_PACKET;
                  updateConnectionStatusUI(connectionState);
              }
          }
      } break;

      case STATE_WAIT_FOR_FIRST_PACKET: {
          // Hold at ~100%, do nothing until first UDP packet arrives
          // (handled in coreHandleUDP)
      } break;

      case STATE_DISCONNECTED: {
          // Watchdog fade-to-zero (always active in this state)
          if (now - lastFadeTime >= FADE_INTERVAL) {
              lastFadeTime = now;

              for (int ch = 0; ch < NUM_CHANNELS; ch++) {

                if (currentDuty[ch] > 0.0f) {

                  currentDuty[ch] *= FADE_OUT_FACTOR;

                  if (currentDuty[ch] < 0.01f)
                    currentDuty[ch] = 0.0f;

                  float calibratedDuty = applyCalibration(ch, currentDuty[ch]);
                  int duty = (int)((calibratedDuty / 100.0f) * ((1 << pwmResolution) - 1));

                  ledcWrite(pwmPins[ch], duty);
                }
              }
          }
      } break;
  }
}
