/**********************************************************************************************
*            SOLVING A COMPLEX LINEAR MATRIX SYSTEM AX=B BY GAUSS-JORDAN METHOD               *
* ------------------------------------------------------------------------------------------- *
*                                                          C++ Version By J-P Moreau, Paris   *
* SAMPLE RUN:                                                                                 *
* INPUTS  from example file csysmat.dat,                                                      *
* OUTPUTS to file csysmat.lst.                                                                *
* ------------------------------------------------------------------------------------------- *
* SAMPLE RUN:                                                                                 *
* Input file                                                                                  *
* 4                                                                                           *
* 47.0 -15.0  62.0 5.0    0.0 -72.0 61.0  20.0                                                *
*  6.0  14.0 -17.0 3.0 -102.0  91.0  7.0 -12.0                                                *
* 13.0 -55.0  32.0 8.0   41.0   7.0 25.0   1.0                                                *
*111.0  25.0  40.0 0.0   12.0 -82.0 58.0 -30.0                                                *
* 1                                                                                           *
* 629.0 988.0                                                                                 *
*-180.0 825.0                                                                                 *
* 877.0 441.0                                                                                 *
* 734.0 -88.0                                                                                 *
*                                                                                             *
* Output file                                                                                 *
*------------------------------------------------------                                       *
*  SOLVING THE COMPLEX MATRIX LINEAR SYSTEM AX = B                                            *
*------------------------------------------------------                                       *
*    (BY GAUSS-JORDAN METHOD WITH FULL PIVOTING)                                              *
*                                                                                             *
*  N=4                                                                                        *
*                                                                                             *
*  COMPLEX MATRIX A:                                                                          *
*                                                                                             *
* (47.000000,-15.000000) (62.000000, 5.000000) ( 0.000000,-72.000000) (61.000000,20.000000)   *
* ( 6.000000,14.000000) (-17.000000, 3.000000) (-102.000000,91.000000) ( 7.000000,-12.000000) *
* (13.000000,-55.000000) (32.000000, 8.000000) (41.000000, 7.000000) (25.000000, 1.000000)    *
* (111.000000,25.000000) (40.000000, 0.000000) (12.000000,-82.000000) (58.000000,-30.000000)  *
*                                                                                             *
*  M=1                                                                                        *
*                                                                                             *
*  COMPLEX MATRIX B:                                                                          *
*                                                                                             *
* (629.000000,988.000000)                                                                     *
* (-180.000000,825.000000)                                                                    *
* (877.000000,441.000000)                                                                     *
* (734.000000,-88.000000)                                                                     *
*                                                                                             *
*  INVERSE OF COMPLEX MATRIX A:                                                               *
*                                                                                             *
* ( 0.003941,-0.007988) (-0.008374,-0.004058) (-0.009490, 0.017938) ( 0.000210,-0.001660)     *
* ( 0.036781, 0.021820) ( 0.014793,-0.027076) (-0.033797,-0.035129) (-0.005015,-0.016173)     *
* (-0.004225,-0.005071) (-0.007674,-0.001821) ( 0.003482, 0.004805) ( 0.000692, 0.003649)     *
* (-0.019727,-0.016588) (-0.001490, 0.018806) ( 0.033739, 0.015368) ( 0.005363, 0.017239)     *
*                                                                                             *
*  SOLUTION MATRIX X:                                                                         *
*                                                                                             *
* (-1.000000, 3.000000)                                                                       *
* ( 2.000000,10.000000)                                                                       *
* ( 7.000000,-5.000000)                                                                       *
* (17.000000, 6.000000)                                                                       *
*                                                                                             *
*  DETERMINANT= (17416564.000000,-10598320.000000)                                            *
*                                                                                             *
*                                                                                             *
*  VERIFICATION OF A1 * A = I:                                                                *
*                                                                                             *
* ( 1.000000, 0.000000) (-0.000000, 0.000000) ( 0.000000, 0.000000) ( 0.000000, 0.000000)     *
* (-0.000000, 0.000000) ( 1.000000, 0.000000) ( 0.000000,-0.000000) ( 0.000000,-0.000000)     *
* ( 0.000000, 0.000000) ( 0.000000,-0.000000) ( 1.000000,-0.000000) ( 0.000000,-0.000000)     *
* (-0.000000, 0.000000) ( 0.000000, 0.000000) ( 0.000000,-0.000000) ( 1.000000, 0.000000)     *
*                                                                                             *
*  VERIFICATION OF AX = B:                                                                    *
*                                                                                             *
* (629.000000,988.000000)                                                                     *
* (-180.000000,825.000000)                                                                    *
* (877.000000,441.000000)                                                                     *
* (734.000000,-88.000000)                                                                     *
*                                                                                             *
* --------------------------------------------------------------------------------------------*
*                                                                                             *
**********************************************************************************************/
#include <stdio.h>
#include <string.h>
#include <math.h>

