//
// Created by ZK on 2023/3/10.
//

#include "APP_Main.h"
#include "APP_Task.h"
#include "Controller/Controller.h"
#include "PreDrive.h"
#include "controller.h"
#include "encoder.h"
#include "foc.h"
#include "pwm_curr.h"
#include "usr_config.h"

volatile uint32_t SystickCount = 0;
//uint8_t RxBuffer[2];//接收数据
//float32_t NowAngle;
uint8_t USART1_IRQHandler_Status;

void APP_Init() {
    USR_CONFIG_set_default_config();
    USR_CONFIG_read_config();

    __HAL_SPI_ENABLE(&hspi1);
    HAL_Delay(10);
    //    Data.Angle.EncoderModel = MT6816;
    //    Data_Init(&Data);
    //    NowAngle = Data.Angle.getAngle();


    //�???启内部运�???
    HAL_OPAMP_Start(&hopamp1);
    HAL_OPAMP_Start(&hopamp2);
    HAL_OPAMP_Start(&hopamp3);
    HAL_Delay(10);

    //    while (InteriorADC_Init())
    HAL_ADC_Start_DMA(&hadc1, (uint32_t *) adc1_RegularBuf, 2);

    //    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    //    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    //    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    //    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
    //    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    //    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
    //    HAL_Delay(10);
    //    __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
    //    HAL_Delay(10);


    //    PreDrive_Init();
    //    GPIOE->BSRR = 1 << 7;
    //    HAL_Delay(10);

    TIM1->CCR1 = 0;
    TIM1->CCR2 = 0;
    TIM1->CCR3 = 0;
    HAL_Delay(10);

    MCT_init();
    FOC_init();
    PWMC_init();
    ENCODER_init();
    CONTROLLER_init();

    __HAL_ADC_DISABLE_IT(&hadc1, ADC_IT_JEOS);//关闭ADC1的中断，避免ADC1_2_IRQHandler触发两次
    __HAL_ADC_DISABLE_IT(&hadc2, ADC_IT_JEOS);//关闭ADC1的中断，避免ADC1_2_IRQHandler触发两次

    for (uint8_t i = 0, j = 0; i < 250; i++) {
        if (Foc.v_bus_filt > 20) {
            if (++j > 20) {
                break;
            }
        }
        HAL_Delay(2);
    }

    while (PWMC_CurrentReadingPolarization()) {
        StatuswordNew.errors.adc_selftest_fatal = 1;
    }


    __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
    __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOS);//关闭ADC1的中断，避免ADC1_2_IRQHandler触发两次

    //    MCT_set_state(IDLE);
}

uint16_t ADC_VAL1, ADC_VAL2;

//MCU内部温度传感器
#define TS_CAL1 ((uint16_t *) ((uint32_t) 0x1FFF75A8))// 30摄氏度时的MCU内部温度传感器校准值
#define TS_CAL2 ((uint16_t *) ((uint32_t) 0x1FFF75CA))//110摄氏度时的MCU内部温度传感器校准值

int ret;
void APP_Main() {
    //    MCT_set_state(CALIBRATION);
    printf("%f,%f,%f\n", Foc.i_a, Foc.i_b, Foc.i_c);

    if (USART1_IRQHandler_Status) {
        switch (RxBuffer[0]) {
            case 10:
                switch (RxBuffer[1]) {
                    case 10:
                        ret = MCT_set_state(IDLE);
                        printf("IDLE ret:%d\r\n", ret);
                        break;
                }
                break;
            case 00:
                switch (RxBuffer[1]) {
                    case 00:
                        UsrConfig.control_mode = CONTROL_MODE_TORQUE_RAMP;
                        printf("UsrConfig.control_mode->CONTROL_MODE_TORQUE_RAMP\r\n");
                        break;
                    case 01:
                        UsrConfig.control_mode = CONTROL_MODE_VELOCITY_RAMP;
                        printf("UsrConfig.control_mode->CONTROL_MODE_VELOCITY_RAMP\r\n");
                        break;
                    case 02:
                        UsrConfig.control_mode = CONTROL_MODE_POSITION_FILTER;
                        printf("UsrConfig.control_mode->CONTROL_MODE_POSITION_FILTER\r\n");
                        break;
                    case 03:
                        UsrConfig.control_mode = CONTROL_MODE_POSITION_PROFILE;
                        printf("UsrConfig.control_mode->CONTROL_MODE_POSITION_PROFILE\r\n");
                        break;
                    case 04:
                        ret = MCT_set_state(IDLE);
                        printf("IDLE ret:%d\r\n", ret);
                        break;
                    case 05:
                        ret = MCT_set_state(CALIBRATION);
                        printf("CALIBRATION ret:%d\r\n", ret);
                        break;
                    case 06:
                        ret = MCT_set_state(RUN);
                        printf("RUN ret:%d\r\n", ret);
                        break;
                }
                Controller.input_position = (float) ((float) RxBuffer[1] / 10.0f);
                Controller.input_velocity = (float) ((float) RxBuffer[1] / 10.0f);
                printf("Controller.input_position->%f\r\n", Controller.input_position);
                printf("Controller.input_position->%f\r\n", Controller.input_position);
                break;
            case 01:
                Controller.input_position = (float) ((float) RxBuffer[1] / 10.0f);
                Controller.input_velocity = (float) ((float) RxBuffer[1] / 10.0f);
                printf("Controller.input_position->%f\r\n", Controller.input_position);
                printf("Controller.input_position->%f\r\n", Controller.input_position);
                break;
            case 02:
                UsrConfig.pos_gain = (float) ((float) RxBuffer[1]);
                printf("UsrConfig.pos_gain->%f\r\n", UsrConfig.pos_gain);
                break;
            case 03:
                UsrConfig.vel_gain = (float) ((float) RxBuffer[1] / 100.0f);
                printf("UsrConfig.vel_gain->%f\r\n", UsrConfig.vel_gain);
                break;
            case 04:
                UsrConfig.vel_integrator_gain = (float) ((float) RxBuffer[1] / 1000.0f);
                printf("UsrConfig.vel_integrator_gain->%f\r\n", UsrConfig.vel_integrator_gain);
                break;
        }
        USART1_IRQHandler_Status = 0;
    }
}