#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("% fan="));
  Serial.print((thermal.fanPwm() * 100) / 255);
  if (thermal.isIdle() && thermal.isFanOff()) {
    Serial.print(F("(off)"));
  } else if (thermal.isIdleCooling()) {
    Serial.print(F("(cooldown)"));
  }
  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"));
  } else if (thermal.isAdaptive()) {
    Serial.print(F("learned"));
  } else {
    Serial.print(F("manual"));
  }
  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("  autotune [C] learn PID (default: 40C when idle)"));
  Serial.println(F("  autotune stop"));
  Serial.println(F("  pid         show PID / adaptive status"));
  Serial.println(F("  pid default reset to factory PID"));
  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 (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 — keep chamber closed, wait ~10-20 min"));
    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, "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);
    } 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);
      }
    }
  }
}