#include <stdlib.h>
#include <stdio.h>
#include "pwm.h"
#include "mt6701.h"
#include "delay.h"
#include "lowpass_filter.h"
#include "pid_control.h"
#include <math.h>
#include <ti_msp_dl_config.h>
#include "config.h"

#define PI         3.14159265359f
#define _3PI_2     4.71238898f
#define _1_SQRT3 	 0.57735026919f
#define _2_SQRT3   1.15470053838f

float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0;
float voltage_limit = 8;
float voltage_power_supply = 0;
float zero_electric_Angle = 0.0;

extern int pp;
extern int Dir;



void Motor_en()
{
    //	GPIO_InitTypeDef GPIO_InitStructure;
    //
    //	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
    //
    //	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    //	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
    //	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    //	GPIO_Init(GPIOA,&GPIO_InitStructure);
    //
    //	GPIO_SetBits(GPIOA, GPIO_Pin_8);
}

//限制幅值
float constrain(float amt, float low, float high)
{
    return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

//将角度归化到0-2PI
float normalizeAngle(float angle)
{
    float a = fmod(angle, 2 * PI);
    return ((a >= 0) ? a : (a + 2 * PI));
}

float electricAngle(void)
{
    return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
}

void SetPwm(float Ua, float Ub, float Uc)
{
    float U_a = 0.0;
    float U_b = 0.0;
    float U_c = 0.0;

    U_a = constrain(Ua, 0.0f, voltage_limit);
    U_b = constrain(Ub, 0.0f, voltage_limit);
    U_c = constrain(Uc, 0.0f, voltage_limit);

    dc_a = constrain(U_a / voltage_power_supply, 0.0f, 1.0f);
    dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f);
    dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f);

    PWM_Channel1(dc_a * 1000.0f);  // 频率15k
    PWM_Channel2(dc_b * 1000.0f);
    PWM_Channel3(dc_c * 1000.0f);
    if(DEBUG_ENABLED & DEBUG_DFOC)
    {
        printf("dc_a %f \n", dc_a);
        printf("dc_b %f \n", dc_b);
        printf("dc_c %f \n", dc_c);
    }


}

//FOC核心算法，克拉克逆变换/帕克逆变换
float test_angle = 0.0;
float last_test_angle = 0.0;
void SetPhaseVoltage(float Uq, float Ud, float angle_el)
{
    //	angle_el = normalizeAngle(angle_el);
    test_angle = angle_el - last_test_angle;

    Ualpha = -Uq * sin(angle_el);
    Ubeta = Uq * cos(angle_el);

    Ua = Ualpha + voltage_power_supply / 2;
    Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
    Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;

    SetPwm(Ua, Ub, Uc);

    last_test_angle = angle_el;
}

void Check_Sensor(void)
{
    SetPhaseVoltage(3, 0, _3PI_2);
    delay_ms(3000);
    zero_electric_Angle = electricAngle();
    SetPhaseVoltage(0, 0, _3PI_2);
    delay_ms(500);
}

void FOC_Init(float power)
{
    voltage_power_supply = power;
    //PWM_Init();
    //CurrSense_Init();
    //AS5600_Init();

    Check_Sensor();
}


// 单角度环
void Set_Angle(float Target)
{
    float angle = GetAngle();
    float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * angle) * 180 / PI);
    SetPhaseVoltage(Uq, 0, electricAngle());
}

double shaft_angle;
double openloop_timestamp;
float velocityopenloop(float target)
{
    float Uq = 0.0;
    double Ts = 0.0;

    uint32_t now_ts  =   DL_SYSTICK_getValue();


    if(now_ts < openloop_timestamp)
    {
        Ts = (openloop_timestamp - now_ts) / 32.0f * 1e-6f;
    }
    else
    {
        Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 32.0f * 1e-6f;
    }

    if(Ts < 0 || Ts >= 0.005)
    {
        Ts = 0.001f;
    }

    shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts);

    Uq = voltage_limit;

    SetPhaseVoltage(Uq, 0, shaft_angle);

    openloop_timestamp = now_ts;

    return Uq;
}