#include "../Inc/User_MotorControl_Functions.h"

// PI regulator with anti wind-up
void PIReg(XPIRegPars *par,XPIRegVars *var)
{
    float int_lim;
    
    //Error computation
    var->err  = var->ref-var->fbk;    //Proportional regulator
    var->prop = par->kp*var->err;
    //Saturation of the proportional part to lim
    if (var->prop>par->lim)
        var->prop=par->lim;
    if (var->prop<(-par->lim))
        var->prop=-par->lim;
    
    //Limit of the intg part
    int_lim = par->lim-fabs(var->prop);
    
    //Integral part
    var->intg+=par->ki*var->err;
    
    //Saturation of the intg part
    if (var->intg > int_lim)
        var->intg = int_lim;
    if (var->intg < (-int_lim))
        var->intg = -int_lim;
    
    //Output computation
    var->out=var->prop+var->intg;
}

void theta_enc_compute(float *thetam, float polepairs, Xsc *SinCos, Xsc *SinCos_elt) {
    
    float theta_elt;
    
    if (*thetam>=TWOPI) 	*thetam-=TWOPI;
    if (*thetam<0.0f)     *thetam+=TWOPI;
    
    SinCos->sin = sinf(*thetam);		// mech rad
    SinCos->cos = cosf(*thetam);
    
    theta_elt = *thetam * polepairs;
    
    SinCos_elt->sin = sinf(theta_elt);
    SinCos_elt->cos = cosf(theta_elt);
    //(*SinCos_elt).sin = sinf(theta_elt);	// elt rad, è la stessa cosa se scritta così
    //(*SinCos_elt).cos = cosf(theta_elt);
    
}

void speed_compute_sc(Xsc sincos, Xsc *sincos_old, float* omega) { 
    
	  float tmp1, tmp2, tmp3;
    
    tmp1 = sincos.sin * sincos_old->cos;
    tmp2 = sincos.cos * sincos_old->sin;
    
    tmp3 = tmp1-tmp2;
    *omega = fs * tmp3;
    
    sincos_old->sin = sincos.sin;
    sincos_old->cos = sincos.cos;
   
}

// funzione implementata per poter usare valori vecchi SinCos_elt_old nell' I-theta flux estimator 
void theta_and_speed_compute_sc(float *thetam, float* omega, float polepairs, Xsc *SinCos, Xsc *SinCos_old, Xsc *SinCos_elt, Xsc *SinCos_elt_old) { 
	
		float theta_elt;
    
    if (*thetam >= TWOPI)  		*thetam -= TWOPI;
    if (*thetam < 0.0f)       *thetam += TWOPI;
  
	  // salvi valori vecchi, volendo puoi usare anche solo:   *SinCos_old = *SinCos;     *SinCos_elt_old = *SinCos_elt;
	  SinCos_old->sin = SinCos->sin;
		SinCos_old->cos = SinCos->cos;
	  SinCos_elt_old->sin = SinCos_elt->sin;
    SinCos_elt_old->cos = SinCos_elt->cos;

    SinCos->sin = sinf(*thetam);		// mech rad
    SinCos->cos = cosf(*thetam);
    
	  // theta_elt = PP*theta_R
    theta_elt = *thetam * polepairs;
    
    SinCos_elt->sin = sinf(theta_elt);  //(*SinCos_elt).sin = sinf(theta_elt);	     // elt rad
    SinCos_elt->cos = cosf(theta_elt);  //(*SinCos_elt).cos = cosf(theta_elt);
	
		// omega_r : Rotor Speed
 	  float tmp1, tmp2, tmp3;
    
    tmp1 = SinCos->sin * SinCos_old->cos;
    tmp2 = SinCos->cos * SinCos_old->sin;
    tmp3 = tmp1 - tmp2;
		
    *omega = fs * tmp3;	
}

void PWMduty(Xabc vsabc_ref, float vdc, Xabc* duty_abc) {
    
		float tmp1, tmp2, tmp3, vdc_inv, vzs;
	
		if (vdc>0.0f)	vdc_inv = 1.0f/vdc;	
		tmp1 = vsabc_ref.a;	
		tmp2 = vsabc_ref.b; 
	
		if (vsabc_ref.a < vsabc_ref.b) {
				tmp1 = vsabc_ref.b;		
				tmp2 = vsabc_ref.a;		
		}
		if (tmp1 < vsabc_ref.c) tmp3 = tmp1;
		else {
			if (tmp2 > vsabc_ref.c) tmp3 = tmp2;
			else 	tmp3 = vsabc_ref.c;
		}
		
		vzs = tmp3 * 0.5f;	// zero sequence voltage
				
		// Duty-cycles, with zero-sequence
		duty_abc->a = 0.5f + (vsabc_ref.a + vzs)*vdc_inv;
		duty_abc->b = 0.5f + (vsabc_ref.b + vzs)*vdc_inv;
		duty_abc->c = 0.5f + (vsabc_ref.c + vzs)*vdc_inv;  
}


