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


mc_sPhase Psi_S, Psi_Sn, i_S, u_S, u_S_lim;

/* ---------------------------------------------------------------------*/
/* FUNCTION rotate_Psi_S												*/
/*  																	*/
/*  This function performs a rotation of the stator flux				*/
/*  vector by a given angle alpha										*/
/*  																	*/
/*	input - Psi_S, alpha												*/
/*	output - Psi_Sn														*/
/*  																	*/
/*  It uses multiplication of the stato flux vector with the 			*/
/*  transformation matrix												*/
/*						[cos alpha		sin alpha]						*/
/*    Psi_Sn = Psi_S *  [	 					 ]						*/
/*						[-sin alpha		cos alpha]						*/
/*	The trigonometric functions are approximated with the simple        */
/*  formulas															*/
/*	  cos alpha = 1 - K_COS * alpha^2									*/
/*	  sin alpha = K_SIN * alpha		 									*/
/* ---------------------------------------------------------------------*/
/* ---------------------------------------------------------------------*/
//Rotate_Psi_S  C-function
/* ---------------------------------------------------------------------*/
void	rotate_Psi_S(mc_sPhase 	*Psi_S, mc_sPhase *Psi_Sn, Frac16 alpha)
{
	Frac16 k1_a_a;
	Frac16 k2_a; 
		
	k1_a_a = __mult_r(__mult_r(K_COS,alpha),alpha);			//K_COS * alpha^2
	k2_a = __mult_r(K_SIN,alpha);							//K_SIN * alpha
	
	Psi_Sn->alpha = __msu_r(L_msu(L_deposit_h(Psi_S->alpha),Psi_S->beta,k2_a),Psi_S->alpha,(k1_a_a));	//Psi_S(k+1)[1] = Psi_S(k)[1] - K_COS * alpha^2 * Psi_S(k)[1] - K_SIN * alpha * Psi_S(k)[2]
	Psi_Sn->beta = __msu_r(L_mac(L_deposit_h(Psi_S->beta),Psi_S->alpha,k2_a),Psi_S->beta,(k1_a_a));		//Psi_S(k+1)[2] = Psi_S(k)[2] - K_COS * alpha^2 * Psi_S(k)[2] - K_SIN * alpha * Psi_S(k)[1]
}

/* ---------------------------------------------------------------------*/
//Rotate_Psi_S  ASM-function
/* ---------------------------------------------------------------------*/
asm void	a_rotate_Psi_S(mc_sPhase *Psi_S, mc_sPhase *Psi_Sn, Frac16 alpha)
{
	MOVE.W  #K_COS,X0				//save K_COS in register X0
	MPYR	Y0,X0,A		X:(R2)+,B	//K_COS * alpha || read Psi_S.alpha
	NOP								//increment the program counter, no operation syntax
	MPYR	-A1,Y0,Y1				//cos(alpha)-1 = - K_COS * alpha^2 
	
	MOVE.W  #K_SIN,X0				//save K_SIN in register X0
	MPYR	Y0,X0,Y0				//sin(alpha) = K_SIN * alpha
	
	CLR.W	N						//set a word in N register to zero
	MOVE.W	B1,X0					//move Psi_S.alpha to register X0
	MAC		Y1,B1,B		X:(R2)+N,A	//Psi_S.alpha -  Psi_S.alpha * K_COS * alpha^2 || read Psi_S.beta
	MACR	-A1,Y0,B				//Psi_S.alpha -  Psi_S.alpha * K_COS * alpha^2 - Psi_S.beta * K_SIN * alpha
	TFR		B,A			X:(R2)+N,B	//move reslut to A  || read Psi_S.beta

	MAC		Y1,B1,B		A1,X:(R3)+	//Psi_S.beta * K_SIN * alpha + Psi_S.beta || save Psi_Sn.alpha
	MACR	X0,Y0,B					//-Psi_S.beta * K_COS * alpha^2 + Psi_S.beta + Psi_S.alpha * K_SIN * alpha
	MOVE.W	B1,X:(R3)				//save Psi_Sn.beta
	
	RTS								//return from a call to a subroutine
}



