#ifndef __THER_HEATING_HPP__
#define __THER_HEATING_HPP__
#include <algorithm>
#include <zephyr/device.h>
#include <zephyr/drivers/pwm.h>
#include <zephyr/drivers/sensor.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

namespace ther {

class HeatingPad {
public:
  /// Start PID temperature control toward the given target.
  static auto Start(sensor_value target_temp) -> void {
    s_target = sensor_value_to_double(&target_temp);
    s_integral = 0.0;
    s_last_error = 0.0;
    s_last_time = k_uptime_get();

    printk("[heat] start target=%.1f°C\n", s_target);

    // Check PWM device
    if (!device_is_ready(s_pwm_spec.dev)) {
      printk("[heat] ERR: PWM device not ready\n");
      return;
    }
    printk("[heat] PWM dev=%s ch=%d period=%u ns\n", s_pwm_spec.dev->name,
           s_pwm_spec.channel, s_pwm_spec.period);

    // Check sensor
    if (!device_is_ready(s_temp_sensor)) {
      printk("[heat] ERR: temp sensor not ready\n");
      return;
    }
    printk("[heat] temp sensor OK\n");

    // Test: set 50% duty immediately to verify PWM works
    const uint32_t pulse_50 = s_pwm_spec.period / 2;
    int ret = pwm_set_pulse_dt(&s_pwm_spec, pulse_50);
    printk("[heat] test 50%% duty pulse=%u ret=%d\n", pulse_50, ret);

    k_timer_init(&s_timer, Tick, nullptr);
    k_timer_start(&s_timer, K_NO_WAIT, K_MSEC(kUpdatePeriodMs));
    s_active = true;
  }
  static auto Start() {

    const uint32_t pulse_50 = s_pwm_spec.period / 2;
    int ret = pwm_set_pulse_dt(&s_pwm_spec, pulse_50);
    return ret;
  }
  /// Stop PID control and turn off heating.
  static auto Stop() -> void {
    printk("[heat] stop\n");
    s_active = false;
    k_timer_stop(&s_timer);
    pwm_set_pulse_dt(&s_pwm_spec, 0);
  }

  /// Change the target temperature while keeping PID running.
  static auto SetTarget(sensor_value target_temp) -> void {
    s_target = sensor_value_to_double(&target_temp);
    printk("[heat] new target=%.1f°C\n", s_target);
  }

private:
  static constexpr uint32_t kPeriodNs = PWM_KHZ(5);
  static constexpr uint32_t kUpdatePeriodMs = 100;
  static constexpr double kOutMin = 0.0;
  static constexpr double kOutMax = 1.0;

  // ── PI gains (tune these) ─────────────────────────
  static constexpr double kKp = 2.0;  // proportional
  static constexpr double kKi = 0.02; // integral
  static constexpr double kKd = 0.0; // derivative (not needed for heating-only)

  inline static bool s_active{false};
  inline static double s_target{0.0};
  inline static double s_integral{0.0};
  inline static double s_last_error{0.0};
  inline static int64_t s_last_time{0};

  inline static pwm_dt_spec s_pwm_spec = {
      .dev = DEVICE_DT_GET(DT_NODELABEL(pwm1)),
      .channel = 1,
      .period = PWM_KHZ(5),
      .flags = PWM_POLARITY_NORMAL,
  };

  inline static const device *s_temp_sensor =
      DEVICE_DT_GET(DT_NODELABEL(heating_pad_ntc));

  inline static k_timer s_timer;

  /// Called periodically by the timer.
  static auto Tick(k_timer * /*timer*/) -> void {
    double current_temp;
    if (ReadTemperature(current_temp) != 0) {
      printk("[heat] ERR: read temp failed\n");
      return;
    }

    printk("[heat] tick: cur=%.1f°C target=%.1f°C\n", current_temp, s_target);

    // PI computation
    const int64_t now = k_uptime_get();
    const double dt = static_cast<double>(now - s_last_time) / 1000.0;
    s_last_time = now;

    const double error = s_target - current_temp;

    // Proportional
    const double p = kKp * error;

    // Integral with anti-windup
    s_integral += kKi * error * dt;
    s_integral = std::clamp(s_integral, kOutMin, kOutMax);

    s_last_error = error;

    // Compute output, clamp to [0, 1]
    double output = p + s_integral;
    output = std::clamp(output, kOutMin, kOutMax);

    // Apply PWM duty cycle
    const uint32_t pulse = static_cast<uint32_t>(output * kPeriodNs);
    printk("[heat] PID out=%.2f%% pulse=%u / %u\n", output * 100.0, pulse,
           s_pwm_spec.period);
    pwm_set_pulse_dt(&s_pwm_spec, pulse);
  }

  /// Read temperature from the NTC sensor into `out_temp` (in °C).
  static auto ReadTemperature(double &out_temp) -> int {
    if (0 != sensor_sample_fetch(s_temp_sensor)) {
      printk("[heat] ERR: sensor fetch failed\n");
      return -EIO;
    }

    sensor_value val{};
    if (0 !=
        sensor_channel_get(s_temp_sensor, SENSOR_CHAN_AMBIENT_TEMP, &val)) {
      printk("[heat] ERR: channel_get failed\n");
      return -EIO;
    }

    out_temp = sensor_value_to_double(&val);
    return 0;
  }
};

} // namespace ther

#endif