/*
 * Copyright 2020-2022 Yuntu Microelectronics co.,ltd
 * All rights reserved.
 *
 * YUNTU Confidential. This software is owned or controlled by YUNTU and may
 * only be used strictly in accordance with the applicable license terms. By
 * expressly accepting such terms or by downloading, installing, activating
 * and/or otherwise using the software, you are agreeing that you have read, and
 * that you agree to comply with and are bound by, such license terms. If you do
 * not agree to be bound by the applicable license terms, then you may not
 * retain, install, activate or otherwise use the software. The production use
 * license in Section 2.3 is expressly granted for this software.
 */

/*!
 * @file motor_foc_lib.h
 * @brief Field Oriented Control (FOC) library for motor control
 *
 * This library provides functions and data structures for implementing Field Oriented Control
 * for brushless motors. It includes functionality for:
 * - SVPWM (Space Vector PWM) generation
 * - Current measurement and reconstruction
 * - PI control loops
 * - Motor state estimation
 * - Flux weakening control
 */

#ifndef MOTOR_FOC_LIB_H
#define MOTOR_FOC_LIB_H

#include <stdint.h>

#ifndef __STATIC_INLINE
#define __STATIC_INLINE static inline
#endif

/*! @brief Sin cos calculation based on look up table */
extern const int16_t Q15Sin[]; /* SIN LUT table */

/*!
 * @ingroup motor_library
 * @{
 */

/*******************************************************************************
 * Enumerations.
 ******************************************************************************/

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define Motor_LIB_Q15sin(x) (Q15Sin[(uint16_t)(x) >> 6])
#define Motor_LIB_Q15cos(x) (Q15Sin[((uint16_t)((uint16_t)(x) + 16384)) >> 6])

/*! @brief Result of sqrt(3) in float type
 */
#define SQRT3 (1.732f)

/*! @brief Motor run state */
typedef enum {
  STATE_STOP = 0,    /*!< Motor stop state */
  STATE_INIT = 1,    /*!< Motor initialization state */
  STATE_CALC = 2,    /*!< Motor calculation state */
  STATE_BOOT = 3,    /*!< Motor boot state */
  STATE_LOCK = 4,    /*!< Motor lock state */
  STATE_ACCE = 5,    /*!< Motor acceleration state */
  STATE_WORK = 6,    /*!< Motor work state */
  STATE_STANDBY = 7, /*!< Motor standby state */
  STATE_ERROR = 8,   /*!< Motor error state */
  STATE_TEST = 9,    /*!< Motor test state */
} motor_state_t;

/*! @brief Motor error state */
typedef enum {
  E_MOTOR_NO_ERROR = 0,                 /*!< Motor no error */
  E_MOTOR_UNDER_VOLTAGE = 1 << 0,       /*!< Motor under voltage error */
  E_MOTOR_OVER_VOLTAGE = 1 << 1,        /*!< Motor over voltage error */
  E_MOTOR_OVER_TEMPERATURE = 1 << 2,    /*!< Motor over temperature error */
  E_MOTOR_MOSFET_VDS_ERROR = 1 << 3,    /*!< Motor MOSFET VDS error */
  E_MOTOR_EXT_FAULT_ERROR = 1 << 4,     /*!< Motor external fault error */
  E_MOTOR_OVER_CURRENT_ERROR = 1 << 5,  /*!< Motor over current error */
  E_MOTOR_STARTUP_ERROR = 1 << 6,       /*!< Motor startup error */
  E_MOTOR_OVER_LOADING_ERROR = 1 << 7,  /*!< Motor over loading error */
  E_MOTOR_CONNECTION_ERROR = 1 << 8,    /*!< Motor connection error */
  E_MOTOR_ADC_SAMPLE_ERROR = 1 << 9,    /*!< Motor adc conversion setting error */
} motor_error_t;

/*! @brief Motor run mode */
typedef enum {
  MOTOR_OPEN_LOOP = 0,          /*!< Motor open loop mode */
  MOTOR_CURRENT_LOOP = 1,       /*!< Motor close loop mode */
  MOTOR_SPEED_LOOP = 2,         /*!< Motor speed loop mode */
} motor_run_mode_t;

