#include <math.h>
#include <stdio.h>
#include "drv8701.h"
#include "driver/mcpwm_cmpr.h"
#include "driver/mcpwm_gen.h"
#include "driver/mcpwm_oper.h"
#include "driver/mcpwm_timer.h"
#include "driver/mcpwm_types.h"
#include "driver/gpio.h"
#include "esp_err.h"
#include "freertos/idf_additions.h"
#include "hal/gpio_types.h"
#include "portmacro.h"

#define PWM_RESOLUTION_HZ 2000000
#define PWM_TIMEBASE_PERIOD PWM_RESOLUTION_HZ/20000


void drv8701_task(void *args) {
    drv8701_task_config_t *motor_cfg = ((drv8701_task_config_t *)args);
    motor_cfg->in_ctrl_queue = xQueueCreate(3, sizeof(float));
    motor_cfg->current_mutex = xSemaphoreCreateMutex();
    
    // --------------------- Configure PWM -------------------------
    mcpwm_timer_handle_t timer = NULL;
    mcpwm_timer_config_t timer_cfg = {
        .group_id = motor_cfg->pwm_group_num,
        .clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
        .resolution_hz = PWM_RESOLUTION_HZ,
        .count_mode = MCPWM_TIMER_COUNT_MODE_UP,
        .period_ticks = PWM_TIMEBASE_PERIOD,
    };
    ESP_ERROR_CHECK(mcpwm_new_timer(&timer_cfg, &timer));

    mcpwm_oper_handle_t oper = NULL;
    mcpwm_operator_config_t oper_cfg = {
        .group_id = motor_cfg->pwm_group_num,
    };

    ESP_ERROR_CHECK(mcpwm_new_operator(&oper_cfg, &oper));
    ESP_ERROR_CHECK(mcpwm_operator_connect_timer(oper, timer));
    
    mcpwm_cmpr_handle_t cmpr = NULL;
    mcpwm_comparator_config_t cmpr_cfg = {
        .flags.update_cmp_on_tep = true,
    };
    
    ESP_ERROR_CHECK(mcpwm_new_comparator(oper, &cmpr_cfg, &cmpr));

    mcpwm_gen_handle_t gen = NULL;
    mcpwm_generator_config_t gen_cfg = {
        .gen_gpio_num = motor_cfg->en_pin,
    };

    ESP_ERROR_CHECK(mcpwm_new_generator(oper, &gen_cfg, &gen));
    ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(cmpr, 0));
    
    ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(gen,
                    MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
    ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(gen,
                    MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpr, MCPWM_GEN_ACTION_LOW)));
    
    ESP_ERROR_CHECK(mcpwm_timer_enable(timer));
    ESP_ERROR_CHECK(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP));

    // --------------------- Configure Direction -------------------------
    
    printf("Configuring Direction\n");
    gpio_config_t phase_cfg = {
        .pin_bit_mask = 1ULL << motor_cfg->ph_pin,
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE
    };
    gpio_config(&phase_cfg);

    // Main Loop
    float target_velocity = 0.0;
    while(1) {
        
        // Get new velocity if it has changed
        if(xQueueReceive(motor_cfg->in_ctrl_queue, &target_velocity, 0)){
            target_velocity = fmaxf(-1.0, fminf(1.0, target_velocity));
        }
        

        uint32_t abs_scaled_vel = (uint32_t) (fabsf(target_velocity) * ((float)PWM_TIMEBASE_PERIOD));
        mcpwm_comparator_set_compare_value(cmpr, abs_scaled_vel);

        if (target_velocity > 0.0) {
            gpio_set_level(motor_cfg->ph_pin, 1); 
        }
        else {
            gpio_set_level(motor_cfg->ph_pin, 0); 
        }

        vTaskDelay(50 / portTICK_PERIOD_MS);
    }
}

void set_motor_vel(drv8701_task_config_t *motor_cfg, float velocity) {
    xQueueSend(motor_cfg->in_ctrl_queue, &velocity, 0);
}