#ifndef __THER_HEATING_HPP__
#define __THER_HEATING_HPP__
#include <algorithm>
#include <cmath>
#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:
  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);

    if (!device_is_ready(s_pwm_spec.dev)) {
      printk("[heat] ERR: PWM not ready\n");
      return;
    }
    printk("[heat] PWM OK\n");

    k_work_init(&s_work, WorkHandler);
    k_timer_init(&s_timer, TimerTick, nullptr);
    k_timer_start(&s_timer, K_NO_WAIT, K_MSEC(kUpdatePeriodMs));
    s_active = true;
  }

  static auto StartFullEnergy() -> void {
    pwm_set_pulse_dt(&s_pwm_spec, s_pwm_spec.period);
  }

  static auto Stop() -> void {
    s_active = false;
    k_timer_stop(&s_timer);
    pwm_set_pulse_dt(&s_pwm_spec, 0);
  }

  static auto CurrentTemp() -> sensor_value { return s_current_temp; }

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;
  static constexpr double kKp = 2.0;
  static constexpr double kKi = 0.02;

  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;
  inline static k_work s_work;
  inline static bool s_work_pending{false};

  /// Timer fires → submit work.  Skips if previous work hasn't finished.
  static auto TimerTick(k_timer * /*timer*/) -> void {
    if (s_work_pending) {
      return;
    }
    s_work_pending = true;
    k_work_submit(&s_work);
  }

  /// PID update running in system workqueue context.
  static auto WorkHandler(k_work * /*work*/) -> void {
    double current_temp;
    if (ReadTemperature(current_temp) != 0) {
      s_work_pending = false;
      return;
    }

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

    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;
    s_integral += kKi * error * dt;
    s_integral = std::clamp(s_integral, kOutMin, kOutMax);

    double output = kKp * error + s_integral;
    output = std::clamp(output, kOutMin, kOutMax);

    const uint32_t pulse = static_cast<uint32_t>(output * kPeriodNs);
    pwm_set_pulse_dt(&s_pwm_spec, pulse);
    s_work_pending = false;
  }

  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);
    s_current_temp = val;
    return 0;
  }
  inline static sensor_value s_current_temp{};
};

} // namespace ther

#endif