//#include "types.h" 
#include "DTC.h" 		//header file 'DTC.c'
int mfr32Sqrt(int);	 	//declaration a function to get a square root of number 	



sRelay srelPsi, srelT;

/* ---------------------------------------------------------------------*/
//Relay C-function
/* ---------------------------------------------------------------------*/ 
short c_relay(Frac16 DeltaPsi, sRelay *srel)
{
	short phi;						//declaration of the parameter phi
	if(DeltaPsi >= srel->eps)		//compare the input value DeltaPsi with hysteresis epsilon_Psi
    {									
    	phi = 1;					//if result of compare is true then output value is 1
       	srel->state = phi;			//and the value of output is save in state
    }
    else							//if result of compare is not true then
    {
    	if(DeltaPsi <= -srel->eps)	//compare the input value DeltaPsi with negation of hysteresis -epsilon_Psi	
    	{
    		phi = 0;				//if result of compare is true then output value is 0
    		srel->state = phi;      //and the value of output is save in state  		
   		}
   		else						//if result of compare is not true then
   		{
   			phi = srel->state;		//output value is last value save in state
   		}
    }
    return phi;						//return the output value from a call function 
}

/* ---------------------------------------------------------------------*/
//Relay ASM-function
/* ---------------------------------------------------------------------*/
asm short a_relay(short DeltaPsi, sRelay *srel)
{
		MOVE.W 	Y0,A		//save the first parameter (DeltaPsi) to the register A
		MOVE.W  X:(R2)+,B	//save the second parameter (epsilon_Psi) to the register B
						
		CMP		B,A			//compare the registers A-B (DeltaPsi - epsilon_Psi) and update the CCR registers  
		BGE		SKOK1		//if A>=B (DeltaPsi >= epsilon_Psi) jump to target address skok1
		NOP					//increment the program counter, no operation syntax
		NEG		B			//negate the B register (-epsilon_Psi)
		CMP		B,A			//compare the registers A-B (DeltaPsi - epsilon_Psi) and update the CCR registers
		BLE		SKOK2		//if A<=-B (DeltaPsi <= -epsilon_Psi) jump to target address skok2
		NOP					//increment the program counter, no operation syntax

		MOVE.W	X:(R2),Y0	//output is the last value saved in register R2
		RTS					//return from a call to a subroutine
				
SKOK1:						//target address skok1
		MOVE.W	#1,Y0		//output is 1
		MOVE.W	Y0,X:(R2)	//save the output value 1 to the register R2
		RTS					//return from a call to a subroutine

SKOK2:						//target address skok2
		MOVE.W	#0,Y0		//output is 0
		MOVE.W	Y0,X:(R2)	//save the output value 0 to the register R2
		RTS					//return from a call to a subroutine	
}



/* ---------------------------------------------------------------------*/
//Double Relay C-function
/* ---------------------------------------------------------------------*/
short c_doublerelay(Frac16 DeltaT, sRelay *sdoubrel)			
{
	short tau;							//declaration of the parameter tau
	if(DeltaT > sdoubrel->eps)			//compare the input value DeltaT with hysteresis epsilon_T
    {    
    	tau = 4;						//if result of compare is true then output value is 4
    	sdoubrel->state = tau;          //and the value of output is save in state 		
    }
    else								//if result of compare is not true then	
    {	
    	if(DeltaT  >= 0)				//compare the input value DeltaT with 0
    	{
    		tau = 2;					//if result of compare is true then output value is 2
    		sdoubrel->state = tau;		//and the value of output is save in state
    	}	
    	
    	else							//if result of compare is not true then	
    	{
    		if(DeltaT < -sdoubrel->eps)	//compare the input value DeltaT with negation of hysteresis -epsilon_T
    		{
    			tau = 0;				//if result of compare is true then output value is 0
    			sdoubrel->state = tau;	//and the value of output is save in state		
    		}
    			
    		else						//if result of compare is not true then							
    		{
    			if(DeltaT < 0)			//compare the input value DeltaT with 0
    			{
       			tau = 0;				//if result of compare is true then output value is 0
        		sdoubrel->state = tau;	//and the value of output is save in state
    			}
    			else					//if result of compare is not true then
    			{
    			tau = sdoubrel->state;	//output value is last value save in state	
    			}
    		}
    	}
    }
	return tau;							//return the output value from a call function 
}

