//
// Created by hu123456 on 2024/1/16.
//
#include "foc.h"
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))


float	Voltage_PowerSupply =	12;
float 	Pole_Pairs = 14;
float	Voltage_Limit	= 6.9;
float	Velocity_Limit = 100;
float   V_Acc_Limit   = 100;
float   dir = 1;

FOCTypeDef FOCStruct_X={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0};

void PID_Iint(void)
{
    FOCStruct_X.ZT_KP = 2;
    FOCStruct_X.ZT_KI = 0;
    FOCStruct_X.ZT_KD = 0.2;

    FOCStruct_X.V_KP = 0.1;
    FOCStruct_X.V_KI = 1.5;
    FOCStruct_X.V_KD = 0;
//2024/1/2  Y轴效果不错
    FOCStruct_Y.ZT_KP = 2;
    FOCStruct_Y.ZT_KI = 0;
    FOCStruct_Y.ZT_KD = 0.5;

    FOCStruct_Y.V_KP = 0.1;
    FOCStruct_Y.V_KI = 1;
    FOCStruct_Y.V_KD = 0;


////2024/1/23  xY轴效果不错
//    FOCStruct_X.ZT_KP = 2;
//    FOCStruct_X.ZT_KI = 0;
//    FOCStruct_X.ZT_KD = 0.2;
//
//    FOCStruct_X.V_KP = 0.1;
//    FOCStruct_X.V_KI = 1;
//    FOCStruct_X.V_KD = 0;
////2024/1/22  Y轴效果不错
//    FOCStruct_Y.ZT_KP = 2;
//    FOCStruct_Y.ZT_KI = 0;
//    FOCStruct_Y.ZT_KD = 0.5;
//
//    FOCStruct_Y.V_KP = 0.1;
//    FOCStruct_Y.V_KI = 1;
//    FOCStruct_Y.V_KD = 0;

}
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
{
    float Uout;
    unsigned int sector;
    float T0,T1,T2;
    float Ta,Tb,Tc;

    if(Ud) // only if Ud and Uq set
    {
        Uout = sqrt(Ud*Ud + Uq*Uq) / Voltage_PowerSupply;
        // angle normalisation in between 0 and 2pi
        // only necessary if using _sin and _cos - approximation functions
        angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
    }
    else
    {// only Uq available - no need for atan2 and sqrt
        Uout = Uq / Voltage_PowerSupply;
        // angle normalisation in between 0 and 2pi
        // only necessary if using _sin and _cos - approximation functions
        angle_el = _normalizeAngle(angle_el + _PI_2);
    }
    if(Uout> 0.577)Uout= 0.577;
    if(Uout<-0.577)Uout=-0.577;

    sector = (angle_el / _PI_3) + 1;
    T1 = sqrt(3)*sin(sector*_PI_3 - angle_el) * Uout;
    T2 = sqrt(3)*sin(angle_el - (sector-1.0)*_PI_3) * Uout;
    T0 = 1 - T1 - T2;

    // calculate the duty cycles(times)
    switch(sector)
    {
        case 1:
            Ta = T1 + T2 + T0/2;
            Tb = T2 + T0/2;
            Tc = T0/2;
            break;
        case 2:
            Ta = T1 +  T0/2;
            Tb = T1 + T2 + T0/2;
            Tc = T0/2;
            break;
        case 3:
            Ta = T0/2;
            Tb = T1 + T2 + T0/2;
            Tc = T2 + T0/2;
            break;
        case 4:
            Ta = T0/2;
            Tb = T1+ T0/2;
            Tc = T1 + T2 + T0/2;
            break;
        case 5:
            Ta = T2 + T0/2;
            Tb = T0/2;
            Tc = T1 + T2 + T0/2;
            break;
        case 6:
            Ta = T1 + T2 + T0/2;
            Tb = T0/2;
            Tc = T1 + T0/2;
            break;
        default:  // possible error state
            Ta = 0;
            Tb = 0;
            Tc = 0;
    }
    if (motor == motory)
    {
                __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800);
                __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*4800);
                __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*4800);

    }
    else if (motor == motorx)
    {
                __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800);
                __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800);
                __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,Tc*4800);
    }
}

