#define		M 4	//liczba wierszy
#define		N 4	//liczba kolumn
#define 	K 4 // liczba kolumn 2-geij macierzy ->tyle kolmn bedzie miala macierz wynikowa
#define 	n 3 //liczba kolumni wierszy w macierzy do odwracania

#include <def21161.h>
#include "adds_21161_ezkit.h"
#include <asm_sprt.h>
#include	"macros.h"

//.extern			_vector_X;
//.extern			_vector_Y;
.extern			_matrix_A_PM;
//---------
.extern			_macierz,_macierz1;
.extern			_mat_a;
.extern 		_vector_X;
.extern			_vector_Y;
.extern			_i,_j;
.extern 		_mat_X;
.extern			_mat_Y;
.extern			_pf;
.extern			_swc;
.extern			_swr;
.extern			_matrix_A_inv;
.extern 		_det;





.segment /pm seg_pmco;
//###########################################################################
//# pusta procedura - w celu obliczenia czasu wejscia i wyjscia z procedury #
//###########################################################################

//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//#############################################################

/* Program memory code */
.section/PM seg_pmco;

/***  Procedura mnozenia macierzy przez wektor *****/
_mat_x_vec:
.global _mat_x_vec;
	leaf_entry;
	I0=_mat_a;	B0=I0;	L0=@_mat_a;	M0=1;//pobranie adr i dlugosci mac mat_a
	I8=_vector_X;	B8=I8;	L8=N;		M8=1;//pobranie adr i dlugosci mac mat_b
	//nie @_mat_b;	musi byc podana stala wartosc (np L8=4)bo inaczej to kicha wychodzi
	I2=_vector_Y;	B2=I2;	L2=0;		M2=1;//pobranie adr i dlugosci mac mat_c

lcntr=M, do column until lce; //petla zewnetrzna - zmiana wiersza
	f0=0;
		lcntr=N, do row until lce; //petla wewnetrzna - wymnozenie wiersza 
				f1=dm(I0,M0); 	f2=pm(i8,m8);
				f3=f1*f2;		
		row:	f0=f0+f3;
column:	DM(I2,M2)=F0;//zapis do odp. komorki
	
//przywrocenie wartosci rejestrom	
	I0=0; 	I8=0;	I2=0;
	B0=i0;	B8=i0;	B2=0;
	L0=0;	L8=0;	L2=0;
	M0=0;	M8=0;	M2=0;	
/**********************************/
	leaf_exit;
_mat_x_vec.end:
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@


_mat_x_mat:
.global _mat_x_mat;
	leaf_entry;
	I0=_mat_a;	B0=I0;	L0=@_mat_a;	M0=1;//pobranie adr i dlugosci mac mat_a
	I8=_mat_X;	B8=I8;	L8=16;		M8=4;	m9=1; m10=-12;//pobranie adr i dlugosci mac mat_b
	//nie @_mat_b;	musi byc podana stala wartosc (np L8=4)bo inaczej to kicha wychodzi
	I2=_mat_Y;	B2=I2;	L2=0;		M2=4;	m3=-15;//pobranie adr i dlugosci mac mat_c

lcntr=K, do column_change until lce;	
	lcntr=M, do row_change until lce; //petla zewnetrzna - zmiana wiersza
		f0=0;	//i8=_mat_X;
			lcntr=N, do row_multiply until lce; //petla wewnetrzna - wymnozenie wiersza 
					f1=dm(I0,M0); 	f2=pm(i8,m8);
					f3=f1*f2;		
			row_multiply:	f0=f0+f3;
			//f0=pm(i8,m10);
			//modify(i8,-16);/**nie trzeba modyfikowac bo indeks=indeks+16 **/
	row_change:	DM(I2,M2)=F0;//zapis do odp. komorki
	f0=dm(i2,m3);//f1=pm(i8,m9);
	//i8=_mat_X; i2=_mat_Y+2;
	modify(i8,1);//modify(i2,1);
column_change: I0=_mat_a;//modify(i1,1);
//i1=0;
//przywrocenie wartosci rejestrom	
	I0=0; 	I8=0;	I2=0;
	B0=i0;	B8=i0;	B2=0;
	L0=0;	L8=0;	L2=0;
	M0=0;	M8=0;	M2=0;	
