#include "SpiderFurnace.hpp"

// C++ system level
#include <cstdio>     // sprintf
// #include <functional>

// ESP32 specific
#include "esp_log.h"
#include "driver/gpio.h"
#include "driver/ledc.h"

// project specific
#include <Types.hpp>
#include <Time.hpp>
#include "SiliconTorch/NVSExplorer.hpp"

// qthing stuff
#include <qthing>
//#include <qthing/mqtt_common.hpp>
#include "SiliconTorch/CyanBus.hpp"

// misc
#include <nlohmann/json.hpp>


using nlohmann::json;
using SpiderLib::Time;


static const char* TAG = "SpiderFurnace";


namespace SiliconTorch {

  namespace Service {

    namespace SpiderFurnace {

      void SpiderFurnace::init() {
        setIcon("🔥");
        setName("SpiderFurnace");
        setNameSpace("SpiderFurnace");
      }


      void SpiderFurnace::start() {

        tc0 = new MAX31856::MAX31856(IO_MISO, IO_MOSI, IO_SCLK, IO_CS);

        // TODO: Configure tc0!!
        tc0->setConversionMode(false);               // disable automatic conversions

        tc0->setCJComp(true);                        // enable cold-junction compensation
        tc0->setPwrLnRjFrq(MAX31856::F50Hz);
        tc0->setOCFaultMode(MAX31856::OC_ENABLED0);  // TODO: Check resistors/datasheet!!

        tc0->setConversionMode(true);                // re-enable automatic conversions


        // Set-up Heater IO
        gpio_config_t conf;

        conf.intr_type    = GPIO_INTR_DISABLE;
        conf.mode         = GPIO_MODE_OUTPUT;
        conf.pin_bit_mask = 1ULL << IO_HEATER;
        conf.pull_up_en   = GPIO_PULLUP_DISABLE;
        conf.pull_down_en = GPIO_PULLDOWN_DISABLE;

        gpio_config(&conf);
        gpio_set_level((gpio_num_t)IO_HEATER, 0);


        // Set-up Heater PWM
        ledc_timer_config_t         timer_cfg;
        timer_cfg.duty_resolution = (ledc_timer_bit_t)PWM_BITS;
        timer_cfg.freq_hz         = PWM_FREQ;
        timer_cfg.speed_mode      = LEDC_HIGH_SPEED_MODE;
        timer_cfg.timer_num       = LEDC_TIMER_1;                  // FxCyanF uses ch0
        timer_cfg.clk_cfg         = LEDC_AUTO_CLK;

        ledc_channel_config_t       ledc_channel;
        ledc_channel.channel      = pwmCh;
        ledc_channel.duty         = 0;
        ledc_channel.gpio_num     = (gpio_num_t)IO_HEATER;
        ledc_channel.speed_mode   = LEDC_HIGH_SPEED_MODE;
        ledc_channel.hpoint       = 0;
        ledc_channel.timer_sel    = LEDC_TIMER_1;
        ledc_channel.intr_type    = LEDC_INTR_DISABLE; 

        ledc_fade_func_install(0);
        ledc_timer_config(&timer_cfg);
        ledc_channel_config(&ledc_channel);


        // pre-seed sliding window
        f32 t = tc0->getTemperature();
        for (u8 n = 0; n < SlidingWindowLength; n++)
          tc0dat->insert(t);


        // Set-up PID controller
        if (CFG_EN_PIDTask) {

          pidTask = new SpiderLib::Util::LambdaTask([&](){

            TickType_t lastWakeTime = xTaskGetTickCount();
            tc0age = Time::ms();

            f32 error      = 0.0f;
            f32 sum_error  = 0.0f;
            f32 diff_error = 0.0f;
            f32 last_error = 0.0f;

            while (true) {

              faultReg.value = tc0->readU8(MAX31856::SR::addr);

              if (!hasFault()) {
                f32 t = tc0->getTemperature();

                f32 deltaT = abs(t - temperature());
                //if (deltaT < 0) deltaT *= -1.0f;

                f32 tc0age_ = (Time::ms() - tc0age) / 1000.0f;  // change to seconds
                if (deltaT > tc0age_ * MaxTempChangePerSecond) {  // temp changed too fast!
                  faultReg.OPEN = true;  // mimic fault!
                } else {
                  tc0dat->insert(t);
                  tc0age = Time::ms();
                }
              }

              f32 t = temperature();

              if (!hasFault() && t <= MAX_TEMP) {

                error      = targetTemperature - t;
                sum_error += error;                  // TODO: "I festhalten"
                diff_error = last_error - error;
                last_error = error;

                f32 out    = kP * error;
                out       += kI * sum_error;
                out       += kD * diff_error;

                // if (out > MAX_PWM)
                // TODO: "I festhalten" ??

                setPWM(out);
              } else {
                setPWM(0.0f);
              }

              ESP_LOGI(TAG, "T: %.2f °C    Fault: %s    _i: %.2f   PWM: %.2f", t, hasFault() ? "true" : "false", sum_error, pwmVal);

              vTaskDelayUntil(&lastWakeTime, 1000 / TICK_FREQ / portTICK_PERIOD_MS);
            }
          });

        } else {

          qthing::add_message_callback(deviceTopic(TAG, "setPWM"), [&](const str& message) {
            setPWM(std::strtof(message.c_str(), NULL));
          });

        }


        if (CFG_EN_MQTT_PIDvals) {

          qthing::add_message_callback(deviceTopic(TAG, "kP"), [&](const str& message) {
            kP = std::strtof(message.c_str(), NULL);
          });

          qthing::add_message_callback(deviceTopic(TAG, "kI"), [&](const str& message) {
            kI = std::strtof(message.c_str(), NULL);
          });

          qthing::add_message_callback(deviceTopic(TAG, "kD"), [&](const str& message) {
            kD = std::strtof(message.c_str(), NULL);
          });

        }

      }

      void SpiderFurnace::setTargetTemperature(f32 t) {
        if (t >= 0.0f && t <= MAX_TEMP) targetTemperature = t;
      }

      json SpiderFurnace::getConfigJSON() const {

        json out;

        out["bla"] = "fasel";

        return out;
      }

      void SpiderFurnace::setPWM(f32 value) {
        if (value < 0.0f)    value = 0.0f;
        if (value > MAX_PWM) value = MAX_PWM;

        pwmVal     = value;
        u32 maxVal = (1 << PWM_BITS) - 1;
        u32 pwmVal = value * maxVal;

        ledc_set_duty(LEDC_HIGH_SPEED_MODE, pwmCh, pwmVal);
        ledc_update_duty(LEDC_HIGH_SPEED_MODE, pwmCh);
      }

      f32 SpiderFurnace::temperature() const {
        return tc0dat->average();
      }

      bool SpiderFurnace::hasFault() const {
        return faultReg.value > 0;
      }
    }
  }
}