// -------------------------- Funzioni per compensare Deadtime e Voltage Drop su transistor e diodi inverter -------------------------- //


void DTComp(Xabc isabc, float amp_dt, Xabc *duty_abc) {
    
    if (isabc.a>0) duty_abc->a += amp_dt;
    else duty_abc->a += -amp_dt;
    
    if (isabc.b>0) duty_abc->b += amp_dt;
    else duty_abc->b += -amp_dt;
    
    if (isabc.c>0) duty_abc->c += amp_dt;
    else duty_abc->c += -amp_dt;
    
}


void DTComp1(Xabc isabc, float amp_dt_V, Xabc *vsabc) {
    
    if (isabc.a >0) vsabc->a += -amp_dt_V;
    else vsabc->a += amp_dt_V;
    
    if (isabc.b>0) 	vsabc->b += -amp_dt_V;
    else vsabc->b += amp_dt_V;
    
    if (isabc.c>0) vsabc->c += -amp_dt_V;
    else vsabc->c += amp_dt_V;
    
}


// presa da file PMSM
void DTComp_PMSM(Xabc duty, Xabc duty_km1, Xabc isabc,float vdc,float dt, Xabc *vsabc) {
	
		float v_error_A, v_error_B, v_error_C, vA, vB, vC;
		float Ir,slopeA,slopeB,slopeC,max,min;
		Ir = 15;
		max = Ir*0.01;
		min = -Ir*0.01;
		
		if(duty.a>1) duty.a = 1.;
		if(duty.a<0) duty.a = 0.;
		if(duty.b>1) duty.b = 1.;
		if(duty.b<0) duty.b = 0.;
		if(duty.c>1) duty.c = 1.;
		if(duty.c<0) duty.c = 0.;
			
		v_error_A = vdc*dt/Ts;
		slopeA = v_error_A/max;
		if(isabc.a<max && isabc.a>min)
				v_error_A = slopeA*(isabc.a-max)+v_error_A;
		else{
				if(isabc.a<min)
			v_error_A = -v_error_A;
		}
			 
		v_error_B = vdc*dt/Ts;
		slopeB = v_error_B/max;
		if(isabc.b<max && isabc.b>min)
				v_error_B = slopeB*(isabc.b-max)+v_error_B;
		else{
				if(isabc.b<min)
						v_error_B = -v_error_B;
		}
		
		v_error_C = vdc*dt/Ts;
		slopeC = v_error_C/max;
		if(isabc.c<max && isabc.c>min)
				v_error_C = slopeC*(isabc.c-max)+v_error_C;
		else{
				if(isabc.c<min)
						v_error_C = -v_error_C;
		}
	
		if ((duty.a  == 0 && duty_km1.a == 0) || (duty.a  == 1 && duty_km1.a == 1))
			 v_error_A = 0;
		if ((duty.b  == 0 && duty_km1.b == 0) || (duty.b  == 1 && duty_km1.b == 1))
			 v_error_B = 0;
		if ((duty.c  == 0 && duty_km1.c == 0) || (duty.c  == 1 && duty_km1.c == 1))
			 v_error_C = 0;
		
		if ((duty.a == 1 && duty_km1.a == 0 && isabc.a<0) || (duty.a == 0 && duty_km1.a == 1 && isabc.a>0))
			 v_error_A = 0;
		if ((duty.b == 1 && duty_km1.b == 0 && isabc.b<0) || (duty.b == 0 && duty_km1.b == 1 && isabc.b>0))
			 v_error_B = 0;
		if ((duty.c == 1 && duty_km1.c == 0 && isabc.c<0) || (duty.c == 0 && duty_km1.c == 1 && isabc.c>0))
			 v_error_C = 0;
		
		vA	= vdc*(duty.a-0.5)- v_error_A;
		vB	= vdc*(duty.b-0.5)- v_error_B;
		vC	= vdc*(duty.c-0.5)- v_error_C;
		
		vsabc->a = 2.0*vA/3.0-(vB+vC)/3.0;
		vsabc->b = 2.0*vB/3.0-(vA+vC)/3.0;
		vsabc->c = 2.0*vC/3.0-(vA+vB)/3.0;
		
		duty_km1.a = duty.a;
		duty_km1.b = duty.b;
		duty_km1.c = duty.c;
}


// Voltage drop computation for a generic component, 2-parameter model ( Von, Rd ---> mdl = 1) or 5-parameter model ( Von1, Rd1, Iknee, Von2, Rd2 ---> mdl = 2 ) 
float deltaV(XCompPar *CompPar){
	
	float voltage_drop;
	
  if (CompPar->mdl < 1.5f) 											voltage_drop = CompPar->vON1 + CompPar->Rd1 * fabs(CompPar->curr);
  else{
		if(fabs(CompPar->curr) <= CompPar->Iknee)		voltage_drop = CompPar->vON1 + CompPar->Rd1 * fabs(CompPar->curr);
		else																				voltage_drop = CompPar->vON2 + CompPar->Rd2 * (fabs(CompPar->curr) - CompPar->Iknee);
		}
	return(voltage_drop);
}

