#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_SHT31.h>

#include "config.h"
#include "csv_logger.h"
#include "tca9548a.h"
#include "thermal_controller.h"

Tca9548a mux(TCA9548A_ADDRESS);
Adafruit_SHT31 sht31;
ThermalController thermal;

SensorReading sensors[SENSOR_COUNT];

uint32_t lastSensorReadMs = 0;
uint32_t lastControlMs = 0;
uint32_t lastReportMs = 0;

char serialLine[48];
uint8_t serialLineLen = 0;
bool csvLogEnabled = LOG_CSV_DEFAULT;

bool readSensorOnChannel(uint8_t channel, SensorReading &out) {
  if (!mux.selectChannel(channel)) {
    out.valid = false;
    return false;
  }

  if (!sht31.begin(SHT31_ADDRESS)) {
    out.valid = false;
    return false;
  }

  const float temp = sht31.readTemperature();
  const float humidity = sht31.readHumidity();

  if (isnan(temp) || isnan(humidity)) {
    out.valid = false;
    return false;
  }

  out.temperatureC = temp;
  out.humidityPct = humidity;
  out.valid = true;
  return true;
}

void readAllSensors() {
  for (uint8_t i = 0; i < SENSOR_COUNT; ++i) {
    readSensorOnChannel(SENSOR_CHANNELS[i], sensors[i]);
  }
  mux.disableAll();
}

float averageValidTemperature() {
  float sum = 0.0f;
  uint8_t count = 0;

  for (uint8_t i = 0; i < SENSOR_COUNT; ++i) {
    if (sensors[i].valid) {
      sum += sensors[i].temperatureC;
      ++count;
    }
  }

  return count > 0 ? sum / static_cast<float>(count) : NAN;
}

float minValidTemperature() {
  float minTemp = INFINITY;

  for (uint8_t i = 0; i < SENSOR_COUNT; ++i) {
    if (sensors[i].valid && sensors[i].temperatureC < minTemp) {
      minTemp = sensors[i].temperatureC;
    }
  }

  return minTemp < INFINITY ? minTemp : NAN;
}

float maxValidTemperature() {
  float maxTemp = -INFINITY;

  for (uint8_t i = 0; i < SENSOR_COUNT; ++i) {
    if (sensors[i].valid && sensors[i].temperatureC > maxTemp) {
      maxTemp = sensors[i].temperatureC;
    }
  }

  return maxTemp > -INFINITY ? maxTemp : NAN;
}

float cornerTemperatureSpread() {
  const float minTemp = minValidTemperature();
  const float maxTemp = maxValidTemperature();
  if (isnan(minTemp) || isnan(maxTemp)) {
    return NAN;
  }
  return maxTemp - minTemp;
}

