/****************************************************
*     Solving a complex linear system A Z = B,      *
*     where A is complex matrix of size N*N,        *
*     B is a complex matrix of size N*M,            *
*     Z is a complex solution matrix of size N*M.   *
* ------------------------------------------------- *
* Ref.: "Mathématiques en Turbo-Pascal By M. Ducamp *
* and A. Reverchon (vol 2), Eyrolles, Paris, 1988"  *
* [BIBLI 05].                                       *
* ------------------------------------------------- *
* SAMPLE RUN:                                       *
*                                                   *
* Solve complex linear system:                      *
*                                                   *
*   z1 +  z2 + iz3 = 5                              *
*   z1 + iz2 +  z3 = -2i                            *
*  iz1 +  z2 +  z3 = 1                              *
*                                                   *
*  Here: N=3, M=1                                   *
*                                                   *
*  System solution:                                 *
*                                                   *
*  z1 =   1.500000  i  -0.500000                    *
*  z2 =   1.000000  i   1.000000                    *
*  z3 =  -0.500000  i  -2.500000                    *
*                                                   *
*  DET =  2.00000000000000E+0001                    *
*                                                   *
*                C++ version by J-P Moreau, Paris.  *
*                       (www.jpmoreau.fr)           *
*****************************************************
 Explanations:
 The original complex linear system to solve is:
     [ a11 a12 ... a1N ] [ z1 ]   [ b1 ]
     [ a21 a22 ... a2N ] [ z2 ]   [ b2 ]
     [ ... ... ... ... ] [... ] = [... ]
     [ aN1 aN2 ... aNN ] [ zN ]   [ bN ]
     a k][l = c k][l + i d k][l
     z k = x k + i y k
     b k = e k + i f k
     where c, d, e, f, x, y are real numbers.
 The system is replaced by the following REAL system of size 2*N:
     [ c11 -d11 c12 -d12 ... c1N -d1N ] [ x1 ]   [ e1 ]
     [ d11  c11 d12  c12 ... d1N  c1N ] [ y1 ]   [ f1 ]
     [ c21 -d21 c22 -d22 ... c2N -d2N ] [ x2 ]   [ e2 ]
     [ d21  c21 d22  c22 ... d2N  c2N ] [ y2 ] = [ f2 ]
     [ ... .... ... .... ... ... .... ] [ ...]   [ ...]
     [ cN1 -dN1 cN2 -dN2 ... cNN -dNN ] [ xN ]   [ eN ]
     [ dN1  cN1 dN2  cN2 ... dNN  cNN ] [ yN ]   [ fN ]
 The real system is then solved by a classic Gauss-Jordan method.         
*/
#include <stdio.h>
#include <math.h>

#define  MACH_EPS  2e-16
#define  NMAX      3
#define  MMAX      1

typedef  double MAT[NMAX][NMAX];
typedef  double VEC[NMAX];
typedef  double MAT2[2*NMAX][2*NMAX];
typedef  double MAT3[2*NMAX][MMAX]; 

MAT      Xm, Ym; 
VEC      Xv, Yv;
MAT2     M;
MAT3     V;

int      i,j,li,co;
double   det;


