#include <stdio.h>
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "driver/pulse_cnt.h"
#include "driver/gpio.h"
#include "bdc_motor.h"
#include "pid_ctrl.h"
#include "modbus.h"
#include "math.h"
#include "servo.h"

static const char *TAG = "bdc_control";

/* PWM输出的定时器配置 */
#define BDC_MCPWM_TIMER_RESOLUTION_HZ 10000000 /* 精度，即计数器的频率 10MHz, 1 tick = 0.1us */
#define BDC_MCPWM_FREQ_HZ 25000 /* PWM周期频率  25KHz PWM */
#define BDC_MCPWM_DUTY_TICK_MAX (BDC_MCPWM_TIMER_RESOLUTION_HZ / BDC_MCPWM_FREQ_HZ) /* PWM占空比最大值，由定时器计数频率和PWM周期频率计算得来 */

/* 编码器对应IO口 */
#define BDC_ENCODER_GPIO_A 35
#define BDC_ENCODER_GPIO_B 34
#define BDC_ENCODER_GPIO_A1 26
#define BDC_ENCODER_GPIO_B1 33

/* 有刷直流电机H桥控制，IR203s驱动芯片的LOW是低有效导通 */
#define BDC_CH1_LOW0_GPIO 1
#define BDC_CH1_HI0_GPIO 2
#define BDC_CH1_LOW1_GPIO 3
#define BDC_CH1_HI1_GPIO 4
#define BDC_CH1_AOUT_GPIO 5
#define BDC_CH0_AOUT_GPIO 6
#define BDC_CH0_LOW0_GPIO 7
#define BDC_CH0_HI0_GPIO 8
#define BDC_CH0_LOW1_GPIO 9
#define BDC_CH0_HI1_GPIO 10

/* 编码器计数范围 */
#define BDC_ENCODER_PCNT_HIGH_LIMIT 1000
#define BDC_ENCODER_PCNT_LOW_LIMIT -1000

/* pid控制时间周期ms */
#define BDC_PID_LOOP_PERIOD_MS 50

/* 范围 */
#define BDC_SERVO_INPUT_RANGE   1000
#define BDC_SPEED_INPUT_RANGE   1000 
#define BDC_SERVO_REAL_RANGE   (45)
#define BDC_SPEED_REAL_RANGE   (50)     /* 实际还要更高，为了测试先降低一点 */ 


/* 车体测量值 */
#define WHEEL_LENGTH    (31.0)
#define WHEEL_WIDTH     (32.4)
#define WHEEL_R0        (WHEEL_WIDTH / 2)

/* 数学值定义 */
#define PI 3.1415926

typedef struct
{
    int id;     /* 标识一个电机 */
    
    uint32_t bdc_low0_gpio; 
    uint32_t bdc_low1_gpio;
    bdc_motor_handle_t motor;
    pcnt_unit_handle_t pcnt_encoder;
    pid_ctrl_block_handle_t pid_ctrl;
    esp_timer_handle_t pid_loop_timer;

    int accumu_count;
    int report_pulses;
} motor_control_context_t;


static bool example_pcnt_on_reach(pcnt_unit_handle_t unit, const pcnt_watch_event_data_t *edata, void *user_ctx)
{
    int *accumu_count = (int *)user_ctx;
    *accumu_count += edata->watch_point_value;
    return false;
}


/* 根据期望速度和期望角度算左轮或者右轮的速度 */
/* id=0左   id=1右 */
int speed_lr_calculate(int speed, int angle, int id)
{
    double r = WHEEL_LENGTH / (tan(angle * PI / 180));  /* 得到旋转半径，以中轴线计算 */

    if (id == 0) return (int)(speed - WHEEL_R0 / r * speed);
    else return (int)(speed + WHEEL_R0 / r * speed);
}


/* 定时器定时触发，使用PID算法控速
 * 注意：速度以每次采样期间编码器计数个数为衡量标准
 * 待修正：对于转弯的逻辑，可能左右轮的速度是期望速度的不同函数关系
 */