#define  EPSMACH  2.2e-16
#define  NMAX    10
#define  MMAX     2

  //complex number
  typedef struct {
     double R,I;  //algebraic form
  } COMPLEX; 

  FILE *fp1, *fp2;

  typedef COMPLEX MATC[NMAX][NMAX];
  typedef COMPLEX MATC1[NMAX][MMAX];

  MATC  A, A1, D;                                             
  MATC1 B, C;
  
  COMPLEX DETER, temp;
  int I,J,M,N;                                             

  static char Separator[] =
  "--------------------------------------------------------------------";

  double CABS(COMPLEX Z) {
    return sqrt(Z.R*Z.R + Z.I*Z.I);
  }

  void CADD(COMPLEX Z1, COMPLEX Z2, COMPLEX *Z) {
    Z->R = Z1.R + Z2.R;
    Z->I = Z1.I + Z2.I;
  }

  void CDIF(COMPLEX Z1, COMPLEX Z2, COMPLEX *Z) {
    Z->R = Z1.R - Z2.R;
    Z->I = Z1.I - Z2.I;
  }

  void CMUL(COMPLEX Z1, COMPLEX Z2, COMPLEX *Z) {
    Z->R = Z1.R*Z2.R - Z1.I*Z2.I;
    Z->I = Z1.R*Z2.I + Z1.I*Z2.R;
  }

  void CDIV(COMPLEX Z1, COMPLEX Z2, COMPLEX *Z) {
    double d; COMPLEX C;
    d = Z2.R*Z2.R+Z2.I*Z2.I;
    if (d<1e-12)
      printf(" Complex Divide by zero!\n");
    else {
      C.R=Z2.R; C.I=-Z2.I;
      CMUL(Z1,C,Z);
      Z->R=Z->R/d; Z->I=Z->I/d;
    }
  }

  void CSwap(COMPLEX *Z1, COMPLEX *Z2) {
    COMPLEX  C;
    C.R=Z1->R; C.I=Z1->I; 
    Z1->R=Z2->R; Z1->I=Z2->I;
    Z2->R=C.R; Z2->I=C.I;
  }   

  void MATPRINT(char *titre,MATC A,int N) {  
     int I, J; 
     fprintf(fp2,"\n\n  %s  \n\n",titre);
     for (I=0; I<N; I++)
        for (J=0; J<N; J++)  {
          fprintf(fp2," (%11.6f,%11.6f)",A[I][J].R, A[I][J].I);   
          if(J==N-1) fprintf(fp2,"\n");
        }                     
  }
  
  void MATPRINT1(char *titre,MATC1 A,int N,int M) {  
     int I, J;
     fprintf(fp2,"\n\n  %s  \n\n",titre); 
     for (I=0; I<N; I++)
        for (J=0; J<M; J++)  {
          fprintf(fp2," (%11.6f,%11.6f)",A[I][J].R, A[I][J].I);   
          if(J==M-1) fprintf(fp2,"\n");
        }                     
  }        