/* ---------------------------------------------------------------------*/
//Double Relay AMS-function
/* ---------------------------------------------------------------------*/
asm short a_doublerelay(short DeltaT, sRelay *sdoubrel)
{
		ADDA	#2,SP		//allocation the memory
		MOVE.W 	Y0,A		//save the first parameter (DeltaT) to the register A
		MOVE.W	X:(R2)+,B	//save the second parameter (epsilon_T) to the register B
		MOVE.W	#0,X:(SP)	//save the value 0 to the memory
				
		CMP		B,A			//compare registers A-B (DeltaT - epsilon_T) and update the CCR registers		
		BGT		SKOK1		//if A>B (DeltaT > epsilon_T) jump to target address skok1 
		nop					//increment the program counter, no operation syntax
				
		CMP		X:(SP),A	//compare the register A-(memory space) (DeltaT - 0) and update the CCR registers
		BGE		SKOK2		//if A>=(memory space) (DeltaT >= 0) jump to target address skok2 
		NOP					//increment the program counter, no operation syntax
				
		CMP		X:(SP),A	//compare the register A-memory space (DeltaT - 0) and update the CCR registers
		BLT		SKOK3		//if A<(memory space) (DeltaT < 0) jump to target address skok3
		NOP					//increment the program counter, no operation syntax
				
		NEG		B			//negate the register B (-epsilon_T)
		CMP		A,B			//compare registers A-B (DeltaT - epsilon_T) and update the CCR registers
		BLT		SKOK3		//if A<B (DeltaT < -epsilon_T) jump to target address skok3 
		NOP					//increment the program counter, no operation syntax
				
		MOVE.W	X:(R2),Y0	//output is the last value saved in register R2		
		SUBA	#2,SP		//dealocation the memory
		RTS					//return from a call to a subroutine
		
SKOK1:						//target address skok1
		MOVE.W	#4,Y0		//output is 4
		MOVE.W	Y0,X:(R2)	//save the output value 1 to the register R2
		SUBA	#2,SP		//dealocation the memory
		RTS					//return from a call to a subroutine

SKOK2:						//target address skok2
		MOVE.W	#2,Y0		//outpu is 2
		MOVE.W	Y0,X:(R2)	//save the output value 1 to the register R2
		SUBA	#2,SP		//dealocation the memory
		RTS					//return from a call to a subroutine

SKOK3:						//target address skok3			
		MOVE.W	#0,Y0		//oputput is 2	
		MOVE.W	Y0,X:(R2)	//save the output value 1 to the register R2
		SUBA	#2,SP		//dealocation the memory
		RTS					//return from a call to a subroutine	
}



/* ---------------------------------------------------------------------*/
//Sector Selector C-function
/* ---------------------------------------------------------------------*/
short c_sectorselector(short Psi_s[])
{
	short s;											//declaration of the parameter s
	if(Psi_s[0] < 0)									//compare1 the input value Psi_s[0] with 0
	{ 
		if(abs(Psi_s[0])*0.5 < abs(Psi_s[1])*0.8660)	//FRAC16(0.8660)//if result of compare1 is true then compare2 the absolute value of input parameters
		{
			if(Psi_s[1] < 0)							//if result of compare2 is true then compare3 the input parameter Psi_s[1] with 0
			{
				s = 24;									//if result of compare3 is true then output value is 24
			}
			else										//if result of compare3 is not true then								
			{
			s = 12;										//output value is 12		
			}
		}
		else											//if result of compare2 is not true then	
		{
			s = 18;										//output value is 18
		}
				
	}
	else												//if result of compare1 is not true then
	{
		if(abs(Psi_s[0])*0.5 < abs(Psi_s[1])*0.8660)	//FRAC16(0.8660)//compare4 the absolute value of input parameters
		{
			if(Psi_s[1] < 0)							//if result of compare4 is true then compare5 the input parameter Psi_s[1] with 0
			{
				s = 30;									//if result of compare5 is true then output value is 30		
			}
			else										//if result of compare5 is not true then
			{
				s = 6;									//output value is 6
			}
		}
		else											//if result of compare4 is not true then
		{
			s = 0;										//output value is 0	
		}
	}
	return s;											//return the output value from a call function
}