static void pid_loop_cb(void *args)
{
    static int last_pulse_count[2] = {0, 0};
    static int cur_pwm_compare[2] = {0, 0};
    motor_control_context_t *ctx = (motor_control_context_t *)args;
    pcnt_unit_handle_t pcnt_unit = ctx->pcnt_encoder; 
    pid_ctrl_block_handle_t pid_ctrl = ctx->pid_ctrl;
    bdc_motor_handle_t motor = ctx->motor;

    /* pid参数设置 */
    if (modbus_data->pid_flag)
    {
        pid_ctrl->Kp = (short)modbus_data->p / 100.0;
        pid_ctrl->Ki = (short)modbus_data->i / 100.0;
        pid_ctrl->Kd = (short)modbus_data->d / 100.0;
        pid_ctrl->integral_err = 0;
        pid_ctrl->last_output = 0;
        pid_ctrl->previous_err1 = 0;
        pid_ctrl->previous_err2 = 0;
        
        ESP_LOGI(TAG, "id%d pid set successfully:%f %f %f", ctx->id, pid_ctrl->Kp, pid_ctrl->Ki, pid_ctrl->Kd);
        modbus_data->speed = 0;
        modbus_data->angle = 0;
        bdc_motor_set_speed(ctx->motor, 0, ctx->id);
        servo_set_angle(0);
        modbus_data->pid_flag &= ~(0x01 << (ctx->id));
    }


     /* 从Modbus服务器端获得期望速度和期望角度 */
    int speed = (short)modbus_data->speed;
    float angle = (short)modbus_data->angle;  
    speed = speed * 1.0 / BDC_SPEED_INPUT_RANGE * BDC_SPEED_REAL_RANGE;
    angle = -1.0 * angle / BDC_SERVO_INPUT_RANGE * BDC_SERVO_REAL_RANGE;  

    /* 后轮速度控制 */
    /* 1. 根据预期速度和角度计算左轮或者右轮的期望速度 */
    speed = speed_lr_calculate(speed, angle, ctx->id);

    /* 2. 从编码器获得计数，得到实际速度 */
    int cur_pulse_count = 0;
    pcnt_unit_get_count(pcnt_unit, &cur_pulse_count);
    cur_pulse_count += ctx->accumu_count;            

    int real_pulses = cur_pulse_count - last_pulse_count[ctx->id]; //新的脉冲数减去上一次的脉冲数为真实脉冲数
    last_pulse_count[ctx->id] = cur_pulse_count;
    ctx->report_pulses = real_pulses;


    /* 3. 计算速度误差->增量式PID->控制电机输出 */
    float error = speed - real_pulses; /* 注意：这里使用的隐式类型转换 */
    float output = 0;
    pid_compute(pid_ctrl, error, &output);
    output += cur_pwm_compare[ctx->id];
    cur_pwm_compare[ctx->id] = output;

    if (output > 0)  ESP_ERROR_CHECK(bdc_motor_forward(ctx->motor, ctx->id)); 
    else if (output < 0) {
        ESP_ERROR_CHECK(bdc_motor_reverse(ctx->motor, ctx->id));
        output = -output;
    }
    else ESP_ERROR_CHECK(bdc_motor_brake(ctx->motor, ctx->id)); 
    bdc_motor_set_speed(ctx->motor, (uint32_t)output, ctx->id);


    /* 更新Modbus端的数据 */
    if (ctx->id == 0)
    {
        modbus_data->encoder_accumul = (int)cur_pulse_count;

        modbus_data->expect_speedl = (short)speed;
        modbus_data->real_speedl = (short)real_pulses; 
        modbus_data->real_motor_outputl = (short)cur_pwm_compare[ctx->id];
    }
    else if (ctx->id == 1)
    {
        modbus_data->encoder_accumur = (int)cur_pulse_count;

        modbus_data->expect_speedr = (short)speed;
        modbus_data->real_speedr = real_pulses; 
        modbus_data->real_motor_outputr = (short)cur_pwm_compare[ctx->id];
    }
}


extern uint32_t low_gpio[2][2];
void bdc_motor_dir_gpio_init(motor_control_context_t motor_ctrl_ctx)
{
    ESP_LOGI(TAG, "DC motor dir gpio init");

    /* low mos管状态初始化，使用GPIO驱动，初始化高电平断开 */
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = ((1ULL<<motor_ctrl_ctx.bdc_low0_gpio) | (1ULL<<motor_ctrl_ctx.bdc_low1_gpio));
    io_conf.pull_down_en = 0;
    io_conf.pull_up_en = 0;
    gpio_config(&io_conf);

    gpio_set_level(motor_ctrl_ctx.bdc_low0_gpio, 1);
    gpio_set_level(motor_ctrl_ctx.bdc_low1_gpio, 1);

    low_gpio[motor_ctrl_ctx.id][0] = motor_ctrl_ctx.bdc_low0_gpio;
    low_gpio[motor_ctrl_ctx.id][1] = motor_ctrl_ctx.bdc_low1_gpio;
}