void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float  V_Target)
{
    float Uq,Ud;
    double electrical_angle;
    unsigned long Time_Now;
    double Ts;

    Time_Now =  __HAL_TIM_GET_COUNTER(&htim5);            //100ns
    if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
    else
        Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
    FOCStruct->Time_Prev = Time_Now;
    if(Ts == 0 || Ts > 400) Ts = 1e-3f;

    //获取角度求出V_Now
    double Angle_Now=getAngle(motor);

    double V_Now = (Angle_Now - FOCStruct->Angle_Prev)/Ts;
    FOCStruct->Angle_Prev = Angle_Now;
    V_Now = dir*LPF_velocity(V_Now,FOCStruct);
    double V_Error = V_Target - V_Now;

    float V_Pid_P = FOCStruct->V_KP * V_Error;
    float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
    V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);

    float  V_Out = V_Pid_P+V_Pid_I;
    V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);

    float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;
    if(V_Acc_Now > V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev + V_Acc_Limit*Ts;
    else if(V_Acc_Now < -V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev - V_Acc_Limit*Ts;

    FOCStruct->V_Pid_IPrev  = V_Pid_I;
    FOCStruct->V_Out_Prev   = V_Out;
    FOCStruct->V_Error_Prev = V_Error;

    Uq = V_Out;
    Ud = 0;


//    double  shaft_angle = dir*Angle_Now;
    double  shaft_angle = dir*Angle_Now;

//    electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs);
    electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
    setPhaseVoltage(motor,Uq,Ud,electrical_angle);
}

void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float  ZT_Error)
{
    float Uq,Ud;
    double electrical_angle;
    unsigned long Time_Now;
    double Ts;

    Time_Now =  __HAL_TIM_GET_COUNTER(&htim5);            //100ns
    if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
    else
        Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
    FOCStruct->Time_Prev = Time_Now;
    if(Ts == 0 || Ts > 400) Ts = 1e-3f;

    //姿态环
    float V_Target  = FOCStruct->ZT_KP * ZT_Error + FOCStruct->ZT_KD * (ZT_Error - FOCStruct->ZT_ErrorPrev) / Ts;
    V_Target = _constrain(V_Target, -Velocity_Limit, Velocity_Limit);
    FOCStruct->ZT_ErrorPrev = ZT_Error;

    //速度环
    //获取角度求出V_Now
    double Angle_Now=getAngle(motor);

    double V_Now = (Angle_Now - FOCStruct->Angle_Prev)/Ts;
    FOCStruct->Angle_Prev = Angle_Now;
    V_Now = dir*LPF_velocity(V_Now,FOCStruct);
    double V_Error = V_Target - V_Now;

    float V_Pid_P = FOCStruct->V_KP * V_Error;
    float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
    V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);

    float  V_Out = V_Pid_P+V_Pid_I;
    V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);

    float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;
    if(V_Acc_Now > V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev + V_Acc_Limit*Ts;
    else if(V_Acc_Now < -V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev - V_Acc_Limit*Ts;

    FOCStruct->V_Pid_IPrev  = V_Pid_I;
    FOCStruct->V_Out_Prev   = V_Out;
    FOCStruct->V_Error_Prev = V_Error;

    Uq = V_Out;
    Ud = 0;


//    double  shaft_angle = dir*Angle_Now;
    double  shaft_angle = dir*Angle_Now;

//    electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs);
    electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
    setPhaseVoltage(motor,Uq,Ud,electrical_angle);
}

void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target)
{
    float Uq,Ud;
    double electrical_angle;
    unsigned long Time_Now;
    double Ts;

    Time_Now =  __HAL_TIM_GET_COUNTER(&htim5);            //100ns
    if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
    else
        Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
    FOCStruct->Time_Prev = Time_Now;
    if(Ts == 0 || Ts > 400) Ts = 1e-3f;

    //角度环
    double Angle_Now=getAngle(motor);

    float error = Angle_target - Angle_Now;
    Uq = 20 * error;
    Uq = _constrain(Uq, -Voltage_Limit, Voltage_Limit);

    double  shaft_angle = dir*Angle_Now;
    electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
    setPhaseVoltage(motor,Uq,Ud,electrical_angle);
}