void VoltageDrop(XCompPar *TR_par, XCompPar *D_par, Xabc isabc, Xabc duty, Xabc *v_error_voltage_drop, float *enable_voltage_drop){
		 
	  *enable_voltage_drop = 1.0f;
	
		// Voltage errors due to the on-state voltage drops	
    TR_par->curr = isabc.a;
    D_par->curr = isabc.a;
	  if (sgn(isabc.a) > 0.0f) 		v_error_voltage_drop->a = deltaV(D_par) + duty.a * (deltaV(TR_par) - deltaV(D_par));
    else{
    	 if (sgn(isabc.a) < 0.0f) v_error_voltage_drop->a = -deltaV(TR_par) + duty.a * (deltaV(TR_par) - deltaV(D_par));
       else 										v_error_voltage_drop->a = 0.0f;   
    } 
    TR_par->curr = isabc.b;
    D_par->curr = isabc.b;
	  if (sgn(isabc.b) > 0.0f)		v_error_voltage_drop->b = deltaV(D_par) + duty.b * (deltaV(TR_par) - deltaV(D_par));
    else{
    	 if (sgn(isabc.b) < 0.0f)	v_error_voltage_drop->b = -deltaV(TR_par) + duty.b * (deltaV(TR_par) - deltaV(D_par));
       else 										v_error_voltage_drop->b = 0.0f;   
    } 
    TR_par->curr = isabc.c;
    D_par->curr = isabc.c;
	  if (sgn(isabc.c) > 0.0f)		v_error_voltage_drop->c = deltaV(D_par) + duty.c * (deltaV(TR_par) - deltaV(D_par));
    else{
    	if (sgn(isabc.c) < 0.0f)	v_error_voltage_drop->c = -deltaV(TR_par) + duty.c * (deltaV(TR_par) - deltaV(D_par));
      else 											v_error_voltage_drop->c = 0.0f;   
    }  
}


void DTComp_IM(Xabc duty, Xabc duty_km1, Xabc isabc, float vdc, Xabc *vsabc, Xabc *v_error_deadtime, float *enable_deadtime) {
		float vA, vB, vC;
	  float lim_inf, lim_sup;
		*enable_deadtime = 1.0f;
    // Duty Saturation  	
		lim_inf = 0.05f; lim_sup = 0.95f;
		if(duty.a > 0.95f) duty.a = 0.95f;
		if(duty.a < 0.05f) duty.a = 0.05f;
		if(duty.b > 0.95f) duty.b = 0.95f;
		if(duty.b < 0.05f) duty.b = 0.05f;
		if(duty.c > 0.95f) duty.c = 0.95f;
		if(duty.c < 0.05f) duty.c = 0.05f;
		v_error_deadtime->a = vdc * DEADTIME * fs * sgn(isabc.a);
		v_error_deadtime->b = vdc * DEADTIME * fs * sgn(isabc.b);
		v_error_deadtime->c = vdc * DEADTIME * fs * sgn(isabc.c);
		if ((duty.a  == 0.05f && duty_km1.a == 0.05f) || (duty.a  == 0.95f && duty_km1.a == 0.95f))																				v_error_deadtime->a = 0.0f;
		if ((duty.b  == 0.05f && duty_km1.b == 0.05f) || (duty.b  == 0.95f && duty_km1.b == 0.95f))																				v_error_deadtime->b = 0.0f;
		if ((duty.c  == 0.05f && duty_km1.c == 0.05f) || (duty.c  == 0.95f && duty_km1.c == 0.95f))																				v_error_deadtime->c = 0.0f;
		if ((duty.a == 0.95f && duty_km1.a == 0.05f && sgn(isabc.a)<0) || (duty.a == 0.05f && duty_km1.a == 0.95f && sgn(isabc.a)>0))			v_error_deadtime->a = 0.0f;
		if ((duty.b == 0.95f && duty_km1.b == 0.05f && sgn(isabc.b)<0) || (duty.b == 0.05f && duty_km1.b == 0.95f && sgn(isabc.b)>0))			v_error_deadtime->b = 0.0f;
		if ((duty.c == 0.95f && duty_km1.c == 0.05f && sgn(isabc.c)<0) || (duty.c == 0.05f && duty_km1.c == 0.95f && sgn(isabc.c)>0)) 		v_error_deadtime->c = 0.0f;
		duty_km1.a = duty.a;
		duty_km1.b = duty.b;
		duty_km1.c = duty.c;
		/* Calcolo delle tensioni considerando solo l'errore del deadtime e non il voltage drop su transistor e diodi 
		// Inverter output voltages respect to the inverter's neutral point	
		vA = vdc*(duty.a-0.5) - v_error_deadtime->a;
		vB = vdc*(duty.b-0.5) - v_error_deadtime->b;
		vC = vdc*(duty.c-0.5) - v_error_deadtime->c;
		
		// Load phase voltages for wye connection (stella)
		vsabc->a = 2.0*vA/3.0-(vB+vC)/3.0;
		vsabc->b = 2.0*vB/3.0-(vA+vC)/3.0;
		vsabc->c = 2.0*vC/3.0-(vA+vB)/3.0;
		
		// Credo siano tensioni a triangolo ma tanto il controllo si fa sempre considerando l'equivalente stella 
		// y[0] = vdc*dutyA-v_errorA;
    // y[1] = vdc*dutyB-v_errorB;
    // y[2] = vdc*dutyC-v_errorC;
		*/
}
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){
	 float vA, vB, vC;
	 // Errors in the inverters' output voltages respect to its neutral point 
   v_error->a = v_error_deadtime.a * enable_deadtime + v_error_voltage_drop.a * enable_voltage_drop;
   v_error->b = v_error_deadtime.b * enable_deadtime + v_error_voltage_drop.b * enable_voltage_drop;
   v_error->c = v_error_deadtime.c * enable_deadtime + v_error_voltage_drop.c * enable_voltage_drop;
	 // Inverter output voltages respect to the inverter's neutral point
	 vA = vdc * (duty.a - 0.5f) - v_error->a;
   vB = vdc * (duty.b - 0.5f) - v_error->b;
   vC = vdc * (duty.c - 0.5f) - v_error->c;
	 // Load phase voltages for wye connection
	 vsabc->a = 2.0f * vA/3.0f -(vB+vC)/3.0f;
	 vsabc->b = 2.0f * vB/3.0f -(vA+vC)/3.0f;
	 vsabc->c = 2.0f * vC/3.0f -(vA+vB)/3.0f;
	// Credo siano tensioni di fase a triangolo ma tanto il controllo si fa sempre considerando l'equivalente stella 
	 // vsabc->a = vdc*duty_abc.a - v_error.a;
   // vsabc->b = vdc*duty_abc.b - v_error.b;
   // vsabc->c = vdc*duty_abc.c - v_error.c;
}