void bdc_motor_speed_pwm_init(uint32_t bdc_hi0_gpio, uint32_t bdc_hi1_gpio, int group_id, bdc_motor_handle_t *motor_ret)
{
    ESP_LOGI(TAG, "DC motor speed pwm init");

    /* hi mos管状态设置，使用mcpwm驱动，高电平有效 */
    bdc_motor_config_t motor_config = {
        .pwm_freq_hz = BDC_MCPWM_FREQ_HZ,
        .pwma_gpio_num = bdc_hi0_gpio,
        .pwmb_gpio_num = bdc_hi1_gpio,
    };

    bdc_motor_mcpwm_config_t mcpwm_config = {
        .group_id = group_id,     
        .resolution_hz = BDC_MCPWM_TIMER_RESOLUTION_HZ, // 0.1us精密度
    };
    
    bdc_motor_handle_t motor = NULL;
    ESP_ERROR_CHECK(bdc_motor_new_mcpwm_device(&motor_config, &mcpwm_config, &motor)); //根据当前误差检查
    *motor_ret = motor;
}


void bdc_encoder_init(uint32_t bdc_encoder_gpioa, uint32_t bdc_encoder_gpiob, int *on_reach_callbackarg, pcnt_unit_handle_t *pcnt_unit_ret)
{
    ESP_LOGI(TAG, "Init pcnt driver to decode rotary signal");

    /* 实例化pcnt，pcnt单元是自动分配，无需特指单元 */
    pcnt_unit_config_t unit_config = {
        .high_limit = BDC_ENCODER_PCNT_HIGH_LIMIT,
        .low_limit = BDC_ENCODER_PCNT_LOW_LIMIT,
    };
    pcnt_unit_handle_t pcnt_unit = NULL;
    ESP_ERROR_CHECK(pcnt_new_unit(&unit_config, &pcnt_unit));

    /* 设置过滤器宽度ns */
    pcnt_glitch_filter_config_t filter_config = {
        .max_glitch_ns = 1000,
    };
    ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit, &filter_config));

    /* 设置通道a */
    pcnt_chan_config_t chan_a_config = {
        .edge_gpio_num = bdc_encoder_gpioa,
        .level_gpio_num = bdc_encoder_gpiob,
    };
    pcnt_channel_handle_t pcnt_chan_a = NULL;
    ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_a_config, &pcnt_chan_a));

    /* 设置通道b */
    pcnt_chan_config_t chan_b_config = {
        .edge_gpio_num = bdc_encoder_gpiob,
        .level_gpio_num = bdc_encoder_gpioa,
    };
    pcnt_channel_handle_t pcnt_chan_b = NULL;
    ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_b_config, &pcnt_chan_b));
    
    /* 配置两通道模式，实现两个通道的边沿都能计数 */
    ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE));
    ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
    ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE));
    ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
    
    /* 配置该pcnt的上下观察点，以及溢出回调函数 */
    ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_HIGH_LIMIT));
    ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_LOW_LIMIT));
    pcnt_event_callbacks_t pcnt_cbs = {
        .on_reach = example_pcnt_on_reach, // accumulate the overflow in the callback
    };
    ESP_ERROR_CHECK(pcnt_unit_register_event_callbacks(pcnt_unit, &pcnt_cbs, on_reach_callbackarg));

    ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit));
    ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit));
    ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit));

    *pcnt_unit_ret = pcnt_unit;
}


/* 注：只将不同的电机的不同参数作为接口留出，里面也有需要配置的共同参数 */
void bdc_pid_init(float p, float i, float d, pid_ctrl_block_handle_t *pid_ctrl_ret)
{
    ESP_LOGI(TAG, "Create PID control block");
    
    pid_ctrl_parameter_t pid_runtime_param = {
        .kp = p,                                
        .ki = i,                                
        .kd = d,                                
        .cal_type = PID_CAL_TYPE_INCREMENTAL,      // 增量式pid
        .max_output = BDC_MCPWM_DUTY_TICK_MAX - 1, //最大输出（占空比-1）、最小输出0
        .min_output = -(BDC_MCPWM_DUTY_TICK_MAX - 1),
        .max_integral = 20, //最大积分限制1000、最小积分限制-1000
        .min_integral = -20,
    };
    pid_ctrl_block_handle_t pid_ctrl = NULL;
    pid_ctrl_config_t pid_config = {
        .init_param = pid_runtime_param,
    };
    ESP_ERROR_CHECK(pid_new_control_block(&pid_config, &pid_ctrl));

    *pid_ctrl_ret = pid_ctrl;
}