double LPF_velocity(double x,FOCTypeDef *FOCStruct)
{
    float y = 0.9*(FOCStruct->V_Prev)+ 0.1*x;
    FOCStruct->V_Prev=y;
    return y;
}

// normalizing radian angle to [0,2PI]
double _normalizeAngle(float angle)
{
    float a = fmod(angle, _2PI);
    return a >= 0 ? a : (a + _2PI);
}


void PWM_Start()
{
    HAL_TIM_Base_Start(&htim2); //启动定时器
    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);


    HAL_TIM_Base_Start(&htim3); //启动定时器
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
}

void moto_Init()
{

    GPIO_InitTypeDef GPIO_InitStruct = {0};
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();

    GPIO_InitStruct.Pin = GPIO_PIN_0;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);


    GPIO_InitStruct.Pin = GPIO_PIN_9;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,1);
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,1);

    PWM_Start();

    HAL_Delay(1000);
    alignSensor(motory,&FOCStruct_Y);
    alignSensor(motorx,&FOCStruct_X);
    HAL_Delay(50);
}

int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct)
{
    long i;
    float angle;
    float mid_angle = 0,end_angle = 0;
    float moved;
    float q = 3.5;
    printf("MOT: Align sensor.\r\n");

    // find natural direction
    // move one electrical revolution forward
    HAL_Delay(100);
    for(i=0; i<=500; i++)
    {
        angle = _3PI_2 + _2PI * i / 500.0;
        setPhaseVoltage(moto,q, 0,  angle);
        HAL_Delay(2);
    }
    mid_angle=getAngle(moto);
    for(i=500; i>=0; i--)
    {
        angle = _3PI_2 + _2PI * i / 500.0 ;
        setPhaseVoltage(moto,q, 0,  angle);
        HAL_Delay(2);
    }
    end_angle=getAngle(moto);
    setPhaseVoltage(moto,0, 0, 0);
    HAL_Delay(200);

    printf("mid_angle=%.4f\r\n",mid_angle);
    printf("end_angle=%.4f\r\n",end_angle);

    moved =  fabs(mid_angle - end_angle);
    if((mid_angle == end_angle)||(moved < 0.02))  //相等或者几乎没有动
    {
        printf("MOT: Failed to notice movement loop222.\r\n");
//        M1_Disable;    //电机检测不正常，关闭驱动
        return 0;
    }
    else if(mid_angle < end_angle)
    {
        printf("MOT: sensor_direction==CCW\r\n");
//        dir = -1;
    }
    else
    {
        printf("MOT: sensor_direction==CW\r\n");
//        dir = 1;
    }
    printf("MOT: PP check: \n");    //计算Pole_Pairs
    int pp;
    if( fabs(moved*Pole_Pairs - _2PI) > 0.5 )  // 0.5 is arbitrary number it can be lower or higher!
    {
        printf("fail - estimated pp\n");
        pp = _2PI/moved+0.5;     //浮点数转整形，四舍五入
        printf("pp=%d\r\n",pp);
    }
    else
        printf("OK!\r\n");
    setPhaseVoltage(moto,q,0,_3PI_2);  //计算零点偏移角度
    HAL_Delay(700);
    FOCStruct->zero_electric_angle = _normalizeAngle(dir*getAngle(moto) * Pole_Pairs);
//    delay_ms(20);
//    printf("MOT: Zero elec. angle:");
//    printf("%.4f\r\n",zero_electric_angle);

    setPhaseVoltage(moto,0, 0, 0);
    HAL_Delay(50);

    return 1;
}