/* ---------------------------------------------------------------------*/
/* FUNCTION adjust_module_Psi_S											*/
/*  																	*/
/*  This function changes the magnitude of the actual stator flux vector*/
/*  to the value specified by the user.									*/
/*  																	*/
/*	input - Psi_Sn, Psi_S_length_req									*/
/*	output - Psi_Sn														*/
/*  																	*/
/*	It computes the length of the stator flux vector					*/
/*		|Psi_Sn| = sqrt(Psi_Sn.alpha^2 + Psi_Sn.beta^2)					*/
/*	and than it changes its length to the required one					*/
/*  			 Psi_Sn													*/
/*		Psi_Sn = ------ * Psi_S_length_req								*/ 
/*  			|Psi_Sn|												*/
/* ---------------------------------------------------------------------*/
/* ---------------------------------------------------------------------*/
//Adjust_module_Psi_S  C-function
/* ---------------------------------------------------------------------*/
void adjust_module_Psi_S(mc_sPhase *Psi_Sn, Frac16 Psi_S_length_req, Frac16 I)
{
	Frac16 Psi_S_length;
		
	Psi_S_length = mfr32Sqrt(L_mac(L_mult(Psi_Sn->alpha,Psi_Sn->alpha),Psi_Sn->beta,Psi_Sn->beta));	//|Psi_S(k+1)| = sqrt(Psi_S(k+1)[1]^2 + Psi_S(k+1)[2]^2)
	
	if (I==1)						
	{	
		Psi_Sn->alpha = __mult( div_s4q( Psi_Sn->alpha, Psi_S_length ),Psi_S_length_req);			//Psi_S'(k+1)[1] = (Psi_S(k+1)[1] / |Psi_S(k+1)|) * Psi_S_length_req
	
		if(Psi_S_length == 0)		//limit the output Psi_Sn->beta at the begining in time T=0s
		{
			Psi_Sn->beta = 0;		
		}
		else
		{
			Psi_Sn->beta = __mult( div_s4q( Psi_Sn->beta, Psi_S_length ),Psi_S_length_req);			//Psi_S'(k+1)[2] = (Psi_S(k+1)[2] / |Psi_S(k+1)|) * Psi_S_length_req			
		}
	}
}

/* ---------------------------------------------------------------------*/
//Adjust_module_Psi_S  ASM-function
/* ---------------------------------------------------------------------*/
asm void a_adjust_module_Psi_S(mc_sPhase *Psi_Sn, Frac16 Psi_S_length_req, Frac16 I)
{
		MOVE.W	#-1,N							//prepared to modify index register
		MOVE.W	Y1,C							//save I in register C
		MOVE.W	Y0,Y1							//save 	Psi_S_length_req for later use
		MOVE.W  X:(R2)+,Y0						//read Psi_Sn.alpha
		MPY		Y0,Y0,A		X:(R2)+N,Y0			//Psi_Sn.alpha^2 and read Psi_Sn.beta
		MAC		Y0,Y0,A							//Psi_Sn.alpha^2 + Psi_Sn.beta^2
		JSR		mfr32Sqrt						//Psi_S_length = sqrt(Psi_Sn.alpha^2 + Psi_Sn.beta^2)
	
		CMP		#1,C							//compare I with 1 and set carry
		BNE		OUT								//if I is not equal with 1 jump to target address OUT
		MOVE.W	X:(R2),A						//read Psi_Sn.alpha
		ABS		A			X:(R2)+,B			//abs(Psi_Sn.alpha) and read Psi_Sn.alpha
		TST		B								//compare register B with zero and set the condition codes accordingly (set N flag)
		BFCLR	#$0001,SR						//clear C flag for the first iteration of division
		REP		#16								//repeat the next istruction 16 times	
		DIV		Y0,A							//divide  abs(Psi_Sn.alpha)/Psi_S_length
		BGE		DONE1							//if correct result is positive, then jump to target address DONE1
		NEG		A								//negate the result if Psi_Sn.alpha was negative
DONE1:											//target address DONE1
		MOVE.W 	A0,X0							//result of the division
		MPYR	Y1,X0,X0						//Psi_Sn.alpha/Psi_S_length*Psi_S_length_req
	
		MOVE.W	X:(R2),A						//read Psi_Sn.beta
		ABS		A			X:(R2)+N,B			//abs(Psi_Sn.beta) and read Psi_Sn.beta
		TST		B			X0,X:(R2)+			//compare register B with zero and set the condition codes accordingly (set N flag and save modified Psi_Sn.alpha)
		BFCLR	#$0001,SR						//clear C flag for the first iteration of division
		REP		#16								//repeat the next istruction 16 times	
		DIV		Y0,A							//divide  abs(Psi_Sn.beta)/Psi_S_length
		BGE		DONE2							//if correct result is positive, then jump to target address DONE2
		NEG		A								//negate the result if Psi_Sn.beta was negative
DONE2:											//target address DONE2	
		MOVE.W 	A0,X0							//result of the division
		MPYR	Y1,X0,X0						//Psi_Sn.beta/Psi_S_length*Psi_S_length_req
		MOVE.W	#0,D							//save 0 in register D
		CMP		D,Y0							//compare Psi_S_length with 0 and set carry
		BEQ		OUT								//if Psi_S_length = 0 jump to target address OUT
		MOVE.W	X0,X:(R2)+N						//save modified Psi_Sn.beta
OUT:											//target address OUT
		RTS										//return from a call to a subroutine
}



