#include "motor.h"
#include "globalv.h"

static void MotorPwmSetCompare(unsigned int ch1, unsigned int ch2, unsigned int ch3)
{
    ATIM_SetCompare1((uint16_t)ch1);
    ATIM_SetCompare2((uint16_t)ch2);
    ATIM_SetCompare3((uint16_t)ch3);
}

void PWMtimer_init(void)
{
    ATIM_InitTypeDef ATIM_InitStruct = {0};
    ATIM_OCInitTypeDef ATIM_OCInitStruct = {0};
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    __SYSCTRL_ATIM_CLK_ENABLE();
    __SYSCTRL_GPIOA_CLK_ENABLE();
    __SYSCTRL_GPIOB_CLK_ENABLE();

    PA08_AFx_ATIMCH1();
    PA09_AFx_ATIMCH2();
    PA10_AFx_ATIMCH3();

    GPIO_InitStruct.IT = GPIO_IT_NONE;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pins = GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;
    GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
    GPIO_Init(CW_GPIOB, &GPIO_InitStruct);

    PWM_AH_OFF;
    PWM_BH_OFF;
    PWM_CH_OFF;

    ATIM_InitStruct.BufferState = ENABLE;
    ATIM_InitStruct.CounterAlignedMode = ATIM_COUNT_ALIGN_MODE_EDGE;
    ATIM_InitStruct.CounterDirection = ATIM_COUNTING_UP;
    ATIM_InitStruct.CounterOPMode = ATIM_OP_MODE_REPETITIVE;
    ATIM_InitStruct.Prescaler = 0;
    ATIM_InitStruct.ReloadValue = PWM_PERIOD;
    ATIM_InitStruct.RepetitionCounter = 0;
    ATIM_Init(&ATIM_InitStruct);

    ATIM_OCInitStruct.BufferState = DISABLE;
    ATIM_OCInitStruct.OCComplement = DISABLE;
    ATIM_OCInitStruct.OCFastMode = ATIM_OC_FAST_MODE_DISABLE;
    ATIM_OCInitStruct.OCInterruptState = DISABLE;
    ATIM_OCInitStruct.OCMode = ATIM_OCMODE_PWM1;
    ATIM_OCInitStruct.OCPolarity = ATIM_OCPOLARITY_NONINVERT;
    ATIM_OC1Init(&ATIM_OCInitStruct);
    ATIM_OC2Init(&ATIM_OCInitStruct);
    ATIM_OC3Init(&ATIM_OCInitStruct);
    ATIM_OC4Init(&ATIM_OCInitStruct);

    MotorPwmSetCompare(0, 0, 0);
    ATIM_SetCompare4((uint16_t)(PWM_PERIOD / 2));

    ATIM_CH1Config(ENABLE);
    ATIM_CH2Config(ENABLE);
    ATIM_CH3Config(ENABLE);
    ATIM_CH4Config(ENABLE);

    ATIM_Cmd(ENABLE);
    ATIM_CtrlPWMOutputs(ENABLE);
}

void ATIM_IRQHandler(void)
{
    if (ATIM_GetITStatus(ATIM_IT_UIE) != RESET)
    {
        ATIM_ClearITPendingBit(ATIM_IT_UIE);
    }
}

void Commutation(unsigned int step, unsigned int PWM_ON_flag)
{
    if (PWM_ON_flag == 0 || MOTORSTATE == STATEERROR || MOTORSTATE == STATEERROROVER)
    {
        MotorPwmSetCompare(0, 0, 0);
        PWM_AL_OFF;
        PWM_BL_OFF;
        PWM_CL_OFF;
        return;
    }

    if (step == 0 || step == 5)
    {
        PWM_AL_OFF;
        PWM_CL_OFF;
    }
    else if (step == 1 || step == 2)
    {
        PWM_AL_OFF;
        PWM_BL_OFF;
    }
    else if (step == 3 || step == 4)
    {
        PWM_BL_OFF;
        PWM_CL_OFF;
    }

    if (step == 0 || step == 1)
    {
        MotorPwmSetCompare(OutPwmValue, 0, 0);
    }
    if (step == 2 || step == 3)
    {
        MotorPwmSetCompare(0, OutPwmValue, 0);
    }
    if (step == 4 || step == 5)
    {
        MotorPwmSetCompare(0, 0, OutPwmValue);
    }

    if (step == 0 || step == 5)
    {
        PWM_BL_ON;
    }
    else if (step == 1 || step == 2)
    {
        PWM_CL_ON;
    }
    else if (step == 3 || step == 4)
    {
        PWM_AL_ON;
    }

    STEP_last = step;
}

void UPPWM(void)
{
    if (STEP_last == 0 || STEP_last == 1)
    {
        MotorPwmSetCompare(OutPwmValue, 0, 0);
    }
    if (STEP_last == 2 || STEP_last == 3)
    {
        MotorPwmSetCompare(0, OutPwmValue, 0);
    }
    if (STEP_last == 4 || STEP_last == 5)
    {
        MotorPwmSetCompare(0, 0, OutPwmValue);
    }
}

