/******************************************************
*  Gauss decomposition routines for a complex matrix  *
* --------------------------------------------------- *
* (See Demo Program Tucmat.cpp).                      *
*                                                     *
* Reference:  From Numath Library By Tuan Dang Trong  *
*             in Fortran 77 [BIBLI 18].               *
*                                                     *
*              C++ Release 1.1 By J-P Moreau, Paris.  *
*                                                     *
* Release 1.1 (08/16/07): Added function INVC().      *
******************************************************/
#include "ucmat.h"

#define  ABS  fabs

  int IMIN(int i, int j) {
    if (i<j) return i;
    else return j;
  }

  void DECC (int N, CMat A, int *IP, int *IER)  {
/* VERSION COMPLEX DOUBLE PRECISION
C-----------------------------------------------------------------------
C  MATRIX TRIANGULARIZATION BY GAUSSIAN ELIMINATION
C  ------ MODIFICATION FOR COMPLEX MATRICES --------
C  INPUT..
C     N = ORDER OF MATRIX.
C     A = COMPLEX SQUARE MATRIX TO BE TRIANGULARIZED.
C  OUTPUT..
C     A(I,J), I <= J = UPPER TRIANGULAR FACTOR.
C     AI(I,J), I.LE.J = UPPER TRIANGULAR FACTOR, U ; IMAGINARY PART.
C     AR(I,J), I.GT.J = MULTIPLIERS = LOWER TRIANGULAR FACTOR, I - L.
C                                                    REAL PART.
C     AI(I,J), I.GT.J = MULTIPLIERS = LOWER TRIANGULAR FACTOR, I - L.
C                                                    IMAGINARY PART.
C     IP(K), K.LT.N = INDEX OF K-TH PIVOT ROW.
C     IP(N) = (-1)**(NUMBER OF INTERCHANGES) OR O .
C     IER = 0 IF MATRIX A IS NONSINGULAR, OR K IF FOUND TO BE
C           SINGULAR AT STAGE K.
C  USE  SOL  TO OBTAIN SOLUTION OF LINEAR SYSTEM.
C  IF IP(N)=O, A IS SINGULAR, SOL WILL DIVIDE BY ZERO.
C
C  REFERENCE:
C     C. B. MOLER, ALGORITHM 423, LINEAR EQUATION SOLVER,
C     C.A.C.M. 15 (1972), P. 274.
C---------------------------------------------------------------------*/
      int NM1,K,KP1,M,I,J;
      double DEN,PRODR,PRODI,TR,TI;

      *IER = 0;
      IP[N] = 1;
      if (N == 1) goto e70;
      NM1 = N - 1;
      for (K=1; K<=NM1; K++) {
        KP1 = K + 1;
        M = K;
        for (I=KP1; I<=N; I++)
          if (ABS(A[I][K][0]) + ABS(A[I][K][1]) > ABS(A[M][K][0]) + ABS(A[M][K][1]))  M = I;
        IP[K] = M;
        TR = A[M][K][0];
        TI = A[M][K][1];
        if (M == K) goto e20;
        IP[N] = -IP[N];
        A[M][K][0] = A[K][K][0];
        A[M][K][1] = A[K][K][1];
        A[K][K][0] = TR;
        A[K][K][1] = TI;
e20:    if (ABS(TR) + ABS(TI) == 0.0) goto e80;
        DEN=TR*TR+TI*TI;
        TR=TR/DEN;
        TI=-TI/DEN;
        for (I=KP1; I<=N; I++) {
          PRODR=A[I][K][0]*TR - A[I][K][1]*TI;
          PRODI=A[I][K][1]*TR + A[I][K][0]*TI;
          A[I][K][0]=-PRODR;
          A[I][K][1]=-PRODI;
        }
        for (J=KP1; J<=N; J++) {
          TR = A[M][J][0];
          TI = A[M][J][1];
          A[M][J][0] = A[K][J][0];
          A[M][J][1] = A[K][J][1];
          A[K][J][0] = TR;
          A[K][J][1] = TI;
          if (ABS(TR) + ABS(TI) == 0.0) goto e48;
          if (TI == 0.0) {
			for (I=KP1; I<=N; I++) {
              PRODR=A[I][K][0]*TR;
              PRODI=A[I][K][1]*TR;
              A[I][J][0] = A[I][J][0] + PRODR;
              A[I][J][1] = A[I][J][1] + PRODI;
            }
            goto e48;
          }
          if (TR == 0.0) {
            for (I=KP1; I<=N; I++) { 
              PRODR =-A[I][K][1]*TI;
              PRODI = A[I][K][0]*TI;
              A[I][J][0] = A[I][J][0] + PRODR;
              A[I][J][1] = A[I][J][1] + PRODI;
            }
            goto e48;
          }
          for (I=KP1; I<=N; I++) {
            PRODR = A[I][K][0]*TR - A[I][K][1]*TI;
            PRODI = A[I][K][1]*TR + A[I][K][0]*TI;
            A[I][J][0] = A[I][J][0] + PRODR;
            A[I][J][1] = A[I][J][1] + PRODI;
          }
e48:;   }
      }
e70:  K = N;
      if (ABS(A[N][N][0]) + ABS(A[N][N][1]) == 0.0) goto e80;
      return;
e80:  *IER = K;
      IP[N] = 0;
}

  void SOLC (int N, CMat A, int LB, COMPLEX *B, int *IP) {
/* VERSION COMPLEX DOUBLE PRECISION
C-----------------------------------------------------------------------
C  SOLUTION OF LINEAR SYSTEM, A*X = B .
C  INPUT..
C    N = ORDER OF MATRIX.
C    A = TRIANGULARIZED COMPLEX MATRIX OBTAINED FROM DECC.
C    B = RIGHT HAND SIDE COMPLEX VECTOR.
C    LB = LOWER BANDWIDTH OF A (LB=N-1).
C    IP = PIVOT VECTOR OBTAINED FROM DEC.
C  DO NOT USE IF DEC HAS SET IER <> 0.
C  OUTPUT..
C    B = SOLUTION COMPLEX VECTOR, X .
C---------------------------------------------------------------------*/
      int NM1,K,KP1,M,I,KB,KM1;
      double DEN,TR,TI;
      COMPLEX PROD;
      
      if (N == 1) goto e50;
      NM1 = N - 1;
      if (LB == 0) goto e25;
      for (K=1; K<=NM1; K++) {
        KP1 = K + 1;
        M = IP[K];
        TR = B[M][0];
        TI = B[M][1];
        B[M][0] = B[K][0];
        B[M][1] = B[K][1];
        B[K][0] = TR;
        B[K][1] = TI;
        for (I=KP1; I<=IMIN(N,LB+K); I++) {
          PROD[0]=A[I][K][0]*TR-A[I][K][1]*TI;
          PROD[1]=A[I][K][1]*TR+A[I][K][0]*TI;
          B[I][0] = B[I][0] + PROD[0];
          B[I][1] = B[I][1] + PROD[1];
        }
      }
e25:  for (KB=1; KB<=NM1; KB++) {
        KM1 = N - KB;
        K = KM1 + 1;
        DEN=A[K][K][0]*A[K][K][0] + A[K][K][1]*A[K][K][1];
        PROD[0]=B[K][0]*A[K][K][0] + B[K][1]*A[K][K][1];
        PROD[1]=B[K][1]*A[K][K][0] - B[K][0]*A[K][K][1];
        B[K][0]=PROD[0]/DEN;
        B[K][1]=PROD[1]/DEN;
        TR = -B[K][0];
        TI = -B[K][1];
        for (I=1; I<=KM1; I++) {
          PROD[0]=A[I][K][0]*TR - A[I][K][1]*TI;
          PROD[1]=A[I][K][1]*TR + A[I][K][0]*TI;
          B[I][0] = B[I][0] + PROD[0];
          B[I][1] = B[I][1] + PROD[1];
        }
      }
e50:  DEN=A[1][1][0]*A[1][1][0] + A[1][1][1]*A[1][1][1];
      PROD[0]=B[1][0]*A[1][1][0] + B[1][1]*A[1][1][1];
      PROD[1]=B[1][1]*A[1][1][0] - B[1][0]*A[1][1][1];
      B[1][0]=PROD[0]/DEN;
      B[1][1]=PROD[1]/DEN;
  }

// Find inverse matrix of complex square matrix A - Result in B
void INVC (int N, CMat A, CMat B) {
  int i,j, Error;
  int IP[SIZE];
  COMPLEX B1[SIZE];

// call decomposition procedure (only once).
  DECC (N, A, IP, &Error);

// Save unity matrix in B
  for (i=1; i<=N; i++)
    for (j=1; j<=N; j++)
	  if (i==j) {
        B[i][j][0] = 1.0; B[i][j][1] = 0.0;
      }
      else {
        B[i][j][0] = 0.0; B[i][j][1] = 0.0;
      }

// call solver if error code is ok
// to obtain inverse of A one column at a time.
  for (j=1; j<=N; j++) {
    for (i=1; i<=N; i++) {
      B1[i][0] = B[i][j][0];
      B1[i][1] = B[i][j][1];
    }

    SOLC (N, A, N-1, B1, IP);

    for (i=1; i<=N; i++) {
      B[i][j][0] = B1[i][0];
      B[i][j][1] = B1[i][1];
    }
  }

// inverse of matrix A is now in matrix B,
// matrix A is destroyed.

}

// End of file clu.cpp