/* ---------------------------------------------------------------------*/
/* FUNCTION compute_stator_voltage										*/
/*  																	*/
/*  This function computes the required stator voltage vector. If the	*/
/*  voltage exceedes its limit then it is limited. The direction of the */
/*	voltage vector is preserved. This algorithm do not prefere the		*/
/*	flux excitation to torque generation.								*/
/*	input - Psi_Sn, Psi_S, i_S 											*/
/*	output - u_S														*/
/*  																	*/
/*	It computes the stator voltage vector using equation				*/
/*  	u_S = (Psi_Sn - Psi_S)/Ts + i_S * R_S							*/
/*	The value is stored as a 32 bit number. Then the squared length  	*/
/*	of the vector is computed 											*/
/* 		u_S_length_sq = u_S_a^2 + u_S_b^2								*/
/*  and compared with squared stator voltage limit U_S_LENGTH_MAX_SQ.	*/
/*  If the limit U_S_LENGTH_MAX is exceeded then the length of the 		*/
/*	voltage vector is computed  										*/
/*		u_S_length = sqrt(u_S_length_sq)								*/
/*	and voltage vector is limited to U_S_LENGTH_MAX						*/
/*  		      u_S													*/
/*		u_S = ----------- * U_S_LENGTH_MAX								*/ 
/*  		  u_S_length												*/
/*	If the limit U_S_LENGTH_MAX is not exceeded then the values of the 	*/
/*	voltage vector are shifted left to finish normalization.			*/
/* ---------------------------------------------------------------------*/
/* ---------------------------------------------------------------------*/
//Compute_stator_voltage  C-function
/* ---------------------------------------------------------------------*/
void compute_stator_voltage(mc_sPhase *Psi_Sn, mc_sPhase *Psi_S,mc_sPhase *i_S, mc_sPhase *u_S)
{
	Frac16 u_S_length;
	Frac32 l_u_S_alpha, l_u_S_beta, u_S_length_sq;

	l_u_S_alpha =  L_mac(L_mult(__sub(Psi_Sn->alpha, Psi_S->alpha),SAMPLE_FREQ),i_S->alpha,R_S);	//u_S[1] = (Psi_S'(k+1)[1] - Psi_S(k)[1]) / Ts + i_S * R_S
	l_u_S_beta  =  L_mac(L_mult(__sub(Psi_Sn->beta, Psi_S->beta),SAMPLE_FREQ),i_S->beta,R_S);		//u_S[2] = (Psi_S'(k+1)[2] - Psi_S(k)[2]) / Ts + i_S * R_S
			
	u_S->alpha = __extract_h(l_u_S_alpha); 	//extracts the 16 MSBs of a 32-bit integer or fractional value,returns a 16-bit value of u_S[1]
	u_S->beta  = __extract_h(l_u_S_beta);	//extracts the 16 MSBs of a 32-bit integer or fractional value,returns a 16-bit value of u_S[2]
			
	u_S_length_sq = L_mac(L_mult(u_S->alpha,u_S->alpha),u_S->beta,u_S->beta);	//u_S = u_S[1]^2 + u_S[2]^2
	
	if ( u_S_length_sq > U_S_LENGTH_MAX_SQ )
	{
		u_S_length = mfr32Sqrt(u_S_length_sq);									//|u_S| = sqrt(u_S)
		u_S->alpha = __mult_r(div_s4q( u_S->alpha, u_S_length ),U_S_LENGTH_MAX);//u_S[1] = (u_S[1] / |u_S|) * U_max 
		u_S->beta  = __mult_r(div_s4q( u_S->beta, u_S_length ),U_S_LENGTH_MAX);	//u_S[2] = (u_S[2] / |u_S|) * U_max
	}
	else
	{
		u_S->alpha = __extract_h(L_shl(l_u_S_alpha,SHIFT_U_S));		//extracts the 16 MSBs of a arithmetic shift of 32-bit integer value or fractional value,returns a 16-bit value of u_S[1]
		u_S->beta  = __extract_h(L_shl(l_u_S_beta,SHIFT_U_S));		//extracts the 16 MSBs of a arithmetic shift of 32-bit integer value or fractional value,returns a 16-bit value of u_S[2]
	}
}