void printStatus(float avgTemp, float minTemp, float maxTemp) {
  Serial.print(F("target="));
  if (thermal.isIdle()) {
    Serial.print(F("idle"));
  } else {
    Serial.print(thermal.target(), 1);
  }
  Serial.print(F("C cutoff="));
  if (thermal.isIdle()) {
    Serial.print(F("n/a"));
  } else {
    Serial.print(thermal.cutoffThreshold(), 1);
  }
  Serial.print(F("C avg="));
  Serial.print(avgTemp, 2);
  Serial.print(F("C min="));
  Serial.print(minTemp, 2);
  Serial.print(F("C max="));
  Serial.print(maxTemp, 2);
  Serial.print(F("C spread="));
  Serial.print(thermal.cornerSpread(), 2);
  Serial.print(F("C heatlim="));
  Serial.print(thermal.heaterAllowance(), 0);
  Serial.print(F("% heater="));
  Serial.print(thermal.heaterDutyPercent(), 1);
  Serial.print(F("% htop="));
  if (thermal.isIdle()) {
    Serial.print(F("n/a"));
  } else {
    Serial.print(thermal.maxHeatStopAt(avgTemp), 1);
  }
  Serial.print(F("C hblk="));
  Serial.print(thermal.heaterBlockReason());
  Serial.print(F(" ssr="));
  Serial.print(thermal.isHeaterSsrOn() ? F("on") : F("off"));
  Serial.print(F(" fan="));
  Serial.print(thermal.fanPwm());
  Serial.print(F("/255("));
  Serial.print((thermal.fanPwm() * 100) / 255);
  Serial.print(F("%)"));
  if (thermal.isFanTestActive(millis())) {
    Serial.print(F(" TEST"));
  } else if (thermal.isFanCharacterizeActive()) {
    Serial.print(F("(fanchars-"));
    Serial.print(thermal.fanCharacterizePhaseName());
    Serial.print(F(")"));
  } else if (thermal.isIdle() && thermal.isFanOff()) {
    Serial.print(F("(off)"));
  } else if (thermal.isIdleCooling()) {
    Serial.print(F("(cooldown)"));
  } else if (thermal.fanPwm() == 0) {
    Serial.print(F("(cmd-off)"));
  }
  Serial.print(F(" cutoff="));
  Serial.print(thermal.isCutoffActive() ? F("YES") : F("no"));
  Serial.print(F(" failsafe="));
  Serial.print(thermal.isFailSafeActive() ? F("YES") : F("no"));
  Serial.print(F(" mode="));
  if (thermal.isAutotuning()) {
    Serial.print(F("autotune/"));
    Serial.print(thermal.autotunePhaseName());
    Serial.print(F(" "));
    Serial.print(thermal.autotuneElapsedMs(millis()) / 1000UL);
    Serial.print(F("s "));
    Serial.print(thermal.autotuneCycleCount());
    Serial.print(F("/"));
    Serial.print(AUTOTUNE_CYCLES_REQUIRED);
    Serial.print(F("cyc pre>="));
    Serial.print(thermal.autotunePreheatTargetC(), 0);
    Serial.print(F("C"));
  } else if (thermal.isFanCharacterizeActive()) {
    Serial.print(F("fanchars/"));
    Serial.print(thermal.fanCharacterizePhaseName());
    Serial.print(F(" "));
    Serial.print(thermal.fanCharacterizeElapsedMs(millis()) / 1000UL);
    Serial.print(F("s run "));
    const char *fcPhase = thermal.fanCharacterizePhaseName();
    if (strcmp(fcPhase, "precool") == 0) {
      Serial.print(F("pre"));
    } else if (strcmp(fcPhase, "cool") == 0) {
      Serial.print(F("n"));
      Serial.print(thermal.fanCharacterizeProfileIndex() + 1);
    } else if (thermal.isFanCharacterizeRefineRun()) {
      Serial.print(F("refine"));
    } else {
      Serial.print(thermal.fanCharacterizeProfileIndex() + 1);
    }
    Serial.print(F("/"));
    Serial.print(thermal.fanCharacterizeProfileCount());
    Serial.print(F(" fan="));
    Serial.print(thermal.fanCharacterizeFanPwm());
    Serial.print(F(" heat="));
    Serial.print(thermal.fanCharacterizeHeaterPct(), 0);
    Serial.print(F("%"));
  } else {
    Serial.print(thermal.regulatingModeName());
  }
  Serial.print(F(" sensors=["));

  for (uint8_t i = 0; i < SENSOR_COUNT; ++i) {
    if (i > 0) {
      Serial.print(F(", "));
    }
    Serial.print(F("ch"));
    Serial.print(SENSOR_CHANNELS[i]);
    Serial.print(F(":"));
    if (sensors[i].valid) {
      Serial.print(sensors[i].temperatureC, 1);
      Serial.print(F("C/"));
      Serial.print(sensors[i].humidityPct, 0);
      Serial.print(F("%"));
    } else {
      Serial.print(F("ERR"));
    }
  }

  Serial.println(F("]"));
}

