#include "ACIM_AB.h"


void ACIM_AB(s15f14_type * p1, s25f24_type * p2, s25f24_type * p3, s25f24_type * p4, s25f24_type * p5,
		s25f24_type * p6, s25f24_type * p7, s25f20_type * p8, s22f16_type * p9, s25f24_type * p10,
		s18f16_type * Mz_in, s18f16_type * Mz_reg, s22f14_type * U_A_in, s22f14_type * U_A_reg,
		s22f14_type * U_B_in, s22f14_type * U_B_reg, s25f24_type * W_out, s25f24_type * W_reg,
		s25f24_type * I_A_out,	s25f24_type * I_A_reg, s25f24_type * I_B_out, s25f24_type * I_B_reg,
		s25f24_type * PSI_A_out, s25f24_type * PSI_A_reg, s25f24_type * PSI_B_out,
		s25f24_type * PSI_B_reg, char * IO_Mux_reg)
{
	/*state vars*/
	s25f24_type I_A, I_B, PSI_A, PSI_B, W;
	static s25f24_type I_A_old = 0, I_B_old = 0, PSI_A_old = 0, PSI_B_old = 0, W_old = 0;
	/*other vars*/
	s25f24_type PsiBW, PsiAW;
	s22f14_type U_A, U_B;
	s18f16_type Mz;



	/*sub multi*/
	PsiBW = PSI_B_old * W_old;
	PsiAW = PSI_A_old * W_old;

	/*input mux*/
	if( (*IO_Mux_reg) & (char)U_IN_MUX_MASK)
	{
		U_A = *U_A_reg;
		U_B = *U_B_reg;
	}
	else
	{
		U_A = *U_A_in;
		U_B = *U_B_in;
	}

	if(!((*IO_Mux_reg) & (char)MZ_IN_MUX_MASK))
	{
		Mz = *Mz_in;
	}
	else
	{
		Mz = *Mz_reg;
	}



	/*ACIM equations*/

	I_A = ((s25f24_type)((s25f24_type)(U_A * *p1) - (s25f24_type)(I_A_old * *p4) + (s25f24_type)(*p2 * PSI_A_old) + (s25f24_type)(*p3 * PsiBW)) * *p5) + I_A_old;
	I_B = ((s25f24_type)((s25f24_type)(U_B * *p1) - (s25f24_type)(I_B_old * *p4) + (s25f24_type)(*p2 * PSI_B_old) - (s25f24_type)(*p3 * PsiAW)) * *p5) + I_B_old;
	PSI_A = ((s25f24_type)((s25f24_type)(I_A_old * *p6) - (PsiBW) - (s25f24_type)(PSI_A_old * *p7)) * *p5) + PSI_A_old;
	PSI_B = ((s25f24_type)((s25f24_type)(I_B_old * *p6) + (PsiAW) + (s25f24_type)(PSI_B_old * *p7)) * *p5) + PSI_B_old;
	W = ((s25f24_type)((((s25f24_type)(I_B_old * PSI_A_old) - (s25f24_type)(I_A_old * PSI_B_old)) * *p8) - (Mz * *p9)) * *p10) + W_old;

	/*set the outputs*/
	*I_A_out = I_A_old;
	*I_A_reg = I_A_old;
	*I_B_out = I_B_old;
	*I_B_reg = I_B_old;
	*PSI_A_out = PSI_A_old;
	*PSI_A_reg = PSI_A_old;
	*PSI_B_out = PSI_B_old;
	*PSI_B_reg = PSI_B_old;
	*W_out = W_old;
	*W_reg = W_old;

	/*state vars update*/
	I_A_old = I_A;
	I_B_old = I_B;
	PSI_A_old = PSI_A;
	PSI_B_old = PSI_B;
	W_old = W;
}