/* ---------------------------------------------------------------------*/
//Sector Selector ASM-function
/* ---------------------------------------------------------------------*/
asm short a_sectorselector(short Psi_s[])
{
		MOVE.W		X:(R2)+,Y0				//save the first parameter (Psi_s[0]) to the register y0
		MOVE.W		#16384,X0				//save the the fractional value 0.5 to the register x0
		MPY			Y0,X0,A		X:(R2)+,Y1	//multiply two parameters (Psi_s[0]*0.5) store the result to the register A and one parallel move (save the second parameter (Psi_s[1]) to the register y1) and post-increment the address register R2 by one 	
		MOVE.W		#28377,X0				//save the the fractional value 0.8660 to the register x0
		MPY			Y1,X0,B					//multiply two parameters (Psi_s[1]*0.8660) and store the result to the register B	
		
		ABS			A						//absolute value of register A |Psi_s[0]*0.5|
		ABS			B						//absolute value of register B |Psi_s[1]*0.8660|
		MOVE.W		A,X0					//save the value from the register A |Psi_s[0]*0.5| to the register x0
		MOVE.W		#0,C					//save the value 0 to the register C	

		CMP			C,Y0					//compare registers A - C (Psi_s[0] - 0) and update the CCR registers
		BLT			SKOK1					//if A<C (Psi_s[0] < 0) jump to target address skok1
		NOP									//increment the program counter, no operation syntax
		
		CMP			B,X0					//compare registers A-B (|Psi_s[0]|*0.5 - |Psi_s[1]|*0.866) and update the CCR registers
		BLT			SKOK4					//if A<B (|Psi_s[0]|*0.5<|Psi_s[1]|*0.8660) jump to target address skok4
		NOP									//increment the program counter, no operation syntax
		
		MOVE.W		#0,Y0					//output is 0
		RTS									//return from a call to a subroutine

SKOK1:										//target address skok1
		CMP			B,X0					//compare registers A-B (|Psi_s[0]|*0.5 - |Psi_s[1]*0.8660|) and update the CCR registers
		BLT			SKOK2					//if A<B (|Psi_s[0]|*0.5<|Psi_s[1]|*0.8660) jump to target address skok2
		NOP									//increment the program counter, no operation syntax
		MOVE.W		#18,Y0					//output is 18
		RTS									//return from a call to a subroutine
SKOK2:										//target address skok2
		CMP			C,Y1					//compare the register A - (memory space) (Psi_s[1] - 0) and update the CCR registers
		BLT			SKOK3					//if A<(memory space) (Psi_s[1] < 0) jump to target address skok3
		NOP									//increment the program counter, no operation syntax
		MOVE.W		#12,Y0					//output is 12
		RTS									//return from a call to a subroutine
SKOK3:										//target address skok3
		MOVE.W		#24,Y0					//output is 24
		RTS									//return from a call to a subroutine
SKOK4:										//target address skok4
		CMP			C,Y1					//compare the register A - (memory space) (Psi_s[1] - 0) and update the CCR registers
		BLT			SKOK5					//if A<(memory space) (Psi_s[1] < 0) jump to target address skok5
		NOP									//increment the program counter, no operation syntax
		MOVE.W		#6,Y0					//output is 6
		RTS									//return from a call to a subroutine
SKOK5:										//target address skok5
		MOVE.W		#30,Y0					//output is 30
		RTS									//return from a call to a subroutine				
}