/* ---------------------------------------------------------------------*/
//Compute_stator_voltage  ASM-function
/* ---------------------------------------------------------------------*/
asm void a_compute_stator_voltage(mc_sPhase *Psi_Sn, mc_sPhase *i_S, mc_sPhase *Psi_S, mc_sPhase *u_S)
{
	MOVE.W	#-1,N							//prepared to modify index registers
	MOVE.W	X:(R2)+,A						//read Psi_Sn.alpha
	MOVE.W	X:(R4)+,Y0						//read Psi_S.alpha
	SUB		Y0,A		X:(R2)+N,B			//Psi_Sn.alpha - Psi_S.alpha || read Psi_Sn.beta
	MOVE.W	A,Y0							//limit if necessary
	MOVE.W	#SAMPLE_FREQ,Y1					//save value SAMPLE_FREQ in register Y1	
	MPY		Y0,Y1,A		X:(R3)+,Y0			//(Psi_Sn.alpha-Psi_S.alpha)*SAMPLE_FREQ || read i_S.alpha	
	MOVE.W	#R_S,X0							//save value R_S in register X0
	MAC		Y0,X0,A		//X:(R4)+N,Y0		//(Psi_Sn.alpha-Psi_S.alpha)*SAMPLE_FREQ + R_S * i_S.alpha
	MOVE.W	X:(R4)+N,Y0						//read Psi_S.beta
											//A contains now long u_S.alpha
	SUB		Y0,B		A1,X:(R1)+			//Psi_Sn.beta - Psi_S.beta || save higher part of u_S.alpha 
	MOVE.W	B,Y0							//limit if necessary
	MPY		Y0,Y1,B		X:(R3)+N,Y0			//(Psi_Sn.beta-Psi_S.beta)*SAMPLE_FREQ || read i_S.beta	
	MAC		Y0,X0,B							//(Psi_Sn.beta-Psi_S.beta)*SAMPLE_FREQ + R_S * i_S.beta	
		
	MOVE.W	A0,X0							//save lower part of u_S.alpha
	MOVE.W	B0,Y1							//save lower part of u_S.beta
	
	MOVE.W	A1,Y0							//read higher part of u_S.alpha	
	MPY		Y0,Y0,A		B1,X:(R1)+N			//u_S.alpha^2 || save higher part of u_S.beta
	MOVE.W	B1,Y0							//read higher part of u_S.beta
	MAC		Y0,Y0,A							//u_S_length_sq = u_S.alpha^2 +  u_S.beta^2 
	
	MOVE.W	#U_S_LENGTH_MAX_SQ_HI,B			//collect two 16-bit values in 32-bit value
	MOVE.W	#U_S_LENGTH_MAX_SQ_LO,B0		//save in register B
	CMP		A,B								//u_S_length_sq < U_S_LENGTH_MAX_SQ_HI
	BHI		NOLIMIT							//branch if yes
	
	JSR		mfr32Sqrt						//Y0 = u_S_length = sqrt(u_S.alpha^2 + u_S.beta^2)
	MOVE.W	X:(R1),A						//read u_S.alpha
	ABS		A			X:(R1)+,B			//abs(u_S.alpha) || read u_S.alpha
	TST		B								//compare register B with zero and set the condition codes accordingly (set N flag)
	BFCLR	#$0001,SR						//clear C flag for the first iteration of division
	REP		#16								//repeat the next istruction 16 times
	DIV		Y0,A							//divide  abs(u_S.alpha)/u_S_length
	BGE		DONE1							//if correct result is positive, then jump to target address DONE1
	NEG		A								//negate the result if u_S.alpha was negative
DONE1:										//target address DONE1
	MOVE.W 	A0,X0							//result of the division
	MOVE.W	#U_S_LENGTH_MAX,Y1				//save value U_S_LENGTH_MAX in register Y1
	MPYR	Y1,X0,X0						//X0 = computed u_S.alpha = u_S.alpha / u_S_length * U_S_LENGTH_MAX
	
	MOVE.W	X:(R1),A						//read u_S.beta
	ABS		A			X:(R1)+N,B			//abs(u_S.beta) || read u_S.beta
	TST		B			X0,X:(R1)+			//compare register B with zero and set the condition codes accordingly (set N flag and save computed u_S.alpha)
	BFCLR	#$0001,SR						//clear C flag for the first iteration of division
	REP		#16								//repeat the next istruction 16 times
	DIV		Y0,A							//divide  abs(u_S.beta)/u_S_length
	BGE		DONE2							//if correct result is positive, then jump to target address DONE2
	NEG		A								//negate the result if u_S.beta was negative
DONE2:										//target address DONE2
	MOVE.W 	A0,X0							//result of the division
	MPYR	Y1,X0,B							//B = computed u_S.beta = u_S.beta / u_S_length * U_S_LENGTH_MAX
	MOVE.W	B,X:(R1)+N						//save computed u_S.beta
	RTS										//return from a call to a subroutine
	 
NOLIMIT:									//target address NOLIMIT

	MOVE.W	X:(R1)+,A						//save higher part of u_S.alpha in register A
	MOVE.W	X0,A0							//restore long u_S.alpha in A
	MOVE.W	X:(R1)+N,B						//save higher part of u_S.beta in register B
	MOVE.W	Y1,B0							//restore long u_S.beta in B

	REP		#SHIFT_U_S						//repeat the next istruction SHIFT_U_S times
	ASL		A								//shift u_S.alpha SHIFT_U_S times to the left

	REP		#SHIFT_U_S						//repeat the next istruction SHIFT_U_S times
	ASL		B								//shift u_S.beta SHIFT_U_S times to the left

	MOVE.W	A,X:(R1)+						//save computed u_S.alpha
	MOVE.W	B,X:(R1)+N						//save computed u_S.beta
		
	RTS										//return from a call to a subroutine
}


