#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

#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;

volatile float zero_electric_Angle = 0.0;

extern int pp;
extern int 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 electricAngle(void) {
  return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
}

void SetPwm(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);

  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);
  if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) {

    printf("dc_a : %f  -- dc_b : %f -- dc_c : %f  \n", dc_a, dc_b, dc_c);
  }
  PWM_Channel1((1 - dc_a) * PWM_COMPARE_VALUE); // 频率15k
  PWM_Channel2((1 - dc_b) * PWM_COMPARE_VALUE);
  PWM_Channel3((1 - dc_c) * PWM_COMPARE_VALUE);
}

// FOC核心算法，克拉克逆变换/帕克逆变换
volatile float test_angle = 0.0;
volatile 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();
  printf("Check_Sensor zero_electric_Angle is %f \n", zero_electric_Angle);
  // SetPhaseVoltage(0, 0, _3PI_2);
  // delay_ms(500);
}

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

// 单角度环
void Set_Angle(float Target) {
  volatile float langle = GetAngle();
  if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) {
    printf("angle readback in dfoc.c is %f \n", langle);
  }
  volatile float Uq =
      PID_Controller(0.03, 0.01, 0, (Target - Dir * langle) * 180 / PI);
  // volatile float Uq = PID_Controller(0.133, 0, 0, (Target - Dir * langle) *
  // 180 / PI);
  if (DEBUG_ENABLED & DEBUG_DFOC_ENABLED) {
    printf("Uq is %f \n", Uq);
  }
  SetPhaseVoltage(Uq, 0, electricAngle());
}

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

  volatile uint32_t now_ts = DL_SYSTICK_getValue();

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

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

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

  if (DEBUG_ENABLED) {
    printf("shaft_angle : %f -- Ts : %f \n", shaft_angle, Ts);
  }

  Uq = VOLTAGE_LIMIT;

  SetPhaseVoltage(Uq, 0, shaft_angle);
  openloop_timestamp = now_ts;

  return Uq;
}