/*! @brief Low pass filter */
typedef struct {
  int32_t qIn;                  /*!< Filter input */
  int32_t qOut;                 /*!< Filter output */
  int32_t qAlpha;               /*!< Filter parameter */
  int32_t qState;               /*!< Filter state */
} motor_low_pass_filter_t;

/*! @brief motor parameters */
typedef struct motor_parameters {
  float rs;                 /*!< Stator resistance in ohm */
  float ls;                 /*!< Stator inductance in H */
  float dead_rate;          /*!< Dead time rate, (Dead time) / (Pwm period) */
  float kfi;                /*!< Voltage constant */
  float alpha;              /*!< Filter ratio */
  float vpeak;              /*!< Max bus voltage ADC can measure in V */
  float ipeak;              /*!< Max bus current ADC can measure in A */
  float freq;               /*!< PWM frequency in Hz */
} motor_parameters_t;

/*!
 * @brief Motor state parameters structure
 * 
 * Contains all runtime state variables needed for FOC control including:
 * - Current measurements and calculations
 * - Voltage calculations
 * - PWM values
 * - Position and speed information
 * - Observer states
 */
typedef struct state_params {
  int32_t pbus_current_offset;  /*!< Bus current ADC value offset */
  int32_t phase_current_offset; /*!< Phase current ADC value offset */
  int32_t pbus_current;         /*!< Bus current without offset */
  int32_t pbus_voltage;         /*!< Bus voltage */
  int32_t state;                /*!< System state */
  int32_t error;                /*!< System error state, ORed of all errors */
  int16_t theta;                /*!< theta angle of d-Axis */
  int16_t sin;                  /*!< sin(theta) */
  int16_t cos;                  /*!< cos(theta) */
  int16_t theta_diff;           /*!< d-Axis theta change */
  int16_t theta_off;            /*!< Observer output difference with real angle */
  uint16_t sector;              /*!< Current sector */
  int32_t vd;                   /*!< d-Axis voltage */
  int32_t vq;                   /*!< q-Axis voltage */
  int32_t valpha;               /*!< alpha axis voltage */
  int32_t vbeta;                /*!< beta axis voltage */
  int16_t pwma[2];              /*!< PWM value of phase A */
  int16_t pwmb[2];              /*!< PWM value of phase B */
  int16_t pwmc[2];              /*!< PWM value of phase C */
  int32_t c1;                   /*!< First current sample value without offset */
  int32_t c2;                   /*!< Second current sample value without offset */
  int32_t ia;                   /*!< phase a rebuilt current result */
  int32_t ib;                   /*!< phase b rebuilt current result */
  int32_t ic;                   /*!< phase c rebuilt current result */
  int32_t sample_offset1;       /*!< First sample point offset */
  int32_t sample_offset2;       /*!< Second sample point offset */
  int16_t s1;                   /*!< First sample point */
  int16_t s2;                   /*!< Second sample point */
  int32_t ialpha;               /*!< alpha current result */
  int32_t ibeta;                /*!< beta current result */
  int32_t id;                   /*!< d-axis current result */
  int32_t iq;                   /*!< q-axis current result */
  int32_t i_step;               /*!< Lock phase current increase step/PWM,
                                   gp.iq_lock/gp.lock_time */
  int32_t w_step;               /*!< acceleration phase speed increase step/PWM,
                                   gp.speed/gp.acc_time */
  int32_t speed;                /*!< Target motor electric speed */

  int32_t pbus_power;           /*!< Bus power */
  int32_t pbus_temp;            /*!< Temperature */
  int32_t est_sin;              /*!< Estimate sin value */
  int32_t est_cos;              /*!< Estimate cos value */
  uint32_t norm_sqr;            /*!< */
  uint32_t norm_sqrt;           /*!< */
  int32_t bemf_acc;             /*!< bemf acc */
  int32_t phase;                /*!< current phase of motor */
  int32_t period_60d;           /*!< period of 60 electric degree */
  int32_t period_30d;           /*!< period of 30 electric degree */
  int32_t cross_time;           /*!< latest 0 crossing time */
  int32_t v1;                   /*!< SVPWM u1 voltage */
  int32_t v2;                   /*!< SVPWM u2 voltage */
  int32_t v3;                   /*!< SVPWM u3 voltage */
} motor_state_params_t;

