/*
 * THALES S-Net // SAC demonstrator
 *
 * Stephan Herhut (s.a.herhut@herts.ac.uk)
 * 
 * $Id: STAP_Mat_Invert.sac,v 1.2 2008/03/10 09:37:23 rbarrere Exp $
 */

module STAP_Mat_Invert;

use Structures : all;
use Numerical : all;

export {STAP_Mat_Invert, STAP_Mat_InvertTHALES};

/*
 * SAC version of the C function
 */

#ifdef SPLITMATRIX
complex[.,.] GJ( complex[.,.] A, int i)
{
  irow = reshape( [shape(A)[[1]]], take( [1], A));

  Ap = drop( [1], A);

  val = irow[[i]];

  irow = irow / val;

  /*
   * { [j,k] -> Ap[[j,k]] - Ap[[j,i]] * irow[[k]] }; 
   */
  Ap = with {
         (. <= [j,k] <= .) : Ap[[j,k]] - Ap[[j,i]] * irow[[k]];
       } : genarray( shape( Ap), toc(0));

  result = Ap ++ reshape( [1] ++ shape(irow), irow);

  i = i + 1;
  if ( i < shape( result)[[0]]) {
    result = GJ( result, i);
  }

  return( result);
}
#endif

inline
complex[.,.] GJ( complex[.,.] A, int i)
{
  for (i = 0; i < shape(A)[[0]]; i++) {
    A[[i]] = A[[i]] / A[[i,i]];

    /*
     * { [j,k] -> A[[j,k]] - A[[j,i]] * A[[i,k]] }; 
     */
    A = with {
           ([0,0] <= [j,k] < [i,shape(A)[[1]]]) : A[[j,k]] - A[[j,i]] * A[[i,k]];
           ([i,0] <= [j,k] < [i+1, shape(A)[[1]]]) : A[[j,k]];
           ([i+1,0] <= [j,k] < shape(A)) : A[[j,k]] - A[[j,i]] * A[[i,k]];

         } : genarray(A, toc(0));
  }

  return( A);
}

inline
complex[.,.] E( complex[.,.] mat) 
{
  result = with {
             ( . <= [i,j] <= .) : (i==j) ? toc(1) : toc(0);
           } : genarray( shape( mat), toc(0));

  return( result);
}

inline
complex[*] STAP_Mat_Invert( complex[*] input)
{
  result = with {
             ( . <= iv <= .) : STAP_Mat_Invert( input[iv]);
           } : genarray( drop( [-2], shape( input)), 
                         genarray( take( [-2], shape( input)), toc(0)));
  return( result);
}

inline
complex[.,.] STAP_Mat_Invert( complex[.,.] mat)
{
  /*
   * { [j,k] -> Ap[[j,k]] - Ap[[j,i]] * irow[[k]] }; 
   */
  tosolve = with {
              ( . <= [i] <= .) : mat[[i]] ++ E( mat)[[i]];
            } : genarray( [shape(mat)[[0]]], 
                          genarray( [2 * shape(mat)[[1]]], toc(0)));

  solved = GJ( tosolve, 0);

  result = drop( [0,shape(mat)[[1]]], solved);

  return( result);
}

complex[.,.] STAP_Mat_InvertTHALES( complex[.,.] input) 
{
	dim1=shape(input)[[0]];
	dim2=shape(input)[[1]];
	

	emptymatrix = genarray([dim1,dim2], toc(0.0,0.0));
	emptymatrix2 = genarray([dim1,2*dim2], toc(0.0,0.0));
	
	inv = emptymatrix2;
	val = emptymatrix;
	inp = input;
			
	for(i = 0; i < dim1; i++)
	{
		for(j = 0;j < dim2; j++)
		{
			inv[i,j] = toc(real(inp[i,j]) , imag(inp[i,j]));
			if(i == j)
			{
				inv[i,j+dim2] = toc(1.0,0.0);
			}
			else
			{
				inv[i,j+dim2] = toc(0.0,0.0);
			}
		}
	}
			
	for(i = 0;i < dim1; i++)
	{
 		pivot = toc (real(inv[i,i]), imag(inv[i,i]));
		
		for(j=i;j<2*dim2;j++)
		{
			re = real(inv[i,j]);
			im = imag(inv[i,j]);
			inv[i,j] = toc (
						(re * real(pivot) + im * imag(pivot))/(real(pivot) * real(pivot) + imag(pivot) * imag(pivot)) ,
						(im * real(pivot) - re * imag(pivot))/(real(pivot) * real(pivot) + imag(pivot) * imag(pivot))
						);
		}
	
		for(k=0;k<dim2;k++)
		{
    		if(i!=k)
   			{
    			coef = inv[k,i];
    			for(l=i;l<2*dim2;l++)
    			{
					R = real(coef) * real(inv[i,l]) - imag(coef) * imag(inv[i,l]) ;
					I = real(coef) * imag(inv[i,l]) + imag(coef) * real(inv[i,l]) ;
					
					inv[k,l] = inv[k,l] - toc (R,I);
    			}
   			}
		}
	} 

	for(i = 0;i < dim1; i++)
	{
		for(j=0;j<dim2;j++)
		{
			val[i,j] = toc(real(inv[i,j+dim2]),imag(inv[i,j+dim2]));
		}
	}
			
 	return (val);
}