/***********************************************                                     
* SOLVING A COMPLEX MATRIX LINEAR SYSTEM AX=B  *
* with Gauss-Jordan method using full pivoting *
* at each step. During the process, original   *
* AA and BB matrices are destroyed to spare    *
* storage location.                            *
* -------------------------------------------- *
* INPUTS:    AA   COMPLEC MATRIX N*N           *                                     
*            BB   COMPLEX MATRIX N*M           *                                     
* -------------------------------------------- *                                     
* OUTPUS:    AA   INVERSE OF AA N*N            *                                     
*            DET  COMPLEX DETERMINANT OF AA    *                                     
*            BB   SOLUTION COMPLEX MATRIX N*M  *
* -------------------------------------------- *                                     
* NOTA - If M=0 inversion of AA matrix only.   *                                     
***********************************************/
void CMATINV(int N,int M,MATC AA,MATC1 BB, COMPLEX *DET) { 	
      int PC[NMAX],PL[NMAX];
	  COMPLEX CS[NMAX];                                                  
      double PAV,tmp;  
      COMPLEX PV,temp,temp1;
      int I,IK,J,JK,K;

// Initializations :                                                             
      DET->R = 1.0; DET->I=0.0; 
      for (I=0; I<N; I++)  {                                                                
        PC[I]= 0;                                                                
        PL[I]= 0;                                                                
        CS[I].R = 0.0;
		CS[I].I = 0.0;
      }              
// Main loop                                                                         
      for (K=0; K<N; K++) {                                                                  
// Searching greatest pivot :                                               
        PV.R=AA[K][K].R;
		PV.I=AA[K][K].I;
        IK=K;                                                                    
        JK=K;                                                                    
        PAV= CABS(PV);                                                            
        for (I=K; I<N; I++)                                                                
          for (J=K; J<N; J++)  {     
            temp.R = AA[I][J].R;
			temp.I = AA[I][J].I;
			tmp=CABS(temp);
            if (tmp > PAV) {                                      
              PV.R=AA[I][J].R;
			  PV.I=AA[I][J].I;
              PAV= CABS(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 2.2e-16
                                                                               
	if (IK!=K) {DET->R=-DET->R; DET->I=-DET->I;}                                                   
    if (JK!=K) {DET->R=-DET->R; DET->I=-DET->I;}                                                   
    CMUL(*DET,PV,&temp);
    DET->R=temp.R; DET->I=temp.I;   
    tmp= CABS(*DET);                                                            
    if (tmp < EPSMACH) {                                          
      fprintf(fp2,"\n  The complex determinant is too small!\n");                                                              
      return;                                                                  
    }                                                                   
         
// POSITIONNING PIVOT IN K,K:		
        if(IK!=K) 
// EXCHANGE LINES IK and K of matrices AA and BB			
          for (I=0; I<N; I++)  CSwap(&AA[IK][I],&AA[K][I]);                                                              
          if(M!=0)                                                           
            for (I=0; I<M; I++)  CSwap(&BB[IK][I],&BB[K][I]);
                                                                           
// PIVOT is at correct line		  
        if(JK!=K)
// EXCHANGE COLUMNS JK and K of matrix AA:			
          for (I=0; I<N; I++)  CSwap(&AA[I][JK],&AA[I][K]);                                                             
                                                                           
// The PIVOT is at correct column and is located in K,K.                                                   
                                                                               
// Column K of matrix AA is stored in CS complex vector                             
// then column K is set to zero. 		  
        for (I=0; I<N; I++) {                                                                
          CS[I].R=AA[I][K].R;
		  CS[I].I=AA[I][K].I;
          AA[I][K].R= 0.0;
		  AA[I][K].I= 0.0;
        }                                                                   
                                                                               
        CS[K].R= 0.0; CS[K].I= 0.0;                                                                
        AA[K][K].R= 1.0; AA[K][K].I= 0.0;                                                              
// Line K of matrix AA is modified:		
        tmp= CABS(PV);                                          
        if(tmp < EPSMACH) {                                            
          fprintf(fp2,"\n  COMPLEX PIVOT TOO SMALL - STOP\n");                               
          return;                                                                  
        }                                                                   
        for (I=0; I<N; I++) {                                                                
//        AA[K][I]=AA[K][I]/PV
		  CDIV(AA[K][I],PV,&temp);
		  AA[K][I].R=temp.R;
          AA[K][I].I=temp.I; 
        }
                                                                           
        if (M!=0)                                                         
		  for (I=0; I<M; I++) {                                                             
//          BB[K][I]=BB[K][I]/PV;                                                  
		  CDIV(BB[K][I],PV,&temp);
		  BB[K][I].R=temp.R;
          BB[K][I].I=temp.I;          
		  }			  

// 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]
            CMUL(CS[J],AA[K][I],&temp);
			CDIF(AA[J][I],temp,&temp1);
			AA[J][I].R = temp1.R;
            AA[J][I].I = temp1.I;
          } 
          if (M!=0)                                                       
		    for (I=0; I<M; I++) {                                                         
//            BB[J][I]=BB[J][I]-CS[J]*BB[K][I]
              CMUL(CS[J],BB[K][I],&temp);
              CDIF(BB[J][I],temp,&temp1);
			  BB[J][I].R = temp1.R;
              BB[J][I].I = temp1.I;
			}	  
		}                                                                   
// Line K is ready
      } //End of K loop

// COMPLEX 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++)  CSwap(&AA[I][J],&AA[IK][J]);                                                                
        if (M!=0)                                                         
          for (J=0; J<M; J++)  CSwap(&BB[I][J],&BB[IK][J]);

// 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++)  CSwap(&AA[I][J],&AA[I][JK]);                                                                
      fin2: ;                                                                  
// NO MORE EXCHANGE is NECESSARY                                                      
// Go to next column.
      }                                                                     
// REARRANGEMENT TERMINATED.                                                        
      return;  
  } //Matinv()                                                                       
              
                                               