/******************************************                                     
*  SOLVING A LINEAR MATRIX SYSTEM AX = B  *
*  with Gauss-Jordan method using full    *
*  pivoting at each step. During the pro- *
*  cess, original AA and BB matrices are  *
*  destroyed to spare storage location.   *
* --------------------------------------- *
* INPUTS:    AA   MATRIX N*N              *                                     
*            BB   MATRIX N*M              *                                     
* --------------------------------------- *                                     
* OUTPUS:    AA   INVERSE OF AA N*N       *                                     
*            DET  DETERMINANT OF AA       *                                     
*            BB   SOLUTION MATRIX N*M     *                                     
* --------------------------------------- *                                     
* NOTA - If M=0 inversion of AA matrix    *
*        only.                            *                                     
******************************************/
void MATINV(int N, int M, MAT2 AA, MAT3 BB, double *DET)   { 	
      double PC[2*NMAX],PL[2*NMAX],CS[2*NMAX];                                                  
      double PV,PAV,temp,TT;   
      int I,IK,J,JK,K;
                                                         
// Initializations :                                                             
      *DET= 1.0; 
      for (I=0; I<N; I++)  {                                                                
        PC[I]= 0.0;                                                                
        PL[I]= 0.0;                                                                
        CS[I]= 0.0;                                                                
      }              
// Main loop                                                                         
      for (K=0; K<N; K++) {                                                                  
// Searching greatest pivot :                                               
        PV=AA[K][K];                                                              
        IK=K;                                                                    
        JK=K;                                                                    
        PAV= fabs(PV);                                                            
        for (I=K; I<N; I++)                                                                
          for (J=K; J<N; J++)  {     
            temp = fabs(AA[I][J]);                                                        
            if (temp > PAV) {                                      
              PV=AA[I][J];                                                        
              PAV= fabs(PV);                                                      
              IK=I;                                                              
              JK=J;                                                              
            }                                                               
          }                                                                 
                                                                               
// Search terminated, the pivot is in location I=IK, J=JK.                     
// Memorizing pivot location:  		  

		PC[K]=JK;                                                                
        PL[K]=IK;                                                                
                                                                               
// DETERMINANT DET is actualised                                              
// If DET=0, ERROR MESSAGE and STOP                                           
// Machine dependant EPSMACH equals here 2e-16
                                                                               
        if (IK!=K) *DET=-*DET;                                                   
        if (JK!=K) *DET=-*DET;                                                   
        *DET=*DET*PV;  
        temp= fabs(*DET);                                                            
        if (temp < MACH_EPS) {                                          
          printf("\n  The determinant equals ZERO !!!\n");                                                              
          return;                                                                  
        }                                                                   
                                                                               
// POSITIONNING PIVOT IN K,K:		
        if(IK!=K)                                                         
          for (I=0; I<N; I++) {                                                              
// EXCHANGE LINES IK and K of matrix AA:			
            TT=AA[IK][I];                                                         
            AA[IK][I]=AA[K][I];                                                    
            AA[K][I]=TT;                                                          
          }                                                                 
                                                                           
        if(M!=0)                                                           
          for (I=0; I<M; I++) {                                                               
            TT=BB[IK][I];                                                           
            BB[IK][I]=BB[K][I];                                                      
            BB[K][I]=TT;                                                            
          }                                                                   
                                                                           
// PIVOT is at correct line		  
        if(JK!=K)                                                         
          for (I=0; I<N; I++) {                                                              
// EXCHANGE COLUMNS JK and K of matrix AA:
            TT=AA[I][JK];                                                         
            AA[I][JK]=AA[I][K];                                                    
            AA[I][K]=TT;                                                          
          }                                                                 
                                                                           
// The PIVOT is at correct column.                                              
// and is located in K,K.                                                   
                                                                               
// Column K of matrix AA is stored in CS vector                             
// then column K is set to zero. 		  
        for (I=0; I<N; I++) {                                                                
          CS[I]=AA[I][K];                                                         
          AA[I][K]= 0.0;                                                          
        }                                                                   
                                                                               
        CS[K]= 0.0;                                                                
        AA[K][K]= 1.0;                                                              
// Line K of matrix AA is modified:		
        temp= fabs(PV);                                          
        if(temp < MACH_EPS) {                                            
          printf("\n  PIVOT TOO SMALL - STOP\n");                               
          return;                                                                  
        }                                                                   
        for (I=0; I<N; I++)                                                                
          AA[K][I]=AA[K][I]/PV;                                                    
                                                                           
        if (M!=0)                                                         
          for (I=0; I<M; I++)                                                             
            BB[K][I]=BB[K][I]/PV;                                                  
                                                                           
// Other lines of matrix AA are modified:		  
        for (J=0; J<N; J++) {                                                                
          if (J==K) J++;                                                  
          for (I=0; I<N; I++)                                                              
// Line J of matrix AA is modified:
            AA[J][I]=AA[J][I]-CS[J]*AA[K][I];
          if (M!=0)                                                       
            for (I=0; I<M; I++)                                                          
              BB[J][I]=BB[J][I]-CS[J]*BB[K][I];                                     
        }                                                                   
// Line K is ready
      } //End of K loop

// MATRIX AA INVERSION IS DONE - REARRANGEMENT OF MATRIX AA
                                                                               
// EXCHANGE LINES                                                                        
      for (I=N-1; I>=0; I--) {                                                               
        IK=(int) PC[I];                                                                
        if (IK==I) goto fin1;                                                   
// EXCHANGE LINES I and PC(I) of matrix AA:
        for (J=0; J<N; J++) {                                                                
          TT=AA[I][J];                                                            
          AA[I][J]=AA[IK][J];                                                      
          AA[IK][J]=TT;                                                           
        }
        if (M!=0)                                                         
          for (J=0; J<M; J++) {                                                             
            TT=BB[I][J];                                                          
            BB[I][J]=BB[IK][J];                                                    
            BB[IK][J]=TT;                                                         
          }                                                                 
// NO MORE EXCHANGE is NECESSARY                                                      
// Go to next line                                                 
        fin1: ;     			  
      } // for i                                                                     
                                                                               
// EXCHANGE COLUMNS:  	  
      for (J=N-1; J>=0; J--) {                                                                         
        JK=(int) PL[J];                                                                
        if (JK==J) goto fin2;                                                   
// EXCHANGE COLUMNS J ET PL(J) of matrix AA:
        for (I=0; I<N; I++) {                                                                
          TT=AA[I][J];                                                            
          AA[I][J]=AA[I][JK];                                                      
          AA[I][JK]=TT;                                                           
        } 
        fin2: ;                                                                  
// NO MORE EXCHANGE is NECESSARY                                                      
// Go to next column.
      }                                                                     
// REARRANGEMENT TERMINATED.
      return;  
  } //Matinv()                                                                         


