/********************************************************
*          SOLVING A LINEAR MATRIX SYSTEM AX=B          *
*                by Gauss-Jordan method                 *
* ----------------------------------------------------- *
* Ref.: Mathematiques et statistiques by H. Haut, PSI   *
*       Editions, France, 1981 [BIBLI 13].              *
*                                                       *
*                    C++ Version By J-P Moreau, Paris   *
*                           (www.jpmoreau.fr)           *
*                                                       *
* INPUTS  from example file sysmat.dat,                 *
* OUTPUTS to sysmat.lst.                                *
* ----------------------------------------------------- *
* SAMPLE RUN:                                           *
* Input file                                            *
* 3                                                     *
* 2.0  -1.0   1.0                                       *
* 1.0   5.0  -2.0                                       *
* 3.0  -2.0   3.0                                       *
* 4                                                     *
* 5.0  -1.0   1.0  2.0                                  *
* 1.0   2.0   3.0  4.0                                  *
* 3.0   7.556 4.0  4.0                                  *
*                                                       *
* Output file                                           *
*                                                       *
* ------------------------------------------------------*
*  SOLVING THE MATRIX LINEAR SYSTEM AX = B              *
* ------------------------------------------------------*
*     (BY GAUSS-JORDAN METHOD)                          *
*                                                       *
*  N=3                                                  *
*                                                       *
*  MATRIX A:                                            *
*                                                       *
*   2.000000  -1.000000   1.000000                      *
*   1.000000   5.000000  -2.000000                      *
*   3.000000  -2.000000   3.000000                      *
*                                                       *
*  M=4                                                  *
*                                                       *
*  MATRIX B:                                            *
*                                                       *
*   5.000000  -1.000000   1.000000   2.000000           *
*   1.000000   2.000000   3.000000   4.000000           *
*   3.000000   7.556000   4.000000   4.000000           *
*                                                       *
*  INVERSE OF MATRIX A:                                 *
*                                                       *
*   0.785714   0.071429  -0.214286                      *
*  -0.642857   0.214286   0.357143                      *
*  -1.214286   0.071429   0.785714                      *
*                                                       *
*  SOLUTION MATRIX X:                                   *
*                                                       *
*   3.357143  -2.262000   0.142857   1.000000           *
*  -1.928571   3.770000   1.428571   1.000000           *
*  -3.642857   7.294000   2.142857   1.000000           *
*                                                       *
*  DETERMINANT= 14.000000                               *
*                                                       *
*                                                       *
*  VERIFICATION OF AX = B:                              *
*                                                       *
*   5.000000  -1.000000   1.000000   2.000000           *
*   1.000000   2.000000   3.000000   4.000000           *
*   3.000000   7.556000   4.000000   4.000000           *
*                                                       *
*                                                       *
* ------------------------------------------------------*
*                                                       *                                                                               
********************************************************/
#include "sysmat.h"
                   
// main program sysmat
  void main() {
                
      MAT  A,A1,D;                                             
      MAT1 B,C;
      REAL DETER;
    
      int I,J,M,N; 
      
      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 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("  MATRICE 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]=A[I][J];        
                                                                               
// CALL INVERSION ROUTINE                                               
                                                                               
      MATINV(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= %f\n",DETER);                                                         
                                                                               
// 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()                                                                          
                                                     

/******************************************                                     
*  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,MAT AA,MAT1 BB,REAL *DET)   { 	
      REAL PC[NMAX],PL[NMAX],CS[NMAX];                                                  
      REAL 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= ABS(PV);                                                            
        for (I=K; I<N; I++)                                                                
          for (J=K; J<N; J++)  {     
            temp = ABS(AA[I][J]);                                                        
            if (temp > PAV) {                                      
              PV=AA[I][J];                                                        
              PAV= ABS(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 1e-20
                                                                               
        if (IK!=K) *DET=-*DET;                                                   
        if (JK!=K) *DET=-*DET;                                                   
        *DET=*DET*PV;  
        temp= ABS(*DET);                                                            
        if (temp < MACH_EPS) {                                          
          fprintf(fp2,"\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= ABS(PV);                                          
        if(temp < MACH_EPS) {                                            
          fprintf(fp2,"\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()                                                                       
              
                                               
/******************************************
*    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(MAT A,MAT B, MAT C, int N)  { 
      REAL SUM;  
      int I,J,K;                                         
      for (I=0; I<N; I++)                                                                  
        for (J=0; J<N; J++) {                                                                
          SUM= 0.0;                                                                
          for (K=0; K<N; K++)                                                              
            SUM=SUM+A[I][K]*B[K][J];                                               
          C[I][J]=SUM;                                                            
        }                                                                   
      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(MAT A,MAT1 B, MAT1 C, int N, int M)  { 	
      REAL SUM;  
      int I,J,K;                                         
      for (I=0; I<N; I++)                                                                  
        for (J=0; J<M; J++) {                                                                
          SUM= 0.0;                                                                
          for (K=0; K<N; K++)                                                              
            SUM=SUM+A[I][K]*B[K][J];                                               
          C[I][J]=SUM;                                                            
        }                                                                   
      return;                                                                    
  }         
  
// utility routines to read/write matrices
// ***************************************
     
  void MATREAD(MAT A,int N) { 
     int I,J; REAL temp; 
     for (I=0; I<N; I++)
        for (J=0; J<N; J++) {
          fscanf(fp1,"%lg",&temp); 
          A[I][J] = temp;
        }                    
  }      
  
  void MATREAD1(MAT1 A,int N,int M) { 
     int I,J; REAL temp; 
     for (I=0; I<N; I++)
        for (J=0; J<M; J++) {
          fscanf(fp1,"%lg",&temp); 
          A[I][J] = temp;
        }                    
  }        
     
  void MATPRINT(char *titre,MAT 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,"%13.6f",A[I][J]);   
          if(J==N-1) fprintf(fp2,"\n");
        }                     
  }
  
  void MATPRINT1(char *titre,MAT1 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,"%13.6f",A[I][J]);   
          if(J==M-1) fprintf(fp2,"\n");
        }                     
  }        
  
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;
}   

// End of file sysmat.cpp - j-p moreau - july 1997