void bdc_ctrl_timer_init(motor_control_context_t *motor_ctrl_ctx, esp_timer_handle_t *pid_loop_timer_ret)
{
    ESP_LOGI(TAG, "Create a timer to do PID calculation periodically");

    const esp_timer_create_args_t periodic_timer_args = {
        .callback = pid_loop_cb,
        .arg = motor_ctrl_ctx, 
        .name = "pid_loop"      
    };
    esp_timer_handle_t pid_loop_timer = NULL;
    ESP_ERROR_CHECK(esp_timer_create(&periodic_timer_args, &pid_loop_timer));

    *pid_loop_timer_ret = pid_loop_timer;
}


void bdc_motor_init_all(void)
{
    /* 左轮 */
    static motor_control_context_t motor_ctrl_ctxl = {
        .id = 0,    /* 左轮 */
        .bdc_low0_gpio = BDC_CH1_LOW0_GPIO,
        .bdc_low1_gpio = BDC_CH1_LOW1_GPIO,
        .motor = NULL,
        .pcnt_encoder = NULL,
        .accumu_count = 0
    };
    bdc_motor_dir_gpio_init(motor_ctrl_ctxl);
    bdc_motor_speed_pwm_init(BDC_CH1_HI0_GPIO, BDC_CH1_HI1_GPIO, 0, &(motor_ctrl_ctxl.motor));
    bdc_encoder_init(BDC_ENCODER_GPIO_A, BDC_ENCODER_GPIO_B, &(motor_ctrl_ctxl.accumu_count), &(motor_ctrl_ctxl.pcnt_encoder));
    bdc_pid_init(1, 0, 0, &(motor_ctrl_ctxl.pid_ctrl));
    bdc_ctrl_timer_init(&motor_ctrl_ctxl, &(motor_ctrl_ctxl.pid_loop_timer));


    /* 右轮 */
     static motor_control_context_t motor_ctrl_ctxr = {
        .id = 1,    /* 右轮 */
        .bdc_low0_gpio = BDC_CH0_LOW0_GPIO,
        .bdc_low1_gpio = BDC_CH0_LOW1_GPIO,
        .motor = NULL,
        .accumu_count = 0,
        .pcnt_encoder = NULL
    };
    bdc_motor_dir_gpio_init(motor_ctrl_ctxr);
    bdc_motor_speed_pwm_init(BDC_CH0_HI0_GPIO, BDC_CH0_HI1_GPIO, 1, &(motor_ctrl_ctxr.motor));
    bdc_encoder_init(BDC_ENCODER_GPIO_A1, BDC_ENCODER_GPIO_B1, &(motor_ctrl_ctxr.accumu_count), &(motor_ctrl_ctxr.pcnt_encoder));
    bdc_pid_init(1, 0, 0, &(motor_ctrl_ctxr.pid_ctrl));
    bdc_ctrl_timer_init(&motor_ctrl_ctxr, &(motor_ctrl_ctxr.pid_loop_timer));

    /* 启动电机 */
    ESP_LOGI(TAG, "Enable motor"); 
    ESP_ERROR_CHECK(bdc_motor_enable(motor_ctrl_ctxl.motor, motor_ctrl_ctxl.id)); 
    ESP_ERROR_CHECK(bdc_motor_enable(motor_ctrl_ctxr.motor, motor_ctrl_ctxr.id));

    /* 启动电机控制算法 */
    ESP_LOGI(TAG, "Start motor loop");                                                
    ESP_ERROR_CHECK(esp_timer_start_periodic(motor_ctrl_ctxl.pid_loop_timer, BDC_PID_LOOP_PERIOD_MS * 1000)); 
    ESP_ERROR_CHECK(esp_timer_start_periodic(motor_ctrl_ctxr.pid_loop_timer, BDC_PID_LOOP_PERIOD_MS * 1000)); 
}