#include "SBLDC.h"

#define	MZ_IN_MUX_MASK	2
#define U_IN_MUX_MASK	1

void SBLDC (MZ_type *MZ, MZ_type *MZ_reg, invSC_MZ_type invSC_MZ, U_type *U, U_type *U_reg, invSC_U_type invSC_U, SC_LL_type SC_LL,
		TSSC_TS_type TSSC_TS, RSC_R_type RSC_R, CESC_CE_type CESC_CE, SC_JJ_type SC_JJ,
		TSSC_TS2pi_type TSSC_TS2pi, THETA_type *theta, THETA_type *theta_reg, VELOCITY_type *velocity,
		VELOCITY_type *velocity_reg, CURRENT_type *current, CURRENT_type *current_reg, char *mux)
{
	/*State variables definition*/
	static VAR_type I_old = 0, W_old = 0, THETA_old = 0;
	VAR_type I = 0, W = 0, THETA = 0;

	MZ_type MZ_var;
	U_type U_var;

	/*input mux*/
	if(!((*mux) & (char)MZ_IN_MUX_MASK))
		{
			MZ_var = *MZ;
		}
		else
		{
			MZ_var = *MZ_reg;
		}

	if(!((*mux) & (char)U_IN_MUX_MASK))
		{
			U_var = *U;
		}
	else
		{
			U_var = *U_reg;
		}


	/*Calculate three equations I, W, THETA*/
	/*Current I*/
	I = (((U_var * invSC_U) - (W_old * CESC_CE) - (I_old * RSC_R)) * TSSC_TS * SC_LL) + I_old;

	/*Velocity W*/
	W = (((I_old * CESC_CE) - (MZ_var * invSC_MZ)) * SC_JJ * TSSC_TS) + W_old;

	/*Angle THETA*/
	THETA = ((W_old * TSSC_TS2pi) + THETA_old);

	/*set up the outputs*/
	*theta = THETA_old;
	*theta_reg = THETA_old;
	*velocity = W_old;
	*velocity_reg = W_old;
	*current = I_old;
	*current_reg = I_old;

	/*update state variables*/
	I_old = I;
	W_old = W;
	THETA_old = THETA;
}
