#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 "esp_intr_alloc.h"
#include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h"
#include "esp_log.h"

#define TAG "DCMotor"

#define LOW_DUTY 200
#define HIGH_DUTY 1000
#define FWD_CHANNEL LEDC_CHANNEL_1
#define REV_CHANNEL LEDC_CHANNEL_0
#define DEADZONE 0.05
#define KP 0.05
#define KI 0
#define KD 0.020
#define MIN_PWM_DUTY 675
#define MAX_PWM_DUTY 1023
#define TICKS_PER_ROTATION 18.0
#define GEAR_RATIO 298

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();

    // Pin the PID task to Core 1 so it doesn't compete with the RMT driver,
    // which runs its ISR and internal tasks on Core 0.
    xTaskCreatePinnedToCore(reinterpret_cast<TaskFunction_t>(pid_task), "pid_task", 4096, this, 1,
                            &this->m_pid_task, 1);
}

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

static void IRAM_ATTR encoder_isr_handler(void *arg) {
    auto *ticks = static_cast<volatile int32_t *>(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) {
        (*ticks)++;
    } else {
        (*ticks)--;
    }
}

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);

    esp_err_t err = gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1);
    if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
        ESP_ERROR_CHECK(err);
    }
    gpio_isr_handler_add(static_cast<gpio_num_t>(DC_ENCODER_A), encoder_isr_handler,
                         reinterpret_cast<void *>(const_cast<int32_t *>(&m_encoder_ticks)));
}

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.store(angleControlCmd->angle(), std::memory_order_relaxed);
}

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

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

        const int64_t target = that->m_target_angle.load(std::memory_order_relaxed);
        const double error = that->m_current_angle - target;
        that->m_integral += error * KI;
        const double derivative = (error - that->m_last_error) * KD;
        that->m_last_error = error;

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

        if (std::abs(control) < DEADZONE) {
            esp_err_t err;
            err = ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
            err = ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }

            err = ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
            err = ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }

            vTaskDelay(300 / portTICK_PERIOD_MS);
            continue;
        }

        esp_err_t err;
        if (control > 0) {
            err = ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, pwm);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
            err = ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }

            err = ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
            err = ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
        } else {
            err = ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, pwm);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
            err = ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }

            err = ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
            err = ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL);
            if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
        }

        vTaskDelay(75 / portTICK_PERIOD_MS);
    }
}

std::vector<Flatbuffers::sensor_value> DCMotorActuator::get_sensor_data() {
    return {Flatbuffers::target_angle{(int16_t)m_current_angle},
            Flatbuffers::current_angle{(int16_t)m_current_angle}};
}