/*!
 * @brief PI controller structure
 *
 * Standard PI controller implementation with:
 * - Integral sum
 * - Proportional and integral gains
 * - Output limiting
 * - Error tracking
 *
 * The controller operates in Q15 fixed-point format for efficient execution
 */
typedef struct {
  int32_t q15_sum;     /*!< PI sum */
  int32_t q15_kp;      /*!< PI proportional gain */
  int32_t q15_ki;      /*!< PI integral gain */
  int32_t q15_out_max; /*!< PI output max limit */
  int32_t q15_out_min; /*!< PI output min limit */
  int32_t q15_error;   /*!< Error in Q15 format */
  int32_t q15_out;     /*!< PI output */
} motor_pid_t;

/*!
 * @brief Enhanced PI controller with differential and feedforward terms
 *
 * Extended version of standard PI controller that adds:
 * - Maximum error limiting
 * - Feedforward gain
 * - Feedforward input
 *
 * Used for more precise control of current and speed loops
 */
typedef struct {
  int32_t q15_sum;       /*!< PI sum */
  int32_t q15_kp;        /*!< PI proportional gain */
  int32_t q15_ki;        /*!< PI integral gain */
  int32_t q15_out_max;   /*!< PI output max limit */
  int32_t q15_out_min;   /*!< PI output min limit */
  int32_t q15_error;     /*!< Error in Q15 format */
  int32_t q15_error_max; /*!< Max error */
  int32_t q15_out;       /*!< PI output */
  int32_t q15_kf;        /*!< Feed forward */
  int32_t q15_inff;      /*!< Feed forward input */
} motor_pid_diff_t;

/*! @brief Circle limit configuration */
typedef struct {
  uint32_t square_max;       /*!< Max module square */
  int32_t start_index;      /*!< Start index */
  uint16_t *circle_limit_table; /*!< Circle limit table */
} motor_circle_limit_t;

/*!
 * @brief Motor configuration parameters
 *
 * Contains all configurable parameters for motor control including:
 * - Current and speed targets
 * - Timing parameters for startup sequence
 * - Protection thresholds
 * - PI controller configurations
 * - Operating limits
 */
typedef struct config_params {
  int32_t iq;                   /*!< Current calculated Iq */
  int32_t id;                   /*!< Target Id */
  int32_t iq_lock;              /*!< Target lock q-axis current */
  int32_t lock_time;            /*!< Startup lock time, counts of PWM periods */
  int32_t acc_time;             /*!< Speed up time, counts of PWM periods */
  int32_t boot_speed;           /*!< Target boot speed, delta electric theta per PWM */
  int32_t speed;                /*!< electric speed output */
  int32_t close_loop;           /*!< Close loop mode, 0 for open loop; 3 for close loop */
  int32_t rpm;                  /*!< Target motor speed in RPM */
  int16_t pwm_window;           /*!< PWM current sample window width in PWM ticks */
  int16_t pwm_period;           /*!< PWM period in ticks */
  motor_pid_diff_t *id_pi;      /*!< d-axis PI control configuration pointer */
  motor_pid_diff_t *iq_pi;      /*!< q-axis PI control configuration pointer */
  int32_t q15_max_limit;        /*!< Max PI output limit in q15 format */

  int32_t weaken_scale;         /*!< Flux weaken scale */
  int32_t min_boot_voltage;     /*!< Min boot voltage */
  int32_t max_boot_voltage;     /*!< Max boot voltage */
  int32_t min_run_voltage;      /*!< Min runtime voltage */
  int32_t max_run_voltage;      /*!< Max runtime voltage */
  int32_t voltage_error_thresh; /*!< Over under voltage error threshold */
  int32_t voltage_delay_thresh; /*!< Over under voltage recovery threshold */
  int32_t boot_temp;            /*!< Max boot temperature */
  int32_t work_temp;            /*!< Max working temperature */
  int32_t temp_error_thresh;    /*!< Over temperature threshold */
  int32_t max_current;          /*!< Max working bus current */
  int32_t current_error_thresh; /*!< Bus over current detect threshold */
  int32_t no_cross_thresh;      /*!< No cross detect timeout (Motor error) */
  int32_t offset;               /*!< Motor offset */
  int32_t direct;               /*!< Motor direction */
  int32_t pbus_pid_interval;    /*!< Bus current PID control period */
  int32_t pbus_power;           /*!< Target bus power */
  motor_circle_limit_t *circle_limit; /*!< Circle limit configuration */
} motor_config_params_t;