//Reference angle generation (V/Hz, I-Hz)
void Gen_theta_ref(float omega_ref_ramp, float* theta_ref, Xsc* SinCos_ref )
{
    *theta_ref += Ts * omega_ref_ramp;
    if (*theta_ref >= TWOPI)	*theta_ref -= TWOPI;
    if (*theta_ref < 0.0f)		*theta_ref += TWOPI;
    
    SinCos_ref->sin = sinf(*theta_ref);
    SinCos_ref->cos = cosf(*theta_ref);
}

void CurrentProtection(Xabc isabc, int* State, int* pwm_stop) {
    
    
    if (fabs(isabc.a)>CRT_PROT) {
        *pwm_stop = 1;
        *State=ERROR;
    }
    if (fabs(isabc.b)>CRT_PROT) {
        *pwm_stop = 1;
        *State=ERROR;
    }
    if (fabs(isabc.c)>CRT_PROT) {
        *pwm_stop = 1;
        *State=ERROR;
    }
    
}


void ramp(float target, float delta, float *output) {
    
    if (*output <= target) {
        *output = (*output + delta);
        if (*output>target)
            *output=target;
    }
    else {
        *output = (*output - delta);
        if (*output<target)
            *output=target;
    }
}

void SinCosCalc(Xsc *SinCos, Xalphabeta_amp *Lambda) {
	
    float tmp1;
    // Cosine computation with amplitude check
    tmp1=Lambda->amp;
    
    if (Lambda->alpha>=tmp1) SinCos->cos= 1.;
    else {
        if ((Lambda->alpha)<-tmp1) SinCos->cos=-1.;
        else SinCos -> cos = Lambda -> alpha / Lambda-> amp;
    }
    
    // Sine computation with amplitude check
    if (Lambda->beta >= tmp1) SinCos->sin = 1.;
    else {
        if (Lambda->beta < -tmp1) SinCos->sin =  -1.;
        else SinCos -> sin = Lambda -> beta / Lambda-> amp;
    }
}


void PLL_ctrl(XPIRegPars *PLL_par, XPIRegVars *PLL_var, Xsc SinCos, Xsc *theta_PLL, float *omega_PLL){
	
		PLL_var->ref = SinCos.sin * theta_PLL->cos - SinCos.cos * theta_PLL->sin;      
		PLL_var->fbk = 0.0f;                                                                 
		PLL_var->fwd = 0.0f;
		PLL_par->lim = N_MAX*RPM2RAD;                // velocità limite del motore posta a 2000 rpm, nel caso aumenti questo limite 
		PIReg(PLL_par, PLL_var); 
		*omega_PLL = PLL_var->out;                                                            
		theta_PLL->angle += Ts * (*omega_PLL);      // theta_PLL                                                       
		if(theta_PLL->angle >= TWOPI) 
			theta_PLL->angle -= TWOPI;
		if(theta_PLL->angle < 0.0f)		
			theta_PLL->angle += TWOPI;
	
		theta_PLL->cos = cosf(theta_PLL->angle);                                                         
		theta_PLL->sin = sinf(theta_PLL->angle);

}