/* ---------------------------------------------------------------------*/
//FUNCTION select_alpha
/* ---------------------------------------------------------------------*/
/* ---------------------------------------------------------------------*/
//Select_alpha  C-function
/* ---------------------------------------------------------------------*/
Frac16 select_alpha(Frac16 omega_est, Frac16 omega_req, Frac16 Deltaalpha, Frac16 I)
{
	Frac16 alpha;
	if(I == 1)
	{
		alpha = add(mult_r(omega_req, PP_TS),Deltaalpha);	//alpha = omega_req * PP_TS + delta_alpha
		return alpha;
	}
	else
	{
		alpha = mult_r(omega_est, PP_TS);					//alpha = omega_est * PP_TS
		return alpha;
	}
}

/* ---------------------------------------------------------------------*/
//Select_alpha  ASM-function
/* ---------------------------------------------------------------------*/
asm short a_select_alpha(Frac16 omega_est, Frac16 omega_req, Frac16 Deltaalpha, Frac16 I)
{
		MOVE.W 	#PP_TS,X0					//save value PP_TS in register X0
		CMP		#1,B						//compare 'B==1' and set a carry 
		BEQ		ALPHA1						//if B==1 jump to target address ALPHA1
		MPYR	Y0,X0,Y0					//if B is not equal alpha = omega_est*PP_TS
		RTS									//return from a call to a subroutine
ALPHA1:										//target address ALPHA1
		MPYR	Y1,X0,Y1					//omega_req*PP_TS
		ADD		A,Y1						//alpha = omega_req*PP_TS + Deltaalpha
		MOVE.W	Y1,Y0						//save alpha
		RTS									//return from a call to a subroutine
}



/* ---------------------------------------------------------------------*/
//FUNCTION test_current
/* ---------------------------------------------------------------------*/
/* ---------------------------------------------------------------------*/
//Test_current  C-function
/* ---------------------------------------------------------------------*/
Frac16 test_current(mc_sPhase *i_S, Frac16 I_max)
{
	Frac16 i_s;
	i_s = mfr32Sqrt(L_mac(L_mult(i_S->alpha,i_S->alpha),i_S->beta,i_S->beta));	//|i_S|=sqrt(i_S[1]^2 + i_S[2]^2)
	if(i_s < I_max)
	{
		return 1;
	}
	else
	{
		return 0;
	}
}

