#ifndef USER_MOTORCONTROL_FUNCTIONS_H
#define USER_MOTORCONTROL_FUNCTIONS_H

#include "math.h"
#include "User_Constants.h"
#include "User_data_types.h"
#include "User_Macros.h"

void PIReg(XPIRegPars *par,XPIRegVars *var);
void PWMduty(Xabc vsabc_ref, float vdc, Xabc* duty_abc);
void theta_enc_compute(float *thetam, float polepairs, Xsc *SinCos, Xsc *SinCos_elt);
void speed_compute_sc(Xsc sincos, Xsc *sincos_old, float* omega);
void theta_and_speed_compute_sc(float *thetam, float* omega, float polepairs, Xsc *SinCos, Xsc *SinCos_old, Xsc *SinCos_elt, Xsc *SinCos_elt_old);
void DTComp(Xabc isabc, float amp_dt, Xabc *duty_abc);
void DTComp1(Xabc isabc, float amp_dt_V, Xabc *vsabc);
float deltaV(XCompPar *CompPar);
void DTComp_IM(Xabc duty, Xabc duty_km1, Xabc isabc, float vdc, Xabc *vsabc, Xabc *v_error_deadtime, float *enable_deadtime);
void VoltageDrop(XCompPar *TR_par, XCompPar *D_par, Xabc isabc, Xabc duty, Xabc *v_error_voltage_drop, float *enable_voltage_drop);
void VoltageCompensation(Xabc duty, Xabc *vsabc, float vdc, Xabc *v_error, Xabc v_error_deadtime, Xabc v_error_voltage_drop, float enable_voltage_drop, float enable_deadtime);
// void Current_loop(float vdc, float Imax, Xdq isdq_ref, Xdq  isdq, XPIRegPars* id_par, XPIRegVars* id_var, XPIRegPars* iq_par, XPIRegVars* iq_var,Xdq* vsdq_ref);
void CurrentLoop_d_priority(float vdc, float Imax, Xdq *isdq_ref, Xdq isdq, XPIRegPars  *id_par, XPIRegPars  *iq_par, XPIRegVars  *id_var, XPIRegVars *iq_var, Xdq vsdq_ref_ffw, Xdq *vsdq_ref);
void CurrentLoop_q_priority(float vdc, float Imax, Xdq *isdq_ref, Xdq isdq, XPIRegPars  *id_par, XPIRegPars  *iq_par, XPIRegVars  *id_var, XPIRegVars *iq_var, Xdq vsdq_ref_ffw, Xdq *vsdq_ref);
void Gen_theta_ref(float omega_ref_ramp, float* theta_ref, Xsc* SinCos_ref );
void CurrentProtection(Xabc isabc, int* State, int* pwm_stop);
void ramp(float target, float delta, float *output);
void HF_position_detect2(Xalphabeta isab, int commiss_counter,int* counter ,float* theta_hf,Xalphabeta* vsab_ref, float  *theta0);
void ReadLut(float *tab0, float Xin, float Xmax,float Xmin, float DX, float inv_DX, float* Yout);
void ReadLut2d(float *P, float x, float y, float Dx, float invDx, float Dy, float invDy, float Xmax, float Xmin, float Ymax , float Ymin, int Npointx, float* V);
void SinCosCalc(Xsc *SinCos, Xalphabeta_amp *Lambda);

int binary_search(float *array, int size, float target, int increasing);

// prese da User_MotorControl_Functions Simulink PMSM, nel caso guarda anche le variables 
void DTComp_PMSM(Xabc duty, Xabc duty_km1, Xabc isabc,float vdc,float dt, Xabc *vsabc);
float sgn(float i);
void FluxObserver(void);
void Compute_Inductance(void);
void HF_dem(float *sig, float *sig_HF, float *acc, float *buffer, int counterHF, int ns);


void Iw_flux_est(void);
void Itheta_flux_est(void);
void PLL_ctrl(XPIRegPars *PLL_par, XPIRegVars *PLL_var, Xsc SinCos, Xsc *SinCos_PLL, float *omega_PLL);
// void VIpole_flux_est(float kobs, float RS, float SIGMALS,float inv_KR, Xalphabeta isab,Xalphabeta vsab_ref, Xalphabeta_amp* lambdaS_obs,Xalphabeta_amp* lambdaR_VI, Xsc* SinCos_VI,Xsc* SinCos_VI_old, float* omega_flux, float* omega_flux1);
void VI_pole_flux_estimator(Xalphabeta_amp *lambdaS, Xalphabeta_amp *lambdaR, Xsc *SinCos, Xalphabeta vsab, Xalphabeta isab, float g);
void Iw_flux_estimator(Xalphabeta_amp *lambdaR_old, Xalphabeta_amp *lambdaR, Xsc *SinCos, Xalphabeta isab, float polepairs, float omega_r_filtered);
void rotation(Xalphabeta ab, Xsc sc ,Xdq *dq);
void inv_rotation(Xdq *dq, Xsc sc, Xalphabeta_amp *ab);
void Itheta_flux_estimator(Xdq *lambdaR_rotor, Xalphabeta_amp *lambdaR, Xalphabeta isab, Xdq *isab_rotor, Xsc SinCos_r_elt , Xsc SinCos_r_elt_old, Xsc *SinCos);
void VI_omega_flux_observer(Xalphabeta_amp *lambdaR_Iw_old, Xalphabeta_amp *lambdaR_Iw, Xsc *SinCos_Iw, Xalphabeta vsab, Xalphabeta isab, float polepairs, float omega_r_filtered, Xalphabeta_amp *lambdaS_VIpole, Xalphabeta_amp *lambdaR, Xsc *SinCos, float g);
void VI_theta_flux_observer(Xdq *lambdaR_rotor, Xalphabeta_amp *lambdaR_Itheta, Xalphabeta isab, Xdq *isab_rotor, Xsc SinCos_r_elt, Xsc SinCos_r_elt_old, Xsc *SinCos_Itheta, Xalphabeta vsab, Xalphabeta_amp *lambdaS_VIpole, Xalphabeta_amp *lambdaR, Xsc *SinCos, float g);
void Sensorless_flux_observer(Xalphabeta vsab, Xalphabeta isab, Xdq isdq_ref, Xalphabeta *isab_est, Xalphabeta *esab_est, Xalphabeta_amp *lambdaS_Sensorless, Xalphabeta_amp *lambdaR_Sensorless, Xsc *SinCos_Sensorless, float polepairs, float *omega_slip_est, float *omega_elt_est, float *omega_r_est, float g);

#endif