// si usa per fare DC alignment procedure per salient machines senza bisogno di farle muovere: sensorless techniques based on HF signal injection
void HF_position_detect2(Xalphabeta isab, int commiss_counter,int* counter ,float* theta_hf,Xalphabeta* vsab_ref, float  *theta0) {
    //==========================================================================
    //	High Frequency Position Detection
    //	Rotating voltage --> Imax detection
    //==========================================================================
    
	float IHFamp, IHFmax;
	
	// HF_position_detect	
	#define	nsamples_HF  10.025f	// fPWM/fHF
	//#define	20.0f	// fPWM/fHF
    
	#ifdef SPM
		#define V_HF  7.0f			// HF voltage amplitude for SPM motor
	#else
		#define V_HF 120.0f			// HF voltage amplitude for PMA motor
	#endif
	
    IHFamp=isab.alpha*isab.alpha+isab.beta*isab.beta;	// Current (amplitude)^2
    
    // find maximum amplitude position (after 400 samples)
    if((IHFamp>=IHFmax)&&(commiss_counter>400))	{
       // commiss_counter=500;
			IHFmax = IHFamp;
#if defined(SPM)
        *theta0 = *theta_hf - HALFPI;
#else
        *theta0 = *theta_hf - PI;
#endif
    }
    
    *theta_hf += TWOPI/nsamples_HF;	    // HF phase angle on nsamples_HF positions
    if (*theta_hf>TWOPI) *theta_hf -= TWOPI;
    
    if (commiss_counter<3000) {
       
			// Voltage signals
        *counter=*counter+1;
        if(*counter>200) *counter = 200;
        vsab_ref->alpha = (*counter/200.0f)*V_HF*cosf(*theta_hf);
        vsab_ref->beta  = (*counter/200.0f)*V_HF*sinf(*theta_hf);
    }
    else {
        // stop injection
        *counter = 0;
        vsab_ref->alpha = 0.0f;
        vsab_ref->beta  = 0.0f;
    }
}


void ReadLut(float *tab0, float Xin, float Xmax, float Xmin, float DX, float inv_DX, float* Yout){

    int X0;
    
    //Limit input range 
    if (Xin > Xmax) Xin = Xmax;
    if (Xin < Xmin) Xin = Xmin;
    
    X0 = floor((Xin-Xmin)*inv_DX);
    tab0 = tab0 + X0;
    //Interpolate between Y(tab0) and Y(tab0+1) 
    *Yout = *tab0 +(*(tab0+1)-*tab0)*inv_DX*(Xin-Xmin-DX*X0);
	
//	ReadLut(&THETA_R_LUT[0], theta_encoder, THETA_MAX, THETA_MIN, DTHETA, INV_DTHETA, &theta_r); 	// Real-time Angle Compensation 
    
}


// Read a 2 dimensional look-up-table (matrix)
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) {
    
		float ix, iy;
    float rx, ry;
    float V1, V2;
    int delta;
    // Funzione per l'interpolazione di Tabelle 2D
    // Limitazione degli ingressi
    if (x > Xmax) {x=Xmax-0.1f*Dx;}
    if (x < Xmin) {x=Xmin+0.1f*Dx;;}
    if (y > Ymax) {y=Ymax-0.1f*Dy;}
    if (y < Ymin) {y=Ymin+0.1f*Dy;}
    //calcolo delle posizione inziale per l'interpolazione
    rx=(x-Xmin)*invDx;
    ry=(y-Ymin)*invDy+1;
    // indici interi
    ix=floor(rx);
    iy=floor(ry);
    // resti per l'interpolazione
    rx=rx-ix;
    ry=ry-iy;
    //primo punto
    delta=(ix)+(iy-1)*Npointx;
    P=P+delta;
    //interpolazione quadrata
    V1=*P+(*(P+1)-*P)*rx;
    P=P+Npointx;
    V2=*P+(*(P+1)-*P)*rx;
    *V=V1+(V2-V1)*ry; 
}


// prese da User_MotorControl_Functions del PMSM Simulink LAB06
float sgn(float i){

	float sgn;
    if(i>0.000001)
        sgn=1.0f;
    else
     if(i<-0.000001)
				sgn=-1.0f;
     else
         sgn=0;
    
 return (sgn);
}


// Bandpass Fitler