/* ---------------------------------------------------------------------*/
//Torque Estimated C-function
/* ---------------------------------------------------------------------*/
Frac32 c_torqueestimated(Frac16 i_s[], Frac16 Psi_s[])
{
	Frac32 T_est;										//declaration of the parameter T_est
	T_est = L_mult((-i_s[0]),Psi_s[1]);					//multiply the parameters -i_s[0]*Psi_s[1] and save the result to the parameter T_est
	T_est = L_mac(T_est, i_s[1], Psi_s[0]);				//multiply the parameters i_s[1]*Psi_s[0] and accumulate the result to the parameter T_est
	return T_est;										//return the output value from a call function
}

/* ---------------------------------------------------------------------*/
//Torque Estimated ASM-function
/* ---------------------------------------------------------------------*/
asm Frac32 a_torqueestimated(Frac16 i_s[],Frac16 Psi_s[])
{
	MOVE.W		X:(R2)+,Y0				//save the first parameter (i_s[0]) to the register y0
 	MOVE.W		X:(R2)+,Y1				//save the second parameter (i_s[1]) to the register y1
	MOVE.W		X:(R2)+,B1				//save the third parameter (Psi_s[0]) to the register B1
	NEG			Y0						//negate the first parameter (-i_s[0])
	MPY			Y1,B1,A		X:(R2)+,X0	//multiply the parameters (i_s[1]*Psi_s[0]) store the result in the register A and one parallel move (save the fourth parameter (Psi_s[1]) to the register x0) and post-increment the address register R2 by one 	
	MAC			Y0,X0,A					//multiply the parameters (-i_s[0]*Psi_s[1]) and add the result to the register A
	RTS									//return from a call to a subroutine			
}



/* ---------------------------------------------------------------------*/
//Module Psi_S C-function
/* ---------------------------------------------------------------------*/
Frac16 c_modulepsis(Frac16 Psi_s[])
{
	Frac32 Psi;														//declaration of the parameter T_est
	Frac16 Psisqrt;													//declaration of the parameter T_est			
	
	//Psi = L_mult(mult_r(FRAC16(0.5), Psi_s[0]), Psi_s[0]);
	//Psi = L_mac(Psi, mult_r(FRAC16(0.5), Psi_s[1]), Psi_s[1]);
	Psi = L_mult(Psi_s[0], Psi_s[0]);								//multiply the parameters Psi_s[0]*Psi_s[0] and save the result to the parameter Psi
	Psi = L_mac(Psi, Psi_s[1], Psi_s[1]);							//multiply the parameters Psi_s[1]*Psi_s[1] and accumulate the result to the parameter Psi
	Psisqrt = mfr32Sqrt(Psi);										//square root of parameter Psi and save result in parameter Psisqrt 
	return Psisqrt;													//return the output value from a call function 
}

/* ---------------------------------------------------------------------*/
//Module Psi_S ASM-function
/* ---------------------------------------------------------------------*/
asm Frac16 a_modulepsis(Frac16 Psi_s[])
{
	MOVE.W		X:(R2)+,Y0				//save the first parameter (Psi_s[0]) to the register y0
 	MPY			Y0,Y0,A		X:(R2)+,Y1	//multiply the parameters (Psi_s[0]*Psi_s[0]->square(Psi_s[0])) store the result in the register A and one parallel move (save the second parameter (Psi_s[1]) to the register y1) and post-increment the address register R2 by one 	
 	MAC			Y1,Y1,A					//multiply the parameters (Psi_s[1]*Psi_s[1]->square(Psi_s[1])) and add the result to the register A
	JSR			mfr32Sqrt				//call assembly function mfr32Sqrt to get square root->|Psi_s|=sqrt(Psi_s[0]^2+Psi_s[1]^2)
	RTS									//return from a call to a subroutine		
}