/******************************************
*    MULTIPLICATION OF TWO SQUARE REAL    *
*    MATRICES                             *
* --------------------------------------- *
* INPUTS:    A  MATRIX N*N                *
*            B  MATRIX N*N                *
*            N  INTEGER                   *
* --------------------------------------- *
* OUTPUTS:   C  MATRIX N*N PRODUCT A*B    *
*                                         *
******************************************/
  void MATMUL(MATC A,MATC B, MATC C, int N)  { 
      COMPLEX SUM, TMP, TMP1;  
      int I,J,K;                                         
      for (I=0; I<N; I++)                                                                  
        for (J=0; J<N; J++) {                                                                
          SUM.R= 0.0; SUM.I=0.0;                                                                
          for (K=0; K<N; K++) {                                                              
//          SUM=SUM+A[I][K]*B[K][J]
		    CMUL(A[I][K],B[K][J],&TMP);
			CADD(SUM,TMP,&TMP1);
			SUM.R=TMP1.R; SUM.I=TMP1.I;
          }
          C[I][J].R=SUM.R;
		  C[I][J].I=SUM.I;
        }                                                                   
      return;                                                                    
  }                                                                       

                                               
/******************************************
*   MULTIPLICATION OF TWO REAL MATRICES   *
* --------------------------------------- *
* INPUTS:    A  MATRIX N*N                *
*            B  MATRIX N*M                *
*            N  INTEGER                   *
*            M  INTEGER                   *
* --------------------------------------- *
* OUTPUTS:   C  MATRIX N*M PRODUCT A*B    *
*                                         *
******************************************/
  void MATMUL1(MATC A,MATC1 B, MATC1 C, int N, int M)  { 	
      COMPLEX SUM, TMP, TMP1;  
      int I,J,K;                                         
      for (I=0; I<N; I++)                                                                  
        for (J=0; J<M; J++) {                                                                
          SUM.R= 0.0; SUM.I=0.0;                                                                
          for (K=0; K<N; K++) {                                                              
//          SUM=SUM+A[I][K]*B[K][J]
		    CMUL(A[I][K],B[K][J],&TMP);
			CADD(SUM,TMP,&TMP1);
			SUM.R=TMP1.R; SUM.I=TMP1.I;
          }
          C[I][J].R=SUM.R;
		  C[I][J].I=SUM.I;
        }                                                                
      return;                                                                    
  }         
  
// utility routines to read/write matrices
// ***************************************
     
  void MATREAD(MATC A,int N) { 
     int I,J; double x,y; 
     for (I=0; I<N; I++)
        for (J=0; J<N; J++) {
          fscanf(fp1,"%lf %lf", &x, &y); 
          A[I][J].R = x;
          A[I][J].I = y;
        }                    
  }      
  
  void MATREAD1(MATC1 A,int N,int M) { 
     int I,J; double x,y; 
     for (I=0; I<N; I++)
        for (J=0; J<M; J++) {
          fscanf(fp1,"%lf %lf",&x, &y); 
          A[I][J].R = x;
          A[I][J].I = y; 
        }                    
  }        
     

  