void HF_dem(float *sig, float *sig_HF, float *acc, float *buffer, int counterHF, int ns){
	
	*acc += (*sig - *(buffer+counterHF))/ns;
	*(buffer+counterHF) = *sig;
	*sig_HF = *sig - *acc;
}


// -------------------------- Current Loops -------------------------- //
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){

	  float tmp1; 
	
		//d-axis current control loop
		id_var->ref = isdq_ref->d;
		id_var->fbk = isdq.d;
		id_par->lim = SQRT1OVER3*vdc - fabs(vsdq_ref_ffw.d);
		PIReg(id_par, id_var);
		vsdq_ref->d = id_var->out;
		vsdq_ref->d += vsdq_ref_ffw.d;
		
		// Saturation to Imax
		tmp1 = sqrtf(Imax*Imax - isdq_ref->d * isdq_ref->d);
		if(isdq_ref->q > tmp1)   isdq_ref->q = tmp1;
		if(isdq_ref->q < -tmp1)  isdq_ref->q = -tmp1;
		
		//q-axis current control loop
		iq_var->ref = isdq_ref->q;
		iq_var->fbk = isdq.q;
		iq_par->lim = sqrtf(ONE_THIRD*vdc*vdc - vsdq_ref->d * vsdq_ref->d) - fabs(vsdq_ref_ffw.q);
		PIReg(iq_par, iq_var);
		vsdq_ref->q = iq_var->out;
		vsdq_ref->q += vsdq_ref_ffw.q;

}

// Current Loop with feed-forward, ma si può usare sempre tanto al massimo v,ffw sono a zero
// Asse q prioritario, non conviene in questa macchina perchè è più importante mantenerla flussata in ogni situazione e al massimo perdere dinamica di coppia dovuta ad isq
// in ogni caso la tensione su d dovrebbe essere sempre abbastanza bassa quindi su q si ha un limite molto grande di tensione disponibile 
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){

	  float tmp1;
	
	  // Saturation to Imax
		tmp1 = sqrtf(Imax*Imax - isdq_ref->d * isdq_ref->d);
		if(isdq_ref->q > tmp1)   isdq_ref->q = tmp1;
		if(isdq_ref->q < -tmp1)  isdq_ref->q = -tmp1;
	
	  //q-axis current control loop
		iq_var->ref = isdq_ref->q;
		iq_var->fbk = isdq.q;
 	  iq_par->lim = SQRT1OVER3*vdc - fabs(vsdq_ref_ffw.q);
	  PIReg(iq_par, iq_var);
		vsdq_ref->q = iq_var->out;
		vsdq_ref->q += vsdq_ref_ffw.q;
	
		//d-axis current control loop
		id_var->ref = isdq_ref->d;
		id_var->fbk = isdq.d;
		id_par->lim = sqrtf(ONE_THIRD*vdc*vdc - vsdq_ref->q * vsdq_ref->q) - fabs(vsdq_ref_ffw.d);
		PIReg(id_par, id_var);
		vsdq_ref->d = id_var->out;
		vsdq_ref->d += vsdq_ref_ffw.d;

}

// -------------------------- Fine Current Loops -------------------------- //


// -------------------------- Funzioni Estimators e Observers -------------------------- //

// In generale non conviene utilizzare funzioni (void) ma è meglio crearle passando tutti i variabili all'interno della ISR come per tutte le funzioni presenti in questo file
// Per poter utilizzare funzioni (void) nell'ISR che utilizzino in automatico i parametri definiti in Variables.h NON aggiungere all'inizio di questo file #include "../Inc/User_Variables.h" perchè vede le variabili definite due volte.
// Occorre quindi richiamare tutte le variabili definite in Variables.h tramite il comando "extern", ma forse serve aggiungere a inizio file #include "../Inc/User_data_types.h" siccome passi variabili che non sono solamente float o int ma anche struct create da noi
// Nella ISR quando devi usare I-w scirvi solo:  Iw_flux_est();
void Iw_flux_est(void) {

extern Xalphabeta_amp	lambdaR_Iw_old, lambdaR_Iw; 
extern Xalphabeta isab;
extern Xsc SinCos_Iw;
extern float omega_r_filt;
	
			lambdaR_Iw_old.alpha = lambdaR_Iw.alpha;
			lambdaR_Iw_old.beta  = lambdaR_Iw.beta;
			lambdaR_Iw.alpha 		+= Ts * ((LM*isab.alpha - lambdaR_Iw.alpha)*INV_TAUR - PP*omega_r_filt*lambdaR_Iw_old.beta);
			lambdaR_Iw.beta 		+= Ts* ((LM*isab.beta - lambdaR_Iw.beta)*INV_TAUR + PP*omega_r_filt*lambdaR_Iw_old.alpha);
			lambdaR_Iw.amp 			 = sqrtf(lambdaR_Iw.alpha*lambdaR_Iw.alpha + lambdaR_Iw.beta*lambdaR_Iw.beta); 
			
			if(lambdaR_Iw.amp == 0.0f) // avoid division by zero 
			{
				SinCos_Iw.cos = 1.0f;
				SinCos_Iw.sin = 0.0f;
			}
			else SinCosCalc(&SinCos_Iw, &lambdaR_Iw);
}


