#include "my_tsensor.h"
#include <Temperature_LM75_Derived.h>
#include "global.h"

static const char* tag = "tsensor";

T_SENSOR tSensorSettings;
TI_TMP102_Compatible *tSensor = nullptr;

/******************* Temperature Control   ********************/
  void Init_TSensor(uint8_t addr) {
    // Initialize the temperature sensor once with the provided I2C address
    if (tSensor == nullptr) {
      tSensor = new TI_TMP102_Compatible(addr);
      ESP_LOGI(tag, "TSensor initialized at I2C addr 0x%02X", addr);
    } else {
      ESP_LOGW(tag, "TSensor already initialized; ignoring re-init request (addr 0x%02X)", addr);
    }
  }

  
  static inline float clampf(float v, float lo, float hi) {
    if (v < lo) return lo;
    if (v > hi) return hi;
    return v;
  }

  void UpdateFanControl(float temperature, PWM_Output* pwmOut) {
    if (pwmOut == nullptr) {
      ESP_LOGW(tag, "UpdateFanControl called with null PWM output");
      return;
    }

    if (isnan(temperature) || isinf(temperature)) {
      ESP_LOGW(tag, "Invalid temperature reading: %f", temperature);
      return;
    }

    static uint8_t FanState = 0;
    tSensorSettings.temperature = temperature; // cache last temp
    float currentDuty = pwmOut->currDuty;
    float newDuty = currentDuty;

    // Sanitize settings locally (do not modify globals)
    float sp1 = tSensorSettings.Setpoint1;
    float sp2 = tSensorSettings.Setpoint2;
    float hyst = tSensorSettings.hyst;
    float fp1 = tSensorSettings.fanPower1;
    float fp2 = tSensorSettings.fanPower2;

    if (hyst < 0.0f) hyst = 0.0f;
    if (sp2 < sp1) {
      // Ensure sp2 >= sp1
      float tmp = sp1; sp1 = sp2; sp2 = tmp;
    }
    const float maxDuty = pwmOut->getMaxDuty();
    fp1 = clampf(fp1, 0.0f, maxDuty);
    fp2 = clampf(fp2, 0.0f, maxDuty);

    // Fan State Logic
    if ((FanState == 2) && (temperature < (sp2 - hyst))) {
      newDuty = fp1;
      FanState = 1;
      //ESP_LOGD(tag, "Dropping down to FanPower1");
    }
    else if ((FanState == 1) && (temperature < (sp1 - hyst))) {
      newDuty = 0;
      FanState = 0;
      //ESP_LOGD(tag, "Dropping down to FanPower0");
    }
    else if ((FanState <= 1) && (temperature > sp1)) {
      newDuty = fp1;
      if (temperature > sp2) {
        newDuty = fp2;
        FanState = 2;
        //ESP_LOGD(tag, "Raising up to FanPower2");
      } //else {
        //ESP_LOGD(tag, "Raising up to FanPower1");
      //}
    }

    // Apply new duty cycle if changed
    if (currentDuty != newDuty) {
      pwmOut->setOutput(newDuty);
      ESP_LOGD(tag, "Board T: %.2f F, Fan -> %.2f (state=%u)", temperature, newDuty, FanState);
    }
}