int WriteHead (FILE *fp, char * string)
/*====================================================================*
 *                                                                    *
 *  Put out header with text in string in file fp.                    *
 *                                                                    *
 *====================================================================*
 *                                                                    *
 *  Input parameters:                                                 *
 *  ================                                                  *
 *      string   char *string;                                        *
 *               text of headertext (last byte is a 0)                *
 *                                                                    *
 *   Return value :                                                   *
 *   =============                                                    *
 *      =  0      All ok                                              *
 *      = -1      Error writing onto stdout                           *
 *      = -2      Invalid text for header                             *
 *                                                                    *
 *====================================================================*/
{
  if (string == NULL) return (-2);

  if (fprintf (fp,"\n%s\n%s\n%s\n\n", Separator, string, Separator) <= 0)
    return (-1);

  return 0;
}  

int WriteEnd (FILE *fp)
/*====================================================================*
 *                                                                    *
 *  Put out end of writing onto file fp.                              *
 *                                                                    *
 *====================================================================*
 *                                                                    *
 *   Return value :                                                   *
 *   =============                                                    *
 *      =  0      All ok                                              *
 *      = -1      error writing onto stdout                           *
 *                                                                    *
 *====================================================================*/
{
  if (fprintf (fp,"\n%s\n\n", Separator) <= 0) return (-1);
  return 0;
}   

// main program sysmat
  void main() {
      
      char s[13],nom[9]; 
      
      printf("\nInput data file name (without .dat): "); scanf("%s",nom);                                            
                                                                               
// READ MATRIX A[N][N] TO BE INVERTED                                              
// and COPY to OUTPUT FILE                                                            
      
      strcpy(s,nom); strcat(s,".dat");
      fp1=fopen(s,"r");     
      strcpy(s,nom); strcat(s,".lst");
      fp2=fopen(s,"w");
 
      WriteHead(fp2,"  Solving a complex linear matrix system AX = B");    
      fprintf(fp2,"    (by GAUSS-JORDAN method)\n\n");  
            
      fscanf(fp1,"%d",&N);
      fprintf(fp2,"  N=%d\n",N);   
      
      MATREAD(A,N);
      MATPRINT("  MATRIX A:",A,N);
                                                                         
                                                                               
// READ RIGHT SIDE MATRIX B[N][M], IF M<>0                                              

      fscanf(fp1,"%d",&M);
      fprintf(fp2,"\n\n  M=%d\n",M); 
      
      if (M!=0) {                                                                       
        MATREAD1(B,N,M);                                                                 
        MATPRINT1("  MATRIX B :",B,N,M);
      }  
// END DATA SECTION      
      fclose(fp1);                                                                     
                                                                               
// STORING A IN A1 (Original A is lost when calling MATINV)                                                     

      for (I=0; I<N; I++)
		for (J=0; J<N; J++) {
          A1[I][J].R=A[I][J].R;
		  A1[I][J].I=A[I][J].I;
        }

// CALL INVERSION ROUTINE                                               
                                                                               
      CMATINV(N, M, A, B, &DETER);                                                    
                                                                               
// PRINT RESULTS (INVERSE MATRIX ONLY IF M=0)                                                          
                                                                               
      MATPRINT("  INVERSE MATRIX:",A,N);  
                                                                                
      if(M!=0) MATPRINT1("  SOLUTION MATRIX X:",B,N,M);  
                                                                               
      fprintf(fp2,"\n\n  DETERMINANT= (%e,%e)\n",DETER.R,DETER.I);                                                         
                                                                               
// VERIFICATION THAT NEW PRODUCT A*B EQUALS ORIGINAL B MATRIX               
// (OPTIONAL)                                                                
                                                                               
      if(M!=0) {                                                           
// CALL MULTIPLICATION ROUTINE                                         
        MATMUL1(A1,B, C,N,M); 
         
        MATPRINT1("  VERIFICATION AX = B:",C,N,M);   
      }                                                                 
      else  {                                                                      
// CALL SQUARE MULTIPLICATION ROUTINE
        MATMUL(A1,A, D,N);
          
        MATPRINT("  VERIFICATION A1 * A = I :",D,N);   
      }                                                         
                                                                               
      fprintf(fp2,"\n\n  End of results file.\n");  
      WriteEnd(fp2);
      fclose(fp2); 
      printf("\nResults in file %s.\n\n",s);
  } //main()                                                                          

// End of file csysmat.cpp - j-p moreau - Feb. 2010