void Halltimer_init(void)
{
    GTIM_InitTypeDef GTIM_InitStruct = {0};
    GTIM_ICModeCfgTypeDef GTIM_ICInitStruct = {0};
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    __SYSCTRL_GTIM2_CLK_ENABLE();
    __SYSCTRL_GPIOA_CLK_ENABLE();
    __SYSCTRL_GPIOB_CLK_ENABLE();

    GPIO_InitStruct.IT = GPIO_IT_NONE;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT_PULLUP;
    GPIO_InitStruct.Pins = GPIO_PIN_15 | GPIO_PIN_1;
    GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
    GPIO_Init(CW_GPIOA, &GPIO_InitStruct);

    GPIO_InitStruct.Pins = GPIO_PIN_10;
    GPIO_Init(CW_GPIOB, &GPIO_InitStruct);

    PA15_AFx_GTIM2CH1();
    PA01_AFx_GTIM2CH2();
    PB10_AFx_GTIM2CH3();

    __disable_irq();
    NVIC_EnableIRQ(GTIM2_IRQn);
    __enable_irq();

    GTIM_InitStruct.AlignMode = GTIM_ALIGN_MODE_EDGE;
    GTIM_InitStruct.ARRBuffState = GTIM_ARR_BUFF_DIS;
    GTIM_InitStruct.Direction = GTIM_DIRECTION_UP;
    GTIM_InitStruct.EventOption = GTIM_EVENT_NORMAL;
    GTIM_InitStruct.Prescaler = 0;
    GTIM_InitStruct.PulseMode = GTIM_PULSE_MODE_DIS;
    GTIM_InitStruct.ReloadValue = 0xFFFF;
    GTIM_InitStruct.UpdateOption = GTIM_UPDATE_EN;
    GTIM_TimeBaseInit(CW_GTIM2, &GTIM_InitStruct);

    GTIM_ICInitStruct.ICMap = GTIM_IC1_MAP_TI1;
    GTIM_ICInitStruct.ICPrescaler = GTIM_IC_PRESCALER_DIV1;
    GTIM_ICInitStruct.ICFilter = GTIM_IC_FILTER_PCLK_N2;
    GTIM_ICInitStruct.ICPolarity = GTIM_ICPolarity_BothEdge;
    GTIM_IC1ModeCfg(CW_GTIM2, &GTIM_ICInitStruct);
    GTIM_IC1Cmd(CW_GTIM2, ENABLE);

    GTIM_ICInitStruct.ICMap = GTIM_IC2_MAP_TI2;
    GTIM_IC2ModeCfg(CW_GTIM2, &GTIM_ICInitStruct);
    GTIM_IC2Cmd(CW_GTIM2, ENABLE);

    GTIM_ICInitStruct.ICMap = GTIM_IC3_MAP_TI3;
    GTIM_IC3ModeCfg(CW_GTIM2, &GTIM_ICInitStruct);
    GTIM_IC3Cmd(CW_GTIM2, ENABLE);

    GTIM_ITConfig(CW_GTIM2, GTIM_IT_CC1 | GTIM_IT_CC2 | GTIM_IT_CC3, ENABLE);
    GTIM_Cmd(CW_GTIM2, ENABLE);
}

unsigned char HALL_Check(void)
{
    static unsigned char hallerrnum = 0;
    unsigned char x = 0;

    if (GPIO_ReadPin(CW_GPIOA, GPIO_PIN_15) != 0)
    {
        x = 1;
    }
    if (GPIO_ReadPin(CW_GPIOA, GPIO_PIN_1) != 0)
    {
        x |= 0x2;
    }
    if (GPIO_ReadPin(CW_GPIOB, GPIO_PIN_10) != 0)
    {
        x |= 0x4;
    }

    if (x == 0 || x == 7)
    {
        hallerrnum++;
        if (hallerrnum >= 10)
        {
            hallerrnum = 10;
            ErrorCode = 2;
        }
    }
    else
    {
        hallerrnum = 0;
    }

    return x;
}

void GTIM2_IRQHandler(void)
{
    unsigned char x = 0;

    if (GTIM_GetITStatus(CW_GTIM2, GTIM_IT_CC1) != RESET)
    {
        GTIM_ClearITPendingBit(CW_GTIM2, GTIM_IT_CC1);
    }
    if (GTIM_GetITStatus(CW_GTIM2, GTIM_IT_CC2) != RESET)
    {
        GTIM_ClearITPendingBit(CW_GTIM2, GTIM_IT_CC2);
    }
    if (GTIM_GetITStatus(CW_GTIM2, GTIM_IT_CC3) != RESET)
    {
        GTIM_ClearITPendingBit(CW_GTIM2, GTIM_IT_CC3);
    }

    x = HALL_Check();
    if (x == 0 || x == 7)
    {
        return;
    }

    HALLcount++;
    HALLcount1++;
    Cur_step = STEP_TAB[Dir][x - 1];
    if (Motor_Start_F == 1 && ErrorCode == 0)
    {
        Commutation(Cur_step, Motor_Start_F);
    }
}

void HALL_MOTOR_START(void)
{
    unsigned char x = 0;

    x = HALL_Check();
    if (x == 0 || x == 7)
    {
        return;
    }
    Cur_step = STEP_TAB[Dir][x - 1];
    RealS1 = 0;
    Motor_Start_F = 1;
    Commutation(Cur_step, Motor_Start_F);
}

void MOTOR_STOP0(void)
{
    Motor_Start_F = 0;
    Commutation(0, 0);
}