/**********************************/
	leaf_exit;
_mat_x_mat.end:


//Procedura do wyznaczania macierzy odwrotnej//
_mat_inv:
.global _mat_inv;
	leaf_entry;
	//save_reg;
//	I0=_mat_a;	B0=I0;	L0=@_mat_a;	M0=1;//pobranie adr i dlugosci mac mat_a
//	I8=_mat_X;	B8=I8;	L8=16;		M8=4;	m9=1; m10=-12;//pobranie adr i dlugosci mac mat_b
	//nie @_mat_b;	musi byc podana stala wartosc (np L8=4)bo inaczej to kicha wychodzi
//	I2=_mat_Y;	B2=I2;	L2=0;		M2=4;	m3=-15;//pobranie adr i dlugosci mac mat_c

//mat_inv:
 //{i0 -> a(row,col)}
		//puts=B6;
 		//i9=i6;
		
		//b0=_matrix_A_inv;
		b8=_matrix_A_inv;
 		b9=_pf;	//b1=_pf;  //  {i1 -> pf= pivot_flag}
		b15=_swc;	//b7=_swc;  //  {i7 -> sc= swap_col}
		b0=_swr;	//b8=_swr; //  {i8 -> sr= swap_row}
		l15=0;	//l7=0;
		l0=0;	//l8=0;
		m8=1;	//m0=1;
		m9=-1;	//m1=-1;
		m0=1;	//m8=1;
		m1=-1;	//m9=-1;
		
		m11=1;
				
		r14=n;
		r13=r14*r14(ssi), b11=b8;//b3=b0;
		l8=r13;	//l0=r13; //   {matrix in a circular data buffer}
		b12=b8;	//b4=b0;
		b13=b8;	//b5=b0;
		b14=b8;	///b6=b0;
		l11=l8;	//l3=l0;
		l12=l8;	//l4=l0;
		l13=l8;	//l5=l0;
		l14=l8;	//l6=l0;
		r13=r14+1, b10=b9;	// b2=b1;
		l9=r13;	//l1=r13;   // {pf in a circular data buffer}
		l10=l9;	//l2=l1;
		f9=0.0;
		f8=2.0;    //{2.0 is required for DIVIDE_macro}
		f7=1.0;    //{1.0 is a numerator for DIVIDE_macro}
		r13=fix f9, m10=r14;	//m2=r14;
		lcntr=r14, do zero_index until lce;
		pm(i15,m8)=r13, dm(i0,m0)=r13;	//dm(i7,m0)=r13, pm(i8,m8)=r13;
zero_index:	  pm(i9,m8)=r13;	//dm(i1,m0)=r13;
		f0=pass f9, pm(i9,m8)=r13;	//dm(i1,m0)=r13; //{f0= big}

		lcntr=r14, do full_pivot until lce;
			//{find the biggest pivot element}
		  r1=pass r13, r11=pm(i9,m11);		//r11=dm(i1,1); //{r1= row no., r11= pf(row)}
		  lcntr=r14, do row_big until lce;
		    r11=pass r11, i12=i11;		//i4=i3; //{check if pf(row) is zero}
		    if ne jump (PC,12); f4=pm(i8,m10);	//f4=dm(i0,m2); //{i0 -> next row}
		    r5=pass r13, r15=pm(i10,m11);		//r15=dm(i2,1); ///{r5= col no., r15= pf(col)}
		    lcntr=r14, do column_big until lce;
		      r15=pass r15; //{check if pf(col) is zero}
		      if ne jump column_big (db);
		      f4=pm(i8,m11);		//f4=dm(i0,1); //{f4= a(row,col)}
		      f6=abs f4;
		      comp(f6,f0); //{compare abs_element to big}
		      if lt jump column_big;
		      f0=pass f6, f12=f4; //{f0= abs_element, f12= pivot_element}
		      r2=pass r1, r10=r5; //{r2= irow, r10= icol}