void printHelp() {
  Serial.println(F("Commands:"));
  Serial.println(F("  target <C>  set target (0 = idle)"));
  Serial.println(F("  fan off     cancel idle fan override (auto-off below 40C)"));
  Serial.println(F("  fan on      idle fan 30% (optional, auto-off below 40C)"));
  Serial.println(F("  fan test N  set fan PWM 0-255 for 15s (verify wiring)"));
  Serial.println(F("  autotune [C] learn heat PI (default: 40C when idle)"));
  Serial.println(F("  autotune stop"));
  Serial.println(F("  fanchars      learn stir fan (30/100/60/80%% + refine)"));
  Serial.println(F("  fanchars stop | fanchars save"));
  Serial.println(F("  pid         show heat + mix PI gains"));
  Serial.println(F("  pid default reset all PI to factory"));
  Serial.println(F("  pid save    write current PI to EEPROM"));
  Serial.println(F("  mixpi       show mix PI gains"));
  Serial.println(F("  mixpi <Kp> <Ki>  set mix PI (spread -> fan)"));
  Serial.println(F("  mixpi default    reset mix PI to factory"));
  Serial.println(F("  status      print current readings"));
  Serial.println(F("  log on|off  CSV data stream"));
  Serial.println(F("  help        show this message"));
}

void refreshThermalSensorMax() {
  const float maxTemp = maxValidTemperature();
  if (!isnan(maxTemp)) {
    thermal.noteSensorMax(maxTemp);
  }
}

