/*
	This C language subroutine computes the inverse of an
	input matrix.

	The Run Time Library for the C Language.

	Gordon A. Sterling	(617) 461 - 3076
	DSP Development Tools Engineering

	Created on 6/25/90
	Updated on 5/94 by AS

	#include <matrix.h>				    <Header>
	float matinv(float out[][], float in[][], int N);   <Prototype>
*/

#include "lib_glob.h"
#include "mat_glob.h"

.SEGMENT/CODE	Code_Space_Name;
.FILE 		RTL_FILENAME;

.GLOBAL	    	_matinv;

_matinv:	put(R3);
		put(R5);
		put(R6);
		put(R7);
		put(R9);
		R9=R9-R9, put(R10);
		R9=R9+1, put(R11);		/* R9=1		*/
		F7=FLOAT R9, put(R13);		/* F7=1.0	*/
		F11=F7+F7, R2=dma_i;		/* F11=2.0	*/
		put(R2);
		R2=dmb_i;
		put(R2);
		R2=dmc_i;
		COMP(R8,R4), put(R2);

		IF EQ JUMP (PC, error_return);

		R13=PASS R12, dmb_i=R4;		/*Point to output matrix*/

create_identity:DM(dmb_i,dm_M1)=1;
		R12=R13-1, dma_i=R8;		/*Point to input matrix*/
		LCNTR=R12, DO make_ident_row UNTIL LCE;
		    LCNTR=R13, DO make_ident_column UNTIL LCE;
make_ident_column:	DM(dmb_i,dm_M1)=0;
make_ident_row:	    DM(dmb_i,dm_M1)=1;

		R10=R13-1, FETCH_RETURN		/*R10=N-1*/

		LCNTR=R13, DO reduce_row UNTIL LCE;
		    dmc_i=dma_i;
		    F12=DM(dmc_i,0);
		    CALL (PC, ___float_divide) (DB);
		    DM(dmc_i,1)=F7;		
		    dm_ptr=dmc_i;

		    F1=DM(dm_ptr,1);
		    LCNTR=R10, DO reduce_in_row UNTIL LCE;
			F0=F1*F7, F1=DM(dm_ptr,1);  
reduce_in_row:		F0=F1*F7, DM(-2,dm_ptr)=F0;
		    DM(-1,dm_ptr)=F0;
		    dm_ptr=dmb_i;
		    F1=DM(dm_ptr,1);
		    LCNTR=R9, DO reduce_out_row UNTIL LCE;
			F0=F1*F7, F1=DM(dm_ptr,1);
reduce_out_row:		F0=F1*F7, DM(-2,dm_ptr)=F0;
		    DM(-1,dm_ptr)=F0;
	    
		    R3=R8-1, R2=dma_i;		    /*Point to beginning of row*/
		    R3=R3+R9, R6=R10;		    /*Indent to proper position*/
		    CALL (PC, reduce_matrix) (DB);
		    dm_ptr=R3;
		    F7=DM(dm_ptr,0);		    /*Get Scale*/
	    
		    CALL (PC, reduce_matrix) (DB);
		    R3=PASS R4, R2=dmb_i;
		    R6=PASS R9, F0=DM(dma_i,1);

		    R10=R10+1, F0=DM(dma_i,dm_1);
reduce_row:	    R9=R9+1, F0=DM(dmb_i,dm_1);


restore_state:	get(dmc_i,1);
		get(dmb_i,2);
		get(dma_i,3);
		get(R13,4);
		get(R11,5);
		get(R10,6);
		get(R9,7);
		get(R7,8);
		get(R6,9);
		get(R5,10);
		get(R3,11);
		RETURN (DB);
		RESTORE_STACK
		RESTORE_FRAME


error_return:	JUMP (PC, restore_state) (DB);
		F0=0;
		FETCH_RETURN

/*  
    Calling Parameters
	R13 = N
	R2 = Row to use for reduction
	R3 = First point in matrix
	R8 = Number of columns to reduce
	F7 = Scale factor

*/

reduce_matrix:	R12=dma_i;			/*Hold output pointer*/
		dm_ptr=R2;				/*Point to reduction row*/
		LCNTR=R13, DO row_loop UNTIL LCE;
		    COMP(R2,R3), dma_i=R3;		/*Point to row to reduce*/
		    IF EQ JUMP (PC, row_loop);
		    R1=R6-1, F5=DM(dm_ptr,1);	/*Read first point*/
		    LCNTR=R1, DO scale_and_sub UNTIL LCE;
			F0=F5*F7, F1=DM(dma_i,0);
			F1=F1-F0, F5=DM(dm_ptr,1);
scale_and_sub:		F0=F5*F7, DM(dma_i,1)=F1;
		    F1=DM(dma_i,0);
		    F1=F0-F1, dm_ptr=R2;
		    DM(dma_i,0)=F1;
row_loop:	    R3=R3+R13;			/*Increment row to reduce*/
		RTS (DB);
		dma_i=R12;
		NOP;

.ENDSEG;