/* ---------------------------------------------------------------------*/
//Module I C-function
/* ---------------------------------------------------------------------*/
Frac16 c_modulei(Frac16 i_s[])
{
	Frac32 i;														//declaration of the parameter i
	Frac16 isqrt;													//declaration of the parameter isqrt
	
	//i = L_mult(mult_r(FRAC16(0.5), i_s[0]), i_s[0]);
	//i = L_mac(i, mult_r(FRAC16(0.5), i_s[1]), i_s[1]);
	i = L_mult(i_s[0], i_s[0]);										//multiply the parameters i_s[0]*i_s[0] and save the result to the parameter i									
	i = L_mac(i, i_s[1], i_s[1]);									//multiply the parameters i_s[1]*i_s[1] and accumulate the result to the parameter i
	isqrt = mfr32Sqrt(i);											//square root of parameter Psi and save result in parameter isqrt 
	return isqrt;													//return the output value from a call function 
}

/* ---------------------------------------------------------------------*/
//Module I ASM-function
/* ---------------------------------------------------------------------*/
asm Frac16 a_modulei(Frac16 i_s[])
{
	MOVE.W		X:(R2)+,Y0				//save the first parameter (i_s[0]) to the register y0
 	MPY			Y0,Y0,A		X:(R2)+,Y1	//multiply the parameters (i_s[0]*i_s[0]->square(i_s[0])) store the result in the register A and one parallel move (save the second parameter (i_s[1]) to the register y1) and post-increment the address register R2 by one
 	MAC			Y1,Y1,A					//multiply the parameters (i_s[1]*i_s[1]->square(i_s[1])) and add the result to the register A
	JSR			mfr32Sqrt				//call assembly function mfr32Sqrt to get square root->|i|=sqrt(i_s[0]^2+i_s[1]^2)
	RTS									//return from a call to a subroutine
}

/* ---------------------------------------------------------------------*/
//Switching Table C-function
/* ---------------------------------------------------------------------*/
void c_switchingtable(short suma_s_x,short *u_a,short *u_b,short *u_c)
{
	const char swtable[36]={4,5,0,1,2,3,5,1,7,3,6,2,1,3,0,2,4,6,3,2,7,6,5,4,2,6,0,4,1,5,6,4,7,5,3,1};	//declaration and initialization the array swtable
	short state = 0;										//declaration and initialization the parameter state										
	state = swtable[suma_s_x];								//save the required value to the parameter state 
	if(state == 0)											//compare the parameter state with 0
			{
				*u_a = 0;									//if result of compare is true then output values of voltages are 	
				*u_b = 0;
				*u_c = 0;		
			}
			else											//if result of compare is not true then	
			{
				if(state == 1)								//compare the parameter state with 1
				{
				*u_a = 350;									//if result of compare is true then output values of voltages are 	
				*u_b = 0;
				*u_c = 0;	
				}
				else										//if result of compare is not true then
				{
					if(state == 2)							//compare the parameter state with 2
					{
						*u_a = 0;							//if result of compare is true then output values of voltages are 	
						*u_b = 350;
						*u_c = 0;	
					}
					else									//if result of compare is not true then
					{
						if(state == 3)						//compare the parameter state with 3
						{
							*u_a = 350;						//if result of compare is true then output values of voltages are 							
							*u_b = 350;
							*u_c = 0;	
						}
						else								//if result of compare is not true then							
						{
							if(state == 4)					//compare the parameter state with 4
								{
								*u_a = 0;					//if result of compare is true then output values of voltages are 							
								*u_b = 0;
								*u_c = 350;	
								}
							else							//if result of compare is not true then
							{
								if(state == 5)				//compare the parameter state with 5
								{
									*u_a = 350;				//if result of compare is true then output values of voltages are			
									*u_b = 0;
									*u_c = 350;	
								}
								else						//if result of compare is not true then
								{
									if(state == 6)			//compare the parameter state with 6
									{
										*u_a = 0;			//if result of compare is true then output values of voltages are
										*u_b = 350;
										*u_c = 350;	
									}
									else					//if result of compare is not true then 
									{
										*u_a = 350;			//output values of voltages are	
										*u_b = 350;
										*u_c = 350;	
									}
								 }
							 }
						 }
					 }
				 }
			}
}