void main() {
  li=NMAX; co=MMAX;
  //read Xm matrix                        real part of A matrix
  Xm[0][0]= 1; Xm[0][1]=1; Xm[0][2]=0;    //  1  1  0
  Xm[1][0]= 1; Xm[1][1]=0; Xm[1][2]=1;    //  1  0  1
  Xm[2][0]= 0; Xm[2][1]=1; Xm[2][2]=1;    //  0  1  1
  //read Ym matrix                        imaginary part of A matrix
  Ym[0][0]= 0; Ym[0][1]=0; Ym[0][2]=1;    //  0  0  1
  Ym[1][0]= 0; Ym[1][1]=1; Ym[1][2]=0;    //  0  1  0
  Ym[2][0]= 1; Ym[2][1]=0; Ym[2][2]=0;    //  1  0  0
  //read Xv vector                        real part of 1st column of B matrix
  Xv[0]=5; Xv[1]=0; Xv[2]=1;              //  5  0  1
  //read Yv vector                        imaginary part of 1st column of B matrix
  Yv[0]=0; Yv[1]=-2; Yv[2]=0;             //  0 -2  0

  //The complex system AZ=B is replaced by real system MX=V of size 2*NMAX
  //create M matrix
  for (i=0; i<NMAX; i++) {
    for (j=0; j<NMAX; j++) {
      M[2*i][2*j]=Xm[i][j]; M[2*i+1][2*j+1]=Xm[i][j];
      M[2*i+1][2*j]=Ym[i][j]; M[2*i][2*j+1]=-Ym[i][j];
    }
  }
  //create V matrix
  for (i=0; i<NMAX; i++) {
    V[2*i][0]=Xv[i]; V[2*i+1][0]=Yv[i];
  }

  // solve real linear system MX=V of zize 2*NMAX
  // by Gauss-Jordan method - See program sysmat.cpp
  MATINV(li*2, co, M, V, &det);

  //write complex solutions stored in vector V in the form:
  // x1][y1][x2][y2][...][xn][yn][ with zk = xk + i yk... 
  printf("\n  System solution:\n\n");
  for (i=0; i<NMAX; i++) { 
    printf("  z%d = %10.6f  i  %10.6f\n", i,V[2*i][0], V[2*i+1][0]);
  }
  printf("\n  DET = %f\n\n\n",det);

}

// end of file zmatsys.cpp