void Iw_flux_estimator(Xalphabeta_amp *lambdaR_old, Xalphabeta_amp *lambdaR, Xsc *SinCos, Xalphabeta isab, float polepairs, float omega_r_filtered) {
	
		lambdaR_old->alpha = lambdaR->alpha;
		lambdaR_old->beta	 = lambdaR->beta;
		lambdaR->alpha		+= Ts * ((LM * isab.alpha - lambdaR->alpha) * INV_TAUR - polepairs * omega_r_filtered * lambdaR_old->beta);
		lambdaR->beta 		+= Ts * ((LM * isab.beta - lambdaR->beta) * INV_TAUR + polepairs * omega_r_filtered  * lambdaR_old->alpha);
		lambdaR->amp 			 = sqrtf(lambdaR->alpha * lambdaR->alpha + lambdaR->beta * lambdaR->beta); 
		
		if(lambdaR->amp == 0.0f) // avoid division by zero 
		{
			SinCos->cos = 1.0f;
			SinCos->sin = 0.0f;
		}
		else SinCosCalc(SinCos, lambdaR); 
}


void VI_pole_flux_estimator(Xalphabeta_amp *lambdaS, Xalphabeta_amp *lambdaR, Xsc *SinCos, Xalphabeta vsab, Xalphabeta isab, float g){	

		lambdaS->alpha += Ts*(vsab.alpha - RS * isab.alpha - g * lambdaS->alpha);
		lambdaS->beta  += Ts*(vsab.beta - RS * isab.beta - g * lambdaS->beta);
		
		lambdaR->alpha 	= INV_KR*(lambdaS->alpha - SIGMALS * isab.alpha);
		lambdaR->beta	 	= INV_KR*(lambdaS->beta - SIGMALS * isab.beta);
		lambdaR->amp	 	= sqrtf(lambdaR->alpha * lambdaR->alpha + lambdaR->beta * lambdaR->beta); 
		
		if(lambdaR->amp == 0.0f) // avoid division by zero 
		{
			SinCos->cos = 1.0f;
			SinCos->sin = 0.0f;
		}
		else SinCosCalc(SinCos, lambdaR);
}


void rotation(Xalphabeta ab, Xsc sc ,Xdq *dq){
	
		float tmp1, tmp2;
		tmp1 = ab.alpha * sc.cos;	
		tmp2 = ab.beta * sc.sin;					
		dq->d = tmp1 + tmp2;							
		tmp1 = ab.alpha * sc.sin;					
		tmp2 = ab.beta * sc.cos;					
		dq->q = -tmp1 + tmp2;
	
}

