#include <cmath>

#include "AngleControlMessageBuilder.h"
#include "SensorMessageBuilder.h"
#include "constants/module.h"
#include "control/DCMotorActuator.h"
#include "driver/ledc.h"
#include "esp_attr.h"
#include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h"

#define LOW_DUTY 200
#define HIGH_DUTY 1000
#define FWD_CHANNEL LEDC_CHANNEL_0
#define REV_CHANNEL LEDC_CHANNEL_1
#define DEADZONE 0.1
#define KP 0.07
#define KI 0
#define KD 0.065
#define MIN_PWM_DUTY 675
#define MAX_PWM_DUTY 1024
#define TICKS_PER_ROTATION 14.0
#define GEAR_RATIO 298.0

DCMotorActuator::DCMotorActuator() {
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .duty_resolution = LEDC_TIMER_10_BIT,
        .timer_num = LEDC_TIMER_0,
        .freq_hz = 50000, // 4kHz
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    ledc_channel_config_t fwd_ledc_channel = {
        .gpio_num = DC_MOTOR_PWM_FWD,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = FWD_CHANNEL,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0,
    };
    ESP_ERROR_CHECK(ledc_channel_config(&fwd_ledc_channel));

    ledc_channel_config_t rev_ledc_channel = {
        .gpio_num = DC_MOTOR_PWM_REV,
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel = REV_CHANNEL,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0,
    };

    ESP_ERROR_CHECK(ledc_channel_config(&rev_ledc_channel));

    setup_encoder();

    this->m_pid_task = nullptr;
    this->m_target_angle = 0;
    this->m_integral = 0;
    this->m_last_error = 0;

    xTaskCreate(reinterpret_cast<TaskFunction_t>(pid_task), "pid_task", 3072, this, 1,
                &this->m_pid_task);
}

DCMotorActuator::~DCMotorActuator() {
    vTaskDelete(m_pid_task);
}

volatile int32_t encoder_ticks = 0;
volatile int8_t direction = 0;

static void IRAM_ATTR encoder_isr_handler(void *arg) {
    const int a = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_A));
    const int b = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_B));

    // Determine direction
    if (a == b) {
        encoder_ticks++;
        direction = 1;
    } else {
        encoder_ticks--;
        direction = -1;
    }
}

void DCMotorActuator::setup_encoder() {
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_ANYEDGE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = (1ULL << DC_ENCODER_A) | (1ULL << DC_ENCODER_B);
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    gpio_config(&io_conf);

    gpio_install_isr_service(0);
    gpio_isr_handler_add(static_cast<gpio_num_t>(DC_ENCODER_A), encoder_isr_handler, nullptr);
}

void DCMotorActuator::actuate(uint8_t *cmd) {
    // todo: Do we want to verify flatbuffers here?
    // Will introduce latency, and means that we need to also send the size to the actuate function.
    const auto *angleControlCmd =
        Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
    this->m_target_angle = angleControlCmd->angle();
}

void DCMotorActuator::pid_task(char *args) {
    const auto that = reinterpret_cast<DCMotorActuator *>(args);

    while (true) {
        that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);

        const double error = that->m_current_angle - that->m_target_angle;
        that->m_integral += error * KI;
        const double detivative = (error - that->m_last_error) * KD;
        that->m_last_error = error;

        double control = error * KP + that->m_integral + detivative;
        if (control > 1) {
            control = 1;
        } else if (control < -1) {
            control = -1;
        }
        const auto pwm =
            util::mapRange<double>(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY);

        if (std::abs(control) < DEADZONE) {
            ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0));
            ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL));

            ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0));
            ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL));
            vTaskDelay(300 / portTICK_PERIOD_MS);
            continue;
        }

        if (control > 0) {
            ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0));
            ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL));

            ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, pwm));
            ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL));
        } else {
            ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0));
            ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL));

            ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, pwm));
            ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL));
        }

        vTaskDelay(75 / portTICK_PERIOD_MS);
    }
}

std::vector<Flatbuffers::sensor_value> DCMotorActuator::get_sensor_data() {
    // todo: this really needs to return a int32, should also return two sensor data items, one for target one for current
    return {Flatbuffers::target_angle{(int16_t)m_current_angle},
            Flatbuffers::current_angle{(int16_t)m_current_angle}};
}