#pragma once
#include "stm32f4xx_hal.h"
#include <math.h>
#include <stdint.h>

#define PI 3.14159265358979323846f

typedef void (*set_pwm_handler)(TIM_HandleTypeDef *htim, uint32_t phaseA, uint32_t phaseB, uint32_t phaseC);
typedef uint16_t (*get_raw_angle)(SPI_HandleTypeDef *hspi, uint8_t *status);

typedef struct __FOC_MOTOR_CONTROL
{
  float polar_pair;
  float machine_angle;
  float electron_angle;
  float Encoder_ZeroPoint;
  float Udc;

  //    output
  float current_loop_Ud;
  float current_loop_Uq;

  float inv_park_U_alpha;
  float inv_park_U_beta;

  uint16_t svpwm_Ua;
  uint16_t svpwm_Ub;
  uint16_t svpwm_Uc;
  set_pwm_handler set_pwm;
  get_raw_angle raw_angle;
#if 0
//    input
    float Phase_Ia;
    float Phase_Ib;
    float Phase_Ic;

    float clark_I_alpha;
    float clark_I_beta;

    float park_Id;
    float park_Iq;
#endif

} foc_motor_t;

void set_torque(foc_motor_t *__obj, float Uq);

void foc_init(foc_motor_t *__obj, float polar_pair, float Encoder_ZP, float Udc, set_pwm_handler set_pwm, get_raw_angle raw_angle);

void foc_run(foc_motor_t *__obj, TIM_HandleTypeDef *htim, SPI_HandleTypeDef *hspi);

extern foc_motor_t Motor_6208;