column_big:	      r5=r5+1, r15=pm(i10,m11);	//r15=dm(i2,1);
row_big:	    r1=r1+1, r11=pm(i9,m11);	//r11=dm(i1,1);
			
		//{swap rows to make this diagonal the biggest absolute pivot} 
		  f12=pass f12, m13=r10;		//m5=r10; //{check if pivot is zero, m5= icol}
		  if eq jump end_prog;//rts; //{if pivot is zero, matrix is singular}
		  r1=r2*r14 (ssi), pm(m13,i9)=r5;	//dm(m5,i1)=r5; //{pf(col) not zero}
		  r5=r10*r14 (ssi), m14=r1;		//m6=r1;                     
		  comp(r2,r10), r1=pm(i11,m14);	//r1=dm(i3,m6); //{i3 -> a(irow,col)}
		  pm(i15,m8)=r10, dm(i0,m0)=r2;		//dm(i7,m0)=r10, pm(i8,m8)=r2; //{store icol in sc and irow in sr}
                  if eq jump row_divide (db);
		  r2=pass r13, m15=r5;	//, m7=r5; 
		  modify(i12,m15);	//modify(i4,m7); //{i4 -> a(icol,col)}
		  i13=i12;	//i5=i4;
		  lcntr=r14, do swap_row until lce;
		    f4=pm(i11,0);	//f4=dm(i3,0);		//{f4= temp= a(irow,col)}
		    f0=pm(i13,0);	//f0=dm(i5,0);          //      {f0= a(icol,col)}
		    pm(i11,m11)=f0;	//dm(i3,1)=f0;		//{a(irow,col)= a(icol,col)}
swap_row:   pm(i13,m11)=f4;    //    dm(i5,1)=f4;		//{a(icol,col)= temp}
		  
			//{divide the row by the pivot}
row_divide:	  f6=pass f7, i13=i12;	//i5=i4;
		  DIVIDE (f1,f6,f12,f8,f3); //{f1= pivot_inverse}
		  i14=i13;		//  i6=i5;
		  f4=pm(i12,m11);		//f4=dm(i4,1);
		  lcntr=r14, do divide_row until lce;
		    f5=f1*f4, f4=pm(i12,m11);		//f4=dm(i4,1);
divide_row:		pm(i14,m11)=f5;	//dm(i6,1)=f5;
			pm(m13,i13)=f1;		//dm(m5,i5)=f1;

			//{fix the other rows by subtracting}
        	 lcntr=r14, do fix_row until lce;
		    comp(r2,r10), i14=i13;	//, i6=i5; //{check if row= icol}
		    if eq jump (PC,8); f4=pm(i8,m10);	//f4=dm(i0,m2); //{i0 -> next row}
		    f4=pm(m13,i8);	//f4=dm(m5,i0); //{temp= a(row,icol)}
		    pm(m13,i8)=f9;		//dm(m5,i0)=f9;
		  	f3=pm(i14,m11);		//  f3=dm(i6,1);
		    lcntr=r14, do fix_column until lce;
		      f3=f3*f4, f0=pm(i8,0);	//f0=dm(i0,0);
		      f0=f0-f3;	f3=pm(i14,m11);	//, f3=dm(i6,1);
		      
fix_column:	    pm(i8,m11)=f0;	//  dm(i0,1)=f0;
fix_row:	    r2=r2+1;
full_pivot:	f0=pass f9, i11=i8;	//i3=i0;

			//{fix the affect of all the swaps for final answer} 
		r0=pm(i15,m9), r1=dm(i0,m1);		//r0=dm(i7,m1), r1=pm(i8,m9); //{i7 -> sc(N-1), i8 -> sr(N-1)}
		r0=pm(i15,m9), r1=dm(i0,m1);		//r0=dm(i7,m1), r1=pm(i8,m9); //{r0= sc(N-1), r1= sr(N-1)}
		lcntr=r14, do fix_swap until lce;
		  comp(r0,r1), m13=r0;	//m5=r0; //{m5= sc(swap)}
		  if eq jump fix_swap;
		  m12=r1;		//m4=r1; //{m4= sr(swap)}
		  lcntr=r14, do swap until lce;
		    f4=pm(m12,i8);		//f4=dm(m4,i0); //{f4= temp= a(row,sr(swap))}
		    f0=pm(m13,i8);		//f0=dm(m5,i0); //{f0= a(row,sc(swap))}
		    pm(m12,i8)=f0;		//dm(m4,i0)=f0; //{a(row,sr(swap))= a(row,sc(swap))}
		    pm(m13,i8)=f4;		//dm(m5,i0)=f4; //{a(row,sc(swap))= temp}
