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

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

struct Motor_ M0 = {0};
struct Motor_ M1 = {1};

struct _PID M0_VEL_PID = {0.4, 2, 0};
struct _PID M1_VEL_PID = {0.4, 2, 0};

struct LOWPASS M0_VEL_Filter = {0.1};
struct LOWPASS M1_VEL_Filter = {0.1};

struct AS5600_Sensor Angle_Sensor0 = {0};
struct AS5600_Sensor Angle_Sensor1 = {1};

#define VOLTAGE_POWER_SUPPLY 12.0f
#define VOLTAGE_LIMIT 6
#define PWM_COMPARE_VALUE 750.0f

volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0,
               dc_c = 0;

float M0_zero_elc_Angle = 0, M1_zero_elc_Angle = 0;

extern int M0_PP, M0_DIR;
extern int M1_PP, M1_DIR;

void Motor_en() {}

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

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

float M0_electricAngle(void) {
  return normalizeAngle((GetAngle_NoTrack(&Angle_Sensor0) * M0_PP * M0_DIR) -
                        M0_zero_elc_Angle);
}

float M1_electricAngle(void) {
  return normalizeAngle((GetAngle_NoTrack(&Angle_Sensor1) * M1_PP * M1_DIR) -
                        M1_zero_elc_Angle);
}
void SetPwm(int Mot_num, float Ua, float Ub, float Uc) {
  volatile float U_a = 0.0;
  volatile float U_b = 0.0;
  volatile 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);

  if (Mot_num == 0) {
    M0.dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
    M0.dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
    M0.dc_c = constrain(U_c / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);

    M0_PWM_A(M0.dc_a * PWM_COMPARE_VALUE); // 频率15k
    M0_PWM_B(M0.dc_b * PWM_COMPARE_VALUE);
    M0_PWM_C(M0.dc_c * PWM_COMPARE_VALUE);
  } else if (Mot_num == 1) {
    M1.dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
    M1.dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
    M1.dc_c = constrain(U_c / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);

    M1_PWM_A(M1.dc_a * PWM_COMPARE_VALUE); // 频率15k
    M1_PWM_B(M1.dc_b * PWM_COMPARE_VALUE);
    M1_PWM_C(M1.dc_c * PWM_COMPARE_VALUE);
  }
}

// FOC核心算法，克拉克逆变换/帕克逆变换

void SetPhaseVoltage(struct Motor_ *Motor, float Uq, float angle_el) {
  //	angle_el = normalizeAngle(angle_el);

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

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

  SetPwm(Motor->Mot_num, Motor->Ua, Motor->Ub, Motor->Uc);
}

void Check_Sensor(void) {
  SetPhaseVoltage(&M0, 3, _3PI_2);
  delay_ms(2000);
  M0_zero_elc_Angle = M0_electricAngle();
  SetPhaseVoltage(&M0, 0, _3PI_2);
  delay_ms(500);

  SetPhaseVoltage(&M1, 3, _3PI_2);
  delay_ms(2000);
  M1_zero_elc_Angle = M1_electricAngle();
  SetPhaseVoltage(&M1, 0, _3PI_2);
  delay_ms(500);
}

void FOC_Init() {
  // PWM_Init();
  // CurrSense_Init();
  // AS5600_Init();
  Check_Sensor();
}

void Print_Velocity(int Motor_Velocity) {
  if (Motor_Velocity == 0) {
    printf("Velocity M0 is %f \n", GetVelocity(&Angle_Sensor0));
  }
  if (Motor_Velocity == 1) {
    printf("Velocity M1 is %f \n", GetVelocity(&Angle_Sensor1));
  }
}

// 单速度环
void M0_Set_Velocity(float Target) {
  Angle_Sensor0.velocity =
      Lowpassfilter(&M0_VEL_Filter, GetVelocity(&Angle_Sensor0));
  SetPhaseVoltage(
      &M0,
      PID_Controller(&M0_VEL_PID, M0_DIR * (Target - Angle_Sensor0.velocity)),
      M0_electricAngle());
}

void M1_Set_Velocity(float Target) {
  Angle_Sensor1.velocity =
      Lowpassfilter(&M1_VEL_Filter, GetVelocity(&Angle_Sensor1));
  SetPhaseVoltage(
      &M1,
      PID_Controller(&M1_VEL_PID, M1_DIR * (Target - Angle_Sensor1.velocity)),
      M1_electricAngle());
}