/*!
 * @brief Motor state estimator structure
 *
 * Contains parameters and state variables for estimating:
 * - Rotor position
 * - Back-EMF
 * - Motor speed
 *
 * Uses motor electrical parameters and measurements to perform estimation
 */
typedef struct estimator {
  int32_t rs;                       /*!< Stator resistance in Q15 format with shift */
  int32_t rs_shift;                 /*!< Stator resistance shift value */
  int32_t ls;                       /*!< Stator inductance in Q15 format with shift */
  int32_t ls_shift;                 /*!< Stator inductance shift value */
  int32_t inv_kfi;                  /*!< Voltage constant in Q15 format with shift */
  int32_t inv_kfi_offset;           /*!< Voltage constant shift value */
  int32_t omega;                    /*!< Omega output */
  int32_t omega_shift;              /*!< Omega shift value */
  int32_t period;                   /*!< Estimator execute period, PWM period count */
  int32_t vr;                       /*!< Voltage rate */

  int32_t bemf_alpha;               /*!< Alpha axis back EMF */
  int32_t bemf_beta;                /*!< Beta axis back EMF */
  int32_t ialpha_last;              /*!< Last alpha axis current */
  int32_t ibeta_last;               /*!< Last beta axis current */
  int32_t sin;                      /*!< sin(theta) */
  int32_t cos;                      /*!< cos(theta) */
  motor_low_pass_filter_t bemf_d;   /*!< D-Axis back EMF low pass filter */
  motor_low_pass_filter_t bemf_q;   /*!< Q-Axis back EMF low pass filter */
  int16_t theta;                    /*!< theta angle of d-Axis */
  int16_t padding;
} motor_estimator_t;

/*!
 * @brief Flux weakening controller structure
 *
 * Implements field weakening control for operation above base speed:
 * - Monitors back-EMF
 * - Adjusts field strength
 * - Controls maximum torque available
 *
 * Uses PI control with filtered feedback for stable operation
 */
typedef struct flux_weaken {
  int32_t thresh_scale;           /*!< Flux weaken scale */
  int32_t weaken_step;            /*!< Flux weaken step */
  motor_pid_diff_t weaken_pi;     /*!< Flux weaken PI control */
  motor_low_pass_filter_t bemf_q; /*!< Q-Axis back EMF low pass filter */
  int32_t thresh;                 /*!< Flux weaken threshold */
} flux_weaken_t;

/*******************************************************************************
 * Inline functions
 ******************************************************************************/

/*!
 * @brief Motor low pass filter calculation
 * @param pParm Motor low pass filter parameter pointer
 */
__STATIC_INLINE void Motor_LpFilterCalc(motor_low_pass_filter_t *pParm) 
{
    /* Calculate filter */
    pParm->qState += (pParm->qIn - pParm->qOut) * pParm->qAlpha;
    /* Update output */
    pParm->qOut = (pParm->qState >> 12);
}

/*******************************************************************************
 * API
 ******************************************************************************/

/*
 * @brief  Convert uint32_t to q15_t with shift
 * @param[in]  v  - uint32_t value
 * @param[out] shift -  shift value
 * @return  q15_t value
 */
int32_t Motor_Q15_Adapter(int32_t v, int32_t *shift);

/*!
 * @brief Motor PI control calculation
 * @param pid_ptr  Motor PI control parameter pointer
 */
void Motor_PI_Ctrl_Calc(motor_pid_t *pid_ptr);

/*!
 * @brief Motor PI control calculation with differential
 * @param pid_diff_ptr  Motor PI control parameter pointer
 */
void Motor_PI_Ctrl_Diff_Calc(motor_pid_diff_t *pid_diff_ptr);

/*!
 * @brief Initialize motor control parameters
 *
 * @param motorParameters Physical motor parameters including resistance, inductance, etc.
 * @param EstimatorParameters State estimator configuration structure
 *
 * This function:
 * - Converts physical parameters to Q15 format
 * - Initializes estimator coefficients
 * - Sets up initial state variables
 * - Validates parameter ranges
 */