swap:               modify(i8,m10);		//modify(i0,m2);
fix_swap:	  r0=pm(i15,m9), r1=dm(i0,m1);		//r0=dm(i7,m1), r1=pm(i8,m9);

//przywrocenie wartosci rejestrom	

end_prog:	
	i0=0;	i1=0;	I2=0;	I3=0;
	i4=0;	i5=0;	i7=0;
	I8=0; 	I9=0;	I10=0;	I11=0;
	I12=0;	I13=0;	I14=0;	
	
	b0=0;	b1=0;	B2=0;	B3=0;
	B4=0;	B5=0;	B7=0;
	B8=0;	B9=0;	B10=0;	B11=0;	
	B12=0;	b13=0;	B14=0;	b15=0;
			
	L0=0;	L1=0;	L2=0;	L3=0;
	L4=0;	L5=0;	L6=0;	L7=0;
	
	L8=0;	L9=0;	L10=0;	L11=0;
	L12=0;	L13=0;	L14=0;	L15=0;
	
	M0=0;	M1=0;	M2=0;	M3=0;
	M4=3;	M5=0;
	
	m8=0;	m9=0;	m10=0;	m11=0;
	m12=0;	m13=0;	m14=0;	m15=0;
	
/**********************************/
	leaf_exit;
//restore_reg;
_mat_inv.end:




/***  Procedura wyliczanie wyznacznika macierzy 3x3 *****/
_mat_det_3x3:
.global _mat_det_3x3;
	leaf_entry;
	B1=_det; 	I0=B0; 	L1=0; 	M1=0;
	//R1=n+1;
	B8=_matrix_A_inv;		L8=n*n;	M8=n+1;//pobranie adr i dlugosci mac mat_a
	M9=2;
	m10=1;
	m11=n+2;
	m12=-1;
	f0=0;
			f2= pm(i8,m8); //pobranie 1 z diag macierzy, incr o 4
			f3= pm(i8,m8); //pobranie 2 z diag macierzy, incr o 4
			f4= pm(i8,m9); //pobranie 3 z diag macierzy, incr o 2
			f5=f2*f3;		//wymnozenie i dodanie
			f5=f5*f4;
			f0=f0+f5;
			f2= pm(i8,m8);  // anal. pobr kolej 3 liczb, incr o 4
			f3= pm(i8,m10); // incr o 1
			f4= pm(i8,m11);	// incr o n+2=5
			f5=f2*f3;
			f5=f5*f4;
			f0=f0+f5;
			f2= pm(i8,m10); // pobr ostat 3 liczb, incr o 1
			f3= pm(i8,m8);	// incr o 4
			f4= pm(i8,m8);	// incr o 4
			f5=f2*f3;
			f5=f5*f4;
			f0=f0+f5;
	lcntr=n, do odejmij until lce; //petla sluzaca do odjecia kolejnych iloczynow
			f2= pm(i8,m9); //incr o 2
			f3= pm(i8,m9); //incr o 2
			f4= pm(i8,m12);//incr o -1
			f5=f2*f3;
			f5=f5*f4;
odejmij:	f0=f0-f5;
	
	DM(I1,M1)=f0; //zapisanie wartoci wyznacznika do odp komorki pamieci
//przywrocenie wartosci rejestrom	
	I0=0; 	I8=0;	I2=0;
	B0=i0;	B8=i0;	B2=0;
	L0=0;	L8=0;	L2=0;
	M0=0;	M8=0;	M2=0;	
/**********************************/
	leaf_exit;
_mat_det_3x3.end:
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@










//################################################################
//# procedura _count_start  - do pomiaru czasu trwania procedury #
//################################################################

/*_count_start: //* call this to start cycle count 
.global _count_start;
r1=mode1;
bit clr mode1 IRPTEN;
r0=emuclk;
mode1=r1;
exit;
_count_start.end:
*/
//###############################################################
//# procedura _count_end   - do pomiaru czasu trwania procedury #
//###############################################################
/*
_count_end: //* call this to end cycle count 
.global _count_end;
r2=mode1;
bit clr mode1 IRPTEN;
r0=emuclk;
r0=r0-r4;
r1=14;     //* fudge factor to compensate for overhead 
r0=r0-r1;  
mode1=r2;
exit;
_count_end.end:

.endseg;

*/