//
// Created by ZK on 24-5-10.
//

#include "APP_Task.h"
#include "APP_Main.h"
#include "Angle.h"
#include "InteriorADC.h"
#include "Controller/Controller.h"
#include "Controller/SVPWM/SVPWM.h"

#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample)))

float CurrA, CurrB, CurrC;
float Current_Temp, V_Temp;
//float32_t NowAngle;

float Current_Temp1, ii = 0.0f, Test_Angle = 0.0f;

#define PWM_FREQUENCY          24000
#define CURRENT_MEASURE_HZ     PWM_FREQUENCY
#define CURRENT_MEASURE_PERIOD (float) (1.0f / (float) CURRENT_MEASURE_HZ)

#define TIMER0_CLK_MHz         168
#define PWM_PERIOD_CYCLES      (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE)
#define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U)

int up = 1;
float32_t A, Speedtarget;
int32_t number;
int32_t mode = 0;

void High_Frequency_Task() {

    if (mode < 10000) {
//        HAL_Delay(1000);
        mode++;
    }
    if (mode >= 10000) {


        number++;
        if (number >= 1 && number <= 10000) {
            Speedtarget = 1.0;
        } else if (number >= 10000 && number <= 20000) {
            Speedtarget = -1.0;
        } else if (number >= 20001) {
            number = 0;
        }
//    Test_Angle += 0.01f;
//    if (Test_Angle > 360.0f) {
//        Test_Angle = 0.0f;
//    }


        //Data.Angle.getAngle()
//    SendCurrent_Vofa(ADC1->JDR1, ADC2->JDR1, ADC1->JDR2);

//    float test = 0;
//    UTILS_LP_FAST(test, 12, 0.05f);
//    usb_printf("%f\r\n", test);


//    Generate_SVM(0.0f, 0.05f, Data.Angle.getAngle());
//    float32_t Eangle = Data.Angle.getAngle();
        SpeedControl(Speedtarget, MT_ReadAngle(),10.0f);
        FOC_current(0, 10.0f, Data.Angle.getAngle(), 5000);

//    SendCurrent_Vofa(Test_Angle,A, (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES));


//    SendCurrent_Vofa((ADC1->JDR1)/4096.0f*3.3f/0.005f/7.33333f, 0, 0);
    }

}