#include "MotorEncoder.h"
#include "ti_msp_dl_config.h"
#include "ti/driverlib/dl_timerg.h"
#include "zf_common_interrupt.h"
#include "zf_driver_exti.h"
#include "zf_driver_gpio.h"

#define MOTOR_ENCODER_LEFT_A_PIN A9
#define MOTOR_ENCODER_LEFT_B_PIN A12

volatile motor_encoder_data_t motor_speed_right = {.raw = 0};
volatile motor_encoder_data_t motor_speed_left = {.raw = 0};
/*
由于MSPM0G3507只有一个QEI，因此只能测量一个电机的编码器速度，另一个电机的速度需要通过GPIO中断模拟正交编码器来测量。
分配如下：
- QEI0：右电机编码器
- GPIO中断：左电机编码器
*/

//用中断模拟一个正交编码器(或者用输入捕获)
volatile int16_t left_encoder_count = 0;
//A相中断
void motor_encoder_exit_handler_A(uint32 state, void *ptr)
{
    // 读取A相和B相的状态
    uint8_t A_state = gpio_get_level(MOTOR_ENCODER_LEFT_A_PIN);
    uint8_t B_state = gpio_get_level(MOTOR_ENCODER_LEFT_B_PIN);
    // 根据A相和B相的状态更新计数
    if(A_state == B_state)
    {
        left_encoder_count++;
    }
    else
    {
        left_encoder_count--;
    }

}
//B相中断
void motor_encoder_exit_handler_B(uint32 state, void *ptr)
{
    // 读取A相和B相的状态
    uint8_t A_state = gpio_get_level(MOTOR_ENCODER_LEFT_A_PIN);
    uint8_t B_state = gpio_get_level(MOTOR_ENCODER_LEFT_B_PIN);
    // 根据A相和B相的状态更新计数
    if(A_state == B_state)
    {
        left_encoder_count--;
    }
    else
    {
        left_encoder_count++;
    }
}

void motor_encoder_init(void)
{
    //使用QEI
    DL_TimerG_startCounter(QEI_0_INST);
    //使用GPIO中断模拟正交编码器
    exti_init(MOTOR_ENCODER_LEFT_A_PIN, EXTI_TRIGGER_BOTH, motor_encoder_exit_handler_A, NULL);
    exti_init(MOTOR_ENCODER_LEFT_B_PIN, EXTI_TRIGGER_BOTH, motor_encoder_exit_handler_B, NULL);
    interrupt_set_priority(GPIOA_INT_IRQn, 0);
}

void motor_encoder_update(void)
{
    // 读取右电机编码器
    motor_speed_right.raw = (uint16_t)DL_TimerG_getTimerCount(QEI_0_INST);
    DL_TimerG_setTimerCount(QEI_0_INST, 0); // 读取后清零

    // 读取左电机编码器
    motor_speed_left.value = left_encoder_count;
    left_encoder_count = 0; // 读取后清零
}