void inv_rotation(Xdq *dq, Xsc sc, Xalphabeta_amp *ab){
	 
		float tmp1, tmp2;	
    tmp1 = dq->d * sc.cos;		
		tmp2 = dq->q * sc.sin;							
		ab->alpha = tmp1 - tmp2;							
		tmp1 = dq->d * sc.sin;							
		tmp2 = dq->q * sc.cos;							
		ab->beta = tmp1 + tmp2;

}


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) {
	
	  rotation(isab, SinCos_r_elt, isab_rotor);
	 	lambdaR_rotor->d += Ts * (INV_TAUR *(LM * isab_rotor->d - lambdaR_rotor->d));
		lambdaR_rotor->q += Ts * (INV_TAUR *(LM * isab_rotor->q - lambdaR_rotor->q));
		inv_rotation(lambdaR_rotor, SinCos_r_elt_old, lambdaR);
		lambdaR->amp = sqrtf(lambdaR->alpha * lambdaR->alpha + lambdaR->beta * lambdaR->beta);
		
		if(lambdaR->amp == 0.0f) // avoid division by zero 
		{
			SinCos->cos = 1.0f;
			SinCos->sin = 0.0f;
		}
		else SinCosCalc(SinCos, lambdaR);
	
}


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){
	
		Itheta_flux_estimator(lambdaR_rotor, lambdaR_Itheta, isab, isab_rotor, SinCos_r_elt, SinCos_r_elt_old, SinCos_Itheta);
	
		lambdaS_VIpole->alpha += Ts *(vsab.alpha - RS * isab.alpha - g * (lambdaR->alpha - lambdaR_Itheta->alpha));
		lambdaS_VIpole->beta 	+= Ts *(vsab.beta - RS * isab.beta - g * (lambdaR->beta - lambdaR_Itheta->beta));

		lambdaR->alpha 	= INV_KR * (lambdaS_VIpole->alpha - SIGMALS * isab.alpha);
		lambdaR->beta 	= INV_KR * (lambdaS_VIpole->beta - SIGMALS * isab.beta);
		lambdaR->amp 		= sqrtf(lambdaR->alpha * lambdaR->alpha + lambdaR->beta * lambdaR->beta);

		if(lambdaR->amp == 0.0f) // avoid division by zero 
		{
			SinCos->cos = 1.0f;
			SinCos->sin = 0.0f;
		}
		else SinCosCalc(SinCos, lambdaR);
		
}

		
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){
	
		Iw_flux_estimator(lambdaR_Iw_old, lambdaR_Iw, SinCos_Iw, isab, polepairs, omega_r_filtered);
	
		lambdaS_VIpole->alpha += Ts * (vsab.alpha - RS * isab.alpha - g * (lambdaR->alpha - lambdaR_Iw->alpha));
		lambdaS_VIpole->beta	+= Ts * (vsab.beta - RS * isab.beta - g * (lambdaR->beta - lambdaR_Iw->beta));

		lambdaR->alpha 	= INV_KR * (lambdaS_VIpole->alpha - SIGMALS * isab.alpha);
		lambdaR->beta 	= INV_KR * (lambdaS_VIpole->beta - SIGMALS * isab.beta);
		lambdaR->amp 		= sqrtf(lambdaR->alpha * lambdaR->alpha + lambdaR->beta * lambdaR->beta);

		if(lambdaR->amp == 0.0f) // avoid division by zero 
		{
			SinCos->cos = 1.0f;
			SinCos->sin = 0.0f;
		}
		else SinCosCalc(SinCos, lambdaR);

}

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){
	
		esab_est->alpha = vsab.alpha - RS * isab.alpha + g * (isab.alpha - isab_est->alpha);
		esab_est->beta = vsab.beta - RS * isab.beta + g * (isab.beta - isab_est->beta);
		
		lambdaS_Sensorless->alpha += Ts * esab_est->alpha; 
		lambdaS_Sensorless->beta += Ts * esab_est->beta; 
		lambdaS_Sensorless->amp = sqrtf(lambdaS_Sensorless->alpha * lambdaS_Sensorless->alpha + lambdaS_Sensorless->beta * lambdaS_Sensorless->beta);
		
		lambdaR_Sensorless->alpha = INV_KR * (lambdaS_Sensorless->alpha - SIGMALS * isab.alpha);
		lambdaR_Sensorless->beta = INV_KR * (lambdaS_Sensorless->beta - SIGMALS * isab.beta);
		lambdaR_Sensorless->amp = sqrtf(lambdaR_Sensorless->alpha * lambdaR_Sensorless->alpha + lambdaR_Sensorless->beta * lambdaR_Sensorless->beta);
		
		if(lambdaR_Sensorless->amp == 0.0f) // avoid division by zero 
		{
			SinCos_Sensorless->cos = 1.0f;
			SinCos_Sensorless->sin = 0.0f;
			*omega_slip_est =  0.0f;
		}
		else SinCosCalc(SinCos_Sensorless, lambdaR_Sensorless);
		
		if(lambdaR_Sensorless->amp != 0.0f){ // avoid division by zero 
			*omega_slip_est = LM * INV_TAUR * isdq_ref.q / (polepairs * lambdaR_Sensorless->amp);                                                           
		}
		
		if(lambdaS_Sensorless->amp != 0.0f){ // avoid division by zero {
			*omega_elt_est = (lambdaS_Sensorless->alpha * esab_est->beta - lambdaS_Sensorless->beta * esab_est->alpha)/(lambdaS_Sensorless->amp * lambdaS_Sensorless->amp);
		}else{
			*omega_elt_est = 0.0f;
		}
		
		*omega_r_est = (*omega_elt_est - *omega_slip_est * polepairs)/polepairs;
		
		isab_est->alpha = lambdaR_Sensorless->alpha * INV_LM - isdq_ref.q * SinCos_Sensorless->sin;      // si può usare isdq.q nuova misurata ma è rumorosa, dunque si utilizza riferimento isq_ref.q(k-1) trovata all'ISR precedente
		isab_est->beta = lambdaR_Sensorless->beta * INV_LM + isdq_ref.q * SinCos_Sensorless->cos;
		
}

// -------------------------- Fine Funzioni Estimators e Observers -------------------------- //

// Funzione per ricerca binaria
int binary_search(float *array, int size, float target, int increasing) {
	
    int left = 0, right = size - 1;
    int mid;

    while (left <= right) {
        mid = left + (right - left) / 2;

        if (increasing) {
            if (array[mid] <= target && array[mid + 1] > target) {
                return mid;
            }
            if (array[mid] < target) {
                left = mid + 1;
            } else {
                right = mid - 1;
            }
        } else { // Per array decrescente
            if (array[mid] >= target && array[mid + 1] < target) {
                return mid;
            }
            if (array[mid] > target) {
                left = mid + 1;
            } else {
                right = mid - 1;
            }
        }
    }
    return -1; // Non trovato
}