/* ---------------------------------------------------------------------*/
//Test_current  ASM-function
/* ---------------------------------------------------------------------*/
asm short a_test_current(mc_sPhase *i_S, Frac16 I_max)
{
		MOVE.W	#-1,N							//prepared to modify index register
		MOVE.W	Y0,C							//save |I_max| in register C
		MOVE.W  X:(R2)+,Y0						//read Psi_Sn.alpha and update
		MPY		Y0,Y0,A		X:(R2)+N,Y0			//i_S.alpha^2, read i_S.beta and post-update
		MAC		Y0,Y0,A							//i_S.alpha^2 + i_S.beta^2
		JSR		mfr32Sqrt						//|i_s| = sqrt(i_S.alpha^2 + i_S.beta^2)
		
		CMP		C,Y0							//compare |i_s| with |I_max|
		BLT		SKOK							//if |i_s|<|I_max| jump to target address SKOK
		MOVE.W	#0,Y0							//else output I=0
		RTS										//return from a call to a subroutine								
SKOK:											//target address SKOK
		MOVE.W	#1,Y0							//output I=1
		RTS										//return from a call to a subroutine
}



/* ---------------------------------------------------------------------*/
//FUNCTION limit_stator_voltage
/* ---------------------------------------------------------------------*/
/* ---------------------------------------------------------------------*/
//Limit_stator_voltage  C-function
/* ---------------------------------------------------------------------*/
void limit_stator_voltage(mc_sPhase *u_S, Frac16 U_max)
{
	Frac16 u_s;
	
	u_s = mfr32Sqrt(L_mac(L_mult(u_S->alpha,u_S->alpha),u_S->beta,u_S->beta));	//|u_S| = sqrt(u_S[1]^2 + u_S[2]^2)
	
	if(u_s > U_max)
	{
		u_S->alpha = div_s4q(__mult(u_S->alpha, U_max),u_s);					//u_S[1] = u_S[1] * U_max / |u_S| 
		u_S->beta = div_s4q(__mult(u_S->beta, U_max),u_s);						//u_S[2] = u_S[2] * U_max / |u_S|
	}
}

/* ---------------------------------------------------------------------*/
//Limit_stator_voltage  ASM-function
/* ---------------------------------------------------------------------*/
asm void a_limit_stator_voltage(mc_sPhase *u_S, Frac16 U_max)
{
		MOVE.W	#-1,N							//prepared to modify index register
		MOVE.W	Y0,Y1							//save |U_max| in register C
		MOVE.W  X:(R2)+,Y0						//read u_S.alpha and update
		MPY		Y0,Y0,A		X:(R2)+N,Y0			//u_S.alpha^2, read u_S.beta and post-update
		MAC		Y0,Y0,A							//u_S.alpha^2 + u_S.beta^2
		JSR		mfr32Sqrt						//u_S = sqrt(u_S.alpha^2 + u_S.beta^2)
		
		CMP		Y1,Y0							//compare |u_s| with |U_max|
		BLE		OUT								//if |u_s|<=|U_max| jump to target address OUT

		MOVE.W	X:(R2),A						//read u_S.alpha
		ABS		A			X:(R2)+,B			//abs(u_S.alpha) and read u_S.alpha
		TST		B								//compare register B with zero and set the condition codes accordingly (set N flag)
		BFCLR	#$0001,SR						//clear C flag for the first iteration of division
		REP		#16								//repeat the next istruction 16 times	
		DIV		Y0,A							//divide  u_S.alpha/|u_s|
		BGE		DONE1							//if correct result is positive, then jump to target address DONE1
		NEG		A								//else negate the result if u_S.alpha was negative
DONE1:											//target address DONE1
		MOVE.W 	A0,X0							//result of the division
		MPYR	Y1,X0,X0						//u_S.alpha/|u_S|*|U_max|
		
		MOVE.W	X:(R2),A						//read u_S.beta
		ABS		A			X:(R2)+N,B			//abs(u_S.beta) and read u_S.beta
		TST		B			X0,X:(R2)+			//compare register B with zero and set the condition codes accordingly (set N flag and save modified u_S.alpha)
		BFCLR	#$0001,SR						//clear C flag for the first iteration of division
		REP		#16								//repeat the next istruction 16 times
		DIV		Y0,A							//divide  abs(u_S.beta)/|u_S|
		BGE		DONE2							//if correct result is positive, then jump to target address DONE2
		NEG		A								//else negate the result if u_S.beta was negative
DONE2:											//target address DONE2
		MOVE.W 	A0,X0							//result of the division
		MPYR	Y1,X0,X0						//u_S.beta/|u_S|*|U_max|
		MOVE.W	X0,X:(R2)+N						//save modified u_S.beta
OUT:											//target address OUT
		RTS										//return from a call to a subroutine	
}