void Motor_ParameterInit(motor_parameters_t *motorParameters,
                         motor_estimator_t *EstimatorParameters);
/*!
 * @brief Motor estimator reset
 *
 * This function reset estimator state variables
 *
 * @param[in] EstimatorParameters Motor estimator structure pointer
 */
void Motor_EstReset(motor_estimator_t *EstimatorParameters);
/*!
 * @brief Motor SVPWM parameters init
 *
 * This function initialize necessary variable in SVPWM calculation
 *
 * @param[in] MotorConfig Motor configurations pointer
 * @param[in] period PWM period in ticks
 * @param[in] window PWM window size in ticks
 */
void Motor_SvpwmInit(motor_config_params_t *MotorConfig, int16_t period,
                     int16_t window);
/*!
 * @brief SVPWM windows update
 * @param gp Motor configurations pointer
 * @param window PWM window size in ticks
 */
void Motor_SvpwmSetWindow(motor_config_params_t *gp, int16_t window);
/*!
 * @brief Calculate Space Vector PWM duty cycles
 *
 * @param p_state Current motor state including voltage commands
 * @param p_motor_config Motor configuration parameters
 * @return Current sector number (0-5)
 *
 * This function:
 * - Converts alpha/beta voltages to three-phase system
 * - Calculates PWM duty cycles for each phase
 * - Determines optimal switching sequence
 * - Inserts current sampling windows
 */
uint8_t Motor_SvpwmCalc(motor_state_params_t *p_state,
                        motor_config_params_t *p_motor_config);

/*!
 * @brief Calculate Space Vector PWM duty cycles for R23 configuration
 *
 * @param p_state Current motor state including voltage commands
 * @param p_motor_config Motor configuration parameters
 */
uint8_t Motor_SvpwmCalcR23(motor_state_params_t *p_state,
                          motor_config_params_t *p_motor_config);

/*!
 * @brief Rebuilt 3 phase current with PWM window sample value
 *
 * This function will rebuilt motor current value with 2 bus current sample
 * value.
 *
 * @param p_state    motor state parameter pointer
 */
void Motor_RebuiltCurrent(motor_state_params_t *p_state);

/*!
 * @brief Typical motor control service
 *
 * This function will run critical motor control service.
 *
 * @param p_state    motor state parameter pointer
 */
void Motor_FOC_Service(motor_state_params_t *p_state,
                       motor_config_params_t *p_motor_config,
                       motor_estimator_t *ge, int32_t vdc);

/*!
 * @brief Motor FOC service for R23 configuration
 *
 * This function will run critical motor control service for R23 configuration.
 *
 * @param p_state    motor state parameter pointer
 */
void Motor_FOC_ServiceR23(motor_state_params_t *p_state,
                         motor_config_params_t *p_motor_config,
                         motor_estimator_t *ge, int32_t vdc);

/*!
 * @brief Execute motor control estimator
 *
 * This function will estimate current motor angle with sampled motor current
 * and back EFM data.
 * @note This function should not be interrupted.
 *
 * @param p_state   motor state parameter pointer
 * @param p_est     Motor estimator parameter pointer
 * @param vdc       bus voltage sample value
 */
void Motor_RunEstCalc(motor_state_params_t *p_state, motor_estimator_t *p_est,
                      int32_t vdc);

/*!
 * @brief Motor flux weaken control parameter init
 * @param p_flux Motor flux weaken control parameter pointer
 * @param scale Flux weaken scale
 * @param alpha
 * @param step Flux weaken step
 * @param max  Flux weaken max value
 */

void Motor_FluxInit(flux_weaken_t *p_flux, float scale, float alpha,
                    int32_t step, int32_t max);

/*!
 * @brief Motor flux weaken control reset
 */
void Motor_FluxReset(flux_weaken_t *p_flux);

/*!
 * @brief Motor flux weaken control calculation
 * @param vdc   bus voltage
 * @param ge    motor estimator configuration pointer
 * @return
 */
int32_t Motor_FluxCalc(flux_weaken_t *p_flux, int32_t vdc,
                       motor_estimator_t *ge);

#endif /* MOTOR_FOC_LIB_H */