void processSerialLine(const char *line) {
  while (*line == ' ' || *line == '\t') {
    ++line;
  }
  if (*line == '\0') {
    return;
  }

  if (strncmp(line, "target ", 7) == 0) {
    const float targetC = atof(line + 7);
    if (targetC < TARGET_MIN_C || targetC > TARGET_MAX_C) {
      Serial.print(F("ERR target must be "));
      Serial.print(TARGET_MIN_C, 0);
      Serial.print(F("-"));
      Serial.print(TARGET_MAX_C, 0);
      Serial.println(F(" C"));
      return;
    }

    refreshThermalSensorMax();
    thermal.setTarget(targetC);
    if (targetC <= 0.0f) {
      Serial.println(F("OK idle — heater off, fan auto-off when max < 40C"));
    } else {
      Serial.print(F("OK target="));
      Serial.print(targetC, 1);
      Serial.print(F("C cutoff="));
      Serial.print(thermal.cutoffThreshold(), 1);
      Serial.println(F("C"));
    }
    return;
  }

  if (strcmp(line, "fan off") == 0) {
    refreshThermalSensorMax();
    if (!thermal.setFanOff()) {
      Serial.println(F("ERR fan off requires target 0 first (send: target 0)"));
      return;
    }
    if (thermal.isIdleCooling()) {
      Serial.println(F("OK cooling — fans stay on until max < 40C"));
    } else {
      Serial.println(F("OK fans off"));
    }
    return;
  }

  if (strcmp(line, "fan on") == 0) {
    refreshThermalSensorMax();
    if (!thermal.isIdle()) {
      Serial.println(F("ERR fan on only when target is 0"));
      return;
    }
    if (thermal.isIdleCooling()) {
      Serial.println(F("ERR fan on blocked — still cooling (max >= 40C)"));
      return;
    }
    thermal.setFanIdle();
    Serial.println(F("OK fans at idle 30%"));
    return;
  }

  if (strncmp(line, "fan test ", 9) == 0) {
    const int pwm = atoi(line + 9);
    if (pwm < 0 || pwm > 255) {
      Serial.println(F("ERR fan test PWM must be 0-255"));
      return;
    }
    thermal.stopFanTest();
    thermal.startFanTest(static_cast<uint8_t>(pwm), millis());
    Serial.print(F("OK fan PWM="));
    Serial.print(pwm);
    Serial.println(F(" for 15s — check speed/noise on D5"));
    return;
  }

  if (strcmp(line, "status") == 0) {
    const float avgTemp = averageValidTemperature();
    const float minTemp = minValidTemperature();
    const float maxTemp = maxValidTemperature();
    if (!isnan(avgTemp) && !isnan(minTemp) && !isnan(maxTemp)) {
      printStatus(avgTemp, minTemp, maxTemp);
    } else {
      Serial.println(F("WARN: no valid sensor readings"));
    }
    return;
  }

  if (strncmp(line, "autotune", 8) == 0) {
    if (strcmp(line, "autotune stop") == 0) {
      thermal.stopAutotune();
      Serial.println(F("OK autotune cancelled"));
      return;
    }

    float tuneTarget = thermal.target();
    if (line[8] == ' ') {
      tuneTarget = atof(line + 9);
    } else if (tuneTarget <= 0.0f) {
      tuneTarget = AUTOTUNE_DEFAULT_TEMP_C;
    }
    if (tuneTarget <= 0.0f || tuneTarget > TARGET_MAX_C) {
      Serial.println(F("ERR autotune temperature must be 25-80 C"));
      return;
    }

    thermal.setTarget(tuneTarget);
    if (!thermal.startAutotune(tuneTarget)) {
      Serial.println(F("ERR autotune already running"));
      return;
    }
    Serial.println(F("OK autotune started — preheat then relay, typically 15-40 min"));
    return;
  }

  if (strncmp(line, "fanchars", 8) == 0) {
    if (strcmp(line, "fanchars stop") == 0) {
      thermal.stopFanCharacterize();
      Serial.println(F("OK fanchars cancelled"));
      return;
    }
    if (strcmp(line, "fanchars save") == 0) {
      if (!thermal.saveStirFanFromCharacterize()) {
        Serial.println(F("ERR fanchars save — no completed run with winner"));
        return;
      }
      Serial.println(F("OK stir fan saved"));
      return;
    }

    float maxC = FANCHARS_MAX_CORNER_C;
    if (line[8] == ' ') {
      maxC = atof(line + 9);
    }

    if (maxC < 45.0f || maxC > EMERGENCY_MAX_TEMP_C - 5.0f) {
      Serial.println(F("ERR fanchars max 45-65 C"));
      return;
    }

    const float avgTemp = averageValidTemperature();
    if (isnan(avgTemp)) {
      Serial.println(F("ERR fanchars needs sensors"));
      return;
    }
    if (!thermal.startFanCharacterize(maxC, avgTemp)) {
      Serial.println(F("ERR fanchars busy"));
      return;
    }
    Serial.println(F("OK fanchars started"));
    return;
  }

  if (strcmp(line, "pid") == 0 || strcmp(line, "pid show") == 0) {
    thermal.printTuning();
    return;
  }

  if (strcmp(line, "pid default") == 0) {
    thermal.clearTuning();
    return;
  }

  if (strcmp(line, "pid save") == 0) {
    thermal.saveTuningToEeprom();
    return;
  }

  if (strcmp(line, "mixpi") == 0 || strcmp(line, "mixpi show") == 0) {
    Serial.print(F("Mix PI Kp="));
    Serial.print(thermal.mixKp(), 3);
    Serial.print(F(" Ki="));
    Serial.println(thermal.mixKi(), 4);
    return;
  }

  if (strcmp(line, "mixpi default") == 0) {
    thermal.resetMixTunings();
    return;
  }

  if (strncmp(line, "mixpi ", 6) == 0) {
    const float kp = atof(line + 6);
    const char *space = strchr(line + 6, ' ');
    if (space == nullptr) {
      Serial.println(F("ERR mixpi requires: mixpi <Kp> <Ki>"));
      return;
    }
    const float ki = atof(space + 1);
    if (kp <= 0.0f || ki < 0.0f) {
      Serial.println(F("ERR mixpi Kp must be > 0, Ki >= 0"));
      return;
    }
    thermal.setMixTunings(kp, ki);
    return;
  }

  if (strcmp(line, "help") == 0) {
    printHelp();
    return;
  }

  if (strcmp(line, "log on") == 0) {
    csvLogEnabled = true;
    printCsvHeader();
    Serial.println(F("OK csv logging on"));
    return;
  }

  if (strcmp(line, "log off") == 0) {
    csvLogEnabled = false;
    Serial.println(F("OK csv logging off"));
    return;
  }

  if (strcmp(line, "log") == 0) {
    Serial.println(csvLogEnabled ? F("OK csv logging on") : F("OK csv logging off"));
    return;
  }

  Serial.println(F("ERR unknown command (try help)"));
}

void pollSerial() {
  while (Serial.available() > 0) {
    const char c = static_cast<char>(Serial.read());
    if (c == '\n' || c == '\r') {
      if (serialLineLen > 0) {
        serialLine[serialLineLen] = '\0';
        processSerialLine(serialLine);
        serialLineLen = 0;
      }
    } else if (serialLineLen < sizeof(serialLine) - 1) {
      serialLine[serialLineLen++] = c;
    }
  }
}

void setup() {
  // Claim outputs before anything else — fan on, heater off (fail-safe)
  thermal.begin();

  Serial.begin(115200);
  while (!Serial && millis() < 3000) {
    delay(10);
  }

  Wire.begin();

  if (!mux.begin()) {
    Serial.println(F("ERROR: TCA9548A not found on I2C bus"));
  } else {
    Serial.println(F("TCA9548A detected"));
  }

  Serial.print(F("Filament dryer ready. "));
  if (thermal.isIdle()) {
    Serial.println(F("Idle — send target <C> to start drying"));
  } else {
    Serial.print(F("Target "));
    Serial.print(thermal.target(), 1);
    Serial.print(F(" C, hard cutoff at "));
    Serial.print(thermal.cutoffThreshold(), 1);
    Serial.println(F(" C"));
  }
  printHelp();
}

void loop() {
  const uint32_t now = millis();

  pollSerial();

  if (now - lastSensorReadMs >= SENSOR_READ_INTERVAL_MS) {
    lastSensorReadMs = now;
    readAllSensors();
  }

  if (now - lastControlMs >= CONTROL_INTERVAL_MS) {
    lastControlMs = now;

    const float avgTemp = averageValidTemperature();
    const float maxTemp = maxValidTemperature();
    const float spread = cornerTemperatureSpread();

    if (!isnan(avgTemp) && !isnan(maxTemp) && !isnan(spread)) {
      thermal.update(avgTemp, maxTemp, spread, now);

      float sensorTemps[SENSOR_COUNT];
      bool sensorValid[SENSOR_COUNT];
      for (uint8_t i = 0; i < SENSOR_COUNT; ++i) {
        sensorTemps[i] = sensors[i].temperatureC;
        sensorValid[i] = sensors[i].valid;
      }
      const float minTemp = minValidTemperature();
      if (thermal.isFanCharacterizeActive() && !isnan(minTemp)) {
        thermal.logFanCharacterizeIfDue(sensorTemps, sensorValid, SENSOR_COUNT, avgTemp, minTemp,
                                        maxTemp, spread, now);
      }
    } else {
      thermal.enterFailSafe();
      Serial.println(F("WARN: no valid sensor readings — heater off"));
    }
  }

  if (now - lastReportMs >= SERIAL_REPORT_INTERVAL_MS) {
    lastReportMs = now;
    const float avgTemp = averageValidTemperature();
    const float minTemp = minValidTemperature();
    const float maxTemp = maxValidTemperature();
    if (!isnan(avgTemp) && !isnan(minTemp) && !isnan(maxTemp)) {
      printStatus(avgTemp, minTemp, maxTemp);
      if (csvLogEnabled) {
        printCsvRow(now, thermal, sensors, SENSOR_COUNT, avgTemp, minTemp, maxTemp);
      }
    }
  }
}