#include "firehazard.h"
#include "arduinopid.h"
#include <stdio.h>
#include <stdbool.h>

#include <math.h>
#include <esp_log.h>
#include <nvs.h>
#include "driver/ledc.h"
#include "esp_err.h"
#include "utils.h"

static const char *TAG = "fire";

static struct PID pid = PID_DEFAULT();

static void pwm_init(void);
static void pwm_set(float duty);

void fire_get_tuning(float *kp, float *ki, float *kd) {
    *kp = pid.kp;
    *ki = pid.ki;
    *kd = pid.kd;
}

void fire_init() {
    printf("Regulator init");

    PID_Initialize(&pid);
    PID_SetOutputLimits(&pid, 0, 1);
    PID_SetCtlMode(&pid, PID_MANUAL);

    nvs_handle nvs;
    ESP_ERROR_CHECK(nvs_open("config", NVS_READWRITE, &nvs)); // must use RW to allow opening nonexistent
    union uf32 kp, ki, kd;
    kp.f = 0.4f;
    ki.f = 0.5f;
    kd.f = 0.0f;
    nvs_get_u32(nvs, "kp", &kp.u);
    nvs_get_u32(nvs, "ki", &ki.u);
    nvs_get_u32(nvs, "kd", &kd.u);
    PID_SetTunings(&pid, kp.f, ki.f, kd.f);
    nvs_close(nvs);

    pwm_init();

    fire_setlevel(20);
//    fire_enable(true);
}

void fire_set_tuning(float kp, float ki, float kd) {
    ESP_LOGI(TAG, "PID set tuning Kp=%.3f, Ki=%.3f, Kd=%.3f", kp, ki, kd);
    PID_SetTunings(&pid, kp, ki, kd);
}

float fire_get_setpoint(bool off_is_zero) {
    if (off_is_zero) {
        return pid.ctlMode == PID_MANUAL ? 0 : pid.Setpoint;
    } else {
        return pid.Setpoint;
    }
}

void fire_setlevel(float cels) {
    ESP_LOGI(TAG, "PID set target %.3f°C", cels);

    if (cels < 0) cels = 0;
    if (cels > MAX_SETPOINT) cels = MAX_SETPOINT;
    PID_SetSetpoint(&pid, cels);
}

void fire_enable(bool enable) {
    ESP_LOGI(TAG, "Heater %s", enable ? "enable" : "disable");

    PID_SetCtlMode(&pid, enable ? PID_AUTOMATIC : PID_MANUAL);
}

bool fire_enabled() {
    return pid.ctlMode == PID_AUTOMATIC;
}

void fire_regulate(float cels) {
    PID_Compute(&pid, cels);

    if (cels > MAX_TSENSE || cels < MIN_TSENSE) {
        ESP_LOGE(TAG, "Tsense out of bounds! Stopping.");
        fire_enable(false);
    }

    if (pid.ctlMode == PID_MANUAL) {
        pwm_set(0);
    } else {
//        printf("PID in %.2f°C, out %.3f, I %.3f\n", cels, pid.Output, pid.ITerm);
        pwm_set(pid.Output);
    }
}


#define PWM_CHANNEL LEDC_CHANNEL_1
#define PWM_TIMER LEDC_TIMER_1
#define PWM_BIT_NUM LEDC_TIMER_12_BIT

#define PWM_PIN GPIO_NUM_14

static void pwm_init(void)
{
    ledc_channel_config_t ledc_channel_left = {0};
    ledc_channel_left.gpio_num = PWM_PIN;
    ledc_channel_left.speed_mode = LEDC_HIGH_SPEED_MODE;
    ledc_channel_left.channel = PWM_CHANNEL;
    ledc_channel_left.intr_type = LEDC_INTR_DISABLE;
    ledc_channel_left.timer_sel = PWM_TIMER;
    ledc_channel_left.duty = 0;

    ledc_timer_config_t ledc_timer = {0};
    ledc_timer.speed_mode = LEDC_HIGH_SPEED_MODE;
    ledc_timer.duty_resolution = PWM_BIT_NUM;
    ledc_timer.timer_num = PWM_TIMER;
    ledc_timer.freq_hz = 1; // TODO ??

    ESP_ERROR_CHECK( ledc_channel_config(&ledc_channel_left) );
    ESP_ERROR_CHECK( ledc_timer_config(&ledc_timer) );
}

static void pwm_set(float duty)
{
    uint32_t max_duty = (1 << PWM_BIT_NUM);// - 1
    uint32_t dutycycle = lroundf((duty) * (float)max_duty);
//    printf("Dutycycle %d\n", dutycycle);

    ESP_ERROR_CHECK( ledc_set_duty(LEDC_HIGH_SPEED_MODE, PWM_CHANNEL, dutycycle) );
    ESP_ERROR_CHECK( ledc_update_duty(LEDC_HIGH_SPEED_MODE, PWM_CHANNEL) );
}