#include "../2025_include_user/Encoder.h"

#define RADIUS_OF_TYRE        30       // 轮胎半径，单位：mm
#define LINE_SPEED_C  (2.0f * 3.1415926f * RADIUS_OF_TYRE)  // 一圈对应的线长，mm
#define SPEED_RATIO 50

Encoder::Speed Encoder::ticks = {0, 0};
Encoder::Speed Encoder::direction = {0, 0};
Encoder::Speed Encoder::speed = {0, 0};
Encoder::Speed Encoder::real_speed = {0, 0};

extern "C" {
    void Encoder1_TIM_INST_IRQHandler(void)
    {
        switch (DL_TimerG_getPendingInterrupt(Encoder1_TIM_INST))
        {
            case DL_TIMERG_IIDX_LOAD:
                Encoder::ticks.left++;
                Encoder::direction.left = DL_GPIO_readPins(Encoder1_GPIO_PORT, Encoder1_GPIO_Encoder1B_GPIO_PIN) ? -1 : 1;
                break;
            default:
                break;
        }
    }

    void Encoder2_TIM_INST_IRQHandler(void)
    {
        switch (DL_TimerA_getPendingInterrupt(Encoder2_TIM_INST))
        {
            case DL_TIMERG_IIDX_LOAD:
                Encoder::ticks.right++;
                Encoder::direction.right = DL_GPIO_readPins(Encoder2_GPIO_PORT, Encoder2_GPIO_Encoder2B_GPIO_PIN) ? -1 : 1;
                break;
            default:
                break;
        }
    }

    void Encoder_Count_INST_IRQHandler(void) {
        Encoder::inject();
    }
}

void Encoder::init() {
    NVIC_EnableIRQ(Encoder1_TIM_INST_INT_IRQN);
    DL_TimerG_startCounter(Encoder1_TIM_INST);

    NVIC_EnableIRQ(Encoder2_TIM_INST_INT_IRQN);
    DL_TimerA_startCounter(Encoder2_TIM_INST);

    NVIC_EnableIRQ(Encoder_Count_INST_INT_IRQN);
    DL_TimerG_startCounter(Encoder_Count_INST);
}

void Encoder::inject() {
    speed = {ticks.left * SPEED_RATIO, ticks.right * SPEED_RATIO};
    real_speed = {ticks.left * SPEED_RATIO * direction.left, ticks.right * SPEED_RATIO * direction.right};
    ticks = {0, 0};
}

Encoder::Speed Encoder::getSpeeds() {
    return speed;
}

Encoder::Speed Encoder::getRealSpeeds() {
    return real_speed;
}

int Encoder::getRealLeftSpeed() {
    return real_speed.left;
}

int Encoder::getRealRightSpeed() {
    return real_speed.right;
}

int Encoder::getLeftSpeed() {
    return speed.left;
}

int Encoder::getRightSpeed() {
    return speed.right;
}

int Encoder::getLeftDirection() {
    return direction.left;
}

int Encoder::getRightDirection() {
    return direction.right;
}