#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <conio.h>

#define SWITCHI(aaa,bbb); \
itemp=aaa;\
aaa=bbb;\
bbb=itemp;\

#define SWITCH(aaa,bbb); \
temp=aaa;\
aaa=bbb;\
bbb=temp;\

#define IDENTITY(aaa,nnn) \
for(i=0; i<nnn; i++)\
{ for(j=0; j<nnn; j++)\
{aaa[i][j]=0.;}aaa[i][i]=1.;}


#define ZERO(aaa,nnn,mmm) \
for(i=0; i<nnn; i++)\
{ for(j=0; j<mmm; j++)\
{aaa[i][j]=0.;}}

#define ZEROG(aaa,mmm,nnn) \
for(i=0; i<mmm; i++)\
{ for(j=0; j<nnn; j++)\
{aaa[i][j]=0.;}}

#define ZEROV(aaa) \
for(i=0; i<n; i++)\
{aaa[i]=0.;}


#define MATMUL(ccc,aaa,bbb,nnn) \
for(i=0; i<nnn; i++)\
{for(j=0; j<nnn; j++)\
{for(k=0; k<nnn; k++)\
{ ccc[i][j]+=aaa[i][k]*bbb[k][j];	}}}

#define MATMUL_TRANS(ccc,aaa,bbb,nra,nca,ncb) \
for(i=0; i<nra; i++)\
{for(j=0; j<ncb; j++)\
{for(k=0; k<nca; k++)\
{ ccc[i][j]+=aaa[k][i]*bbb[k][j];	}}}


#define MATEQUAL(aaa,bbb) \
for(iii=0; iii<n; iii++)\
{for(jjj=0; jjj<n; jjj++)\
{aaa[iii][jjj]=bbb[iii][jjj];}}\

#define TRANSPOSE(aaa,bbb) \
for(i=0; i<n; i++)\
{ for(j=0; j<n; j++)\
{aaa[i][j]=bbb[j][i];}}

#define PRINTMAT(aaa,nnn,mmm) \
for(iii=0; iii<nnn; iii++)\
{ for(jjj=0; jjj<mmm; jjj++)\
{printf(" %11.4g \t",aaa[iii][jjj]);}printf("\n");}

#define MAX 5100

#define MAX2 MAX*MAX

void input(void);
void pivot(void);
void partition(void);
void transformation_matrix(void);
void inverse(void);
void output(void);
void full_pivot(void);

double anum;

double C[MAX][MAX];
double CC[MAX][MAX];

double stiff[MAX][MAX];

double K21[MAX][6];
double K22[MAX][MAX];
double K22inv[MAX][MAX];

double a[MAX][MAX];

double M2[6][6];

float mass[MAX];
int mass_index[MAX][2];


// double x[MAX][MAX];
// double KID[MAX][MAX];
// double K11[6][6];
// double S2[MAX][MAX];

double max;

// int ngw[MAX];

long dof;

long mnz;

long i,j,k,n,nnn,mmm;
long iii,jjj,kji;

long ijk;

long d2;
long nr;
long nmds;

int iu;
int imu;

int rnn;

int file_choice;

double mass_scale;
double temp;

long itemp;

int nrg;

unsigned long nmn=MAX;

int iret[10];
     
FILE *pFile[10];
char filename[10][FILENAME_MAX];

int main()
{
     printf("\n\n");
     printf(" mass_properties.cpp  ver 1.3  March 24, 2010 \n");
     printf(" by Tom Irvine \n\n");

     printf(" This program calculates the mass properties for a dynamic system \n");
     printf(" from its unconstrained mass and stiffness matrices. \n\n");
     
     printf(" The model must have 6 dof per node. \n\n");
     
     printf("\n The maximum matrix size is: (%ld x %ld) \n",nmn,nmn);

     input();

 //******************************************************************************
     if(nrg != 1)
     {
        pivot();
     }
  //******************************************************************************
    
     printf("\n partition \n");
     
     partition();
  
      printf("\n transformation matrix \n");
         
     transformation_matrix();

     ZERO(a,n,n)
     MATMUL(a,stiff,C,n)
//     MATMUL_TRANS(S2,C,a)
   
     ZERO(a,n,n)  
  

//     MATMUL(a,mass,C,n)   // fix here   
 

/*
#define MATMUL(ccc,aaa,bbb,nnn) \
for(i=0; i<nnn; i++)\
{for(j=0; j<nnn; j++)\
{for(k=0; k<nnn; k++)\
{ ccc[i][j]+=aaa[i][k]*bbb[k][j];	}}}     
*/

     ZERO(a,n,n)  
/*        
     for(i=0; i<n; i++)
     {
     for(j=0; j<n; j++)
     {
     for(k=0; k<n; k++)
     { 
         if( mass_index[i][k] >=0)
         {      
            a[i][j]+=mass[mass_index[i][k]]*C[k][j];
         }	
     }}}     
*/

     
     for(ijk=0;ijk<=mnz;ijk++)
     {
           i=mass_index[ijk][0];
           k=mass_index[ijk][1];
      
           for(j=0; j<n; j++)
           {        
               a[i][j]+=mass[ijk]*C[k][j];      
           }                       
     }
      

     int mn=6;

     ZERO(M2,mn,mn)
         
     MATMUL_TRANS(M2,C,a,mn,n,mn)    
    
     output();

	 printf("\n\n Calculation complete. \n Press any key to exit.");
     getch();  
}
void output(void)
{

    fprintf(pFile[2],"\n Rigid-body Mass Matrix\n\n");
          
     for(i=0; i<d2; i++)
     {  
        for(j=0; j<d2; j++)
        {              
           if(fabs(M2[i][j])<1.0e-10)
           {
              M2[i][j]=0.;                       
           }            
           fprintf(pFile[2],"%10.5e \t",M2[i][j]);
        }
        fprintf(pFile[2],"\n");	       		
     }    
/*     
     
     for(i=0; i<d2; i++)
     {  
        for(j=0; j<d2; j++)
        {              
            fprintf(pFile[3],"%12.5e \t",S2[i][j]);		    	
        }	
        fprintf(pFile[3],"\n");	        	
     } 
     for(i=0; i<n; i++)
     {  
        for(j=0; j<d2; j++)
        {              
            fprintf(pFile[4],"%12.5e \t",C[i][j]);		    
        }		
        fprintf(pFile[4],"\n");   
     }    
     for(i=0; i<n; i++)
     {  
        fprintf(pFile[5],"%ld \n",ngw[i]);		        		
     }    
*/
     
     printf("\n\n Rigid-body Mass Matrix\n\n");
     PRINTMAT(M2,d2,d2)
     
     double xg,yg,zg;
     
     double mmass=(M2[0][0]+M2[1][1]+M2[2][2])/3.;
     
     xg= M2[1][5]/mmass;
     yg=-M2[0][5]/mmass;
     zg= M2[0][4]/mmass;

     printf("\n\n CG coordinates relative to location of selected node \n\n");

     printf(" xg = %8.4g  \n",xg);
     printf(" yg = %8.4g  \n",yg);
     printf(" zg = %8.4g  \n",zg);   
     
     fprintf(pFile[2],"\n\n CG coordinates relative to location of selected node \n\n");

     fprintf(pFile[2]," xg = %8.4g  \n",xg);
     fprintf(pFile[2]," yg = %8.4g  \n",yg);
     fprintf(pFile[2]," zg = %8.4g  \n",zg);      
     
     
     printf("\n\n Mass Values \n\n");
     
     printf(" mx= %8.4g \n",M2[0][0]);
     printf(" my= %8.4g \n",M2[1][1]);   
     printf(" mz= %8.4g \n",M2[2][2]);
     
     printf("\n Average mass = %8.4g \n",mmass);
     
     fprintf(pFile[2],"\n\n Mass Values \n\n");
     
     fprintf(pFile[2]," mx= %8.4g \n",M2[0][0]);
     fprintf(pFile[2]," my= %8.4g \n",M2[1][1]);   
     fprintf(pFile[2]," mz= %8.4g \n",M2[2][2]);    
     
     fprintf(pFile[2],"\n Average mass = %8.4g \n",mmass);
     
     
     double Ixx,Ixy,Ixz,Iyy,Iyz,Izz;
     double Ixx_cg,Ixy_cg,Ixz_cg,Iyy_cg,Iyz_cg,Izz_cg;
          
     Ixx=M2[3][3];
     Ixy=M2[3][4];
     Ixz=M2[3][5];
     Iyy=M2[4][4];
     Iyz=M2[4][5];
     Izz=M2[5][5];  
     
     Ixx_cg=Ixx-mmass*(pow(yg,2)+pow(zg,2));
     Iyy_cg=Iyy-mmass*(pow(xg,2)+pow(zg,2));
     Izz_cg=Izz-mmass*(pow(xg,2)+pow(yg,2));

     Ixy_cg=Ixy+mmass*(xg*yg);
     Ixz_cg=Ixz+mmass*(xg*zg);
     Iyz_cg=Iyz+mmass*(yg*zg);


     printf("\n\n Mass Moments of Inertia Matrix about CG \n\n");
    
     printf(" %8.4g \t %8.4g \t %8.4g \n",Ixx_cg,Ixy_cg,Ixz_cg);  
     printf(" %8.4g \t %8.4g \t %8.4g \n",Ixy_cg,Iyy_cg,Iyz_cg);   
     printf(" %8.4g \t %8.4g \t %8.4g \n",Ixz_cg,Iyz_cg,Izz_cg);
     
     
     fprintf(pFile[2],"\n\n Mass Moments of Inertia Matrix about CG \n\n");
    
     fprintf(pFile[2]," %8.4g \t %8.4g \t %8.4g \n",Ixx_cg,Ixy_cg,Ixz_cg);  
     fprintf(pFile[2]," %8.4g \t %8.4g \t %8.4g \n",Ixy_cg,Iyy_cg,Iyz_cg);   
     fprintf(pFile[2]," %8.4g \t %8.4g \t %8.4g \n",Ixz_cg,Iyz_cg,Izz_cg);   
    
               
/*     
     printf("\n\n Reduced stiffness \n");    
     PRINTMAT(S2,d2,d2)
*/
     
     fclose(pFile[2]); 
     
     printf("\n\n Output file = %s \n\n",filename[2]);
     
//     fclose(pFile[3]); 
//     fclose(pFile[4]);          
//     fclose(pFile[5]);     
}
void input(void)
{
    printf("\n Enter the units system \n");
    printf(" 1=English  2=metric ");
    scanf("%d",&iu);

    mass_scale=1.;

    if(iu==1)
    {
        printf("\n Select input mass unit \n");
        printf("  1=lbm  2=lbf sec^2/in  \n");
        scanf("%d",&imu);
        if(imu==1)
        {
           mass_scale=386.;
        }
    }
    else
    {
        printf(" mass unit = kg ");
    }

    if(iu==1)
    {
        printf(" stiffness unit = lbf/in ");  
    }
    else
    {
        printf(" stiffness unit = N/m ");
    }


    while(pFile[0] == NULL )
    {
       printf("\n\n Enter the input mass matrix filename \n");
       scanf("%s",filename[0]);   
       pFile[0] = fopen(filename[0], "rb");                  
    }  
   
    printf("\n\n\n *** Important Note: the input stiffness matrix should be double precision format ***\n");
 
    while(pFile[1] == NULL )
    {
       printf("\n Enter the input stiffness matrix filename \n");
       scanf("%s",filename[1]);   
       pFile[1] = fopen(filename[1], "rb");                  
    }  

    printf("\n\n Enter the output reduced mass matrix filename \n");
    scanf("%s",filename[2]);   
    pFile[2] = fopen(filename[2], "w"); 

/*
    printf("\n\n Enter the reduced stiffness matrix filename \n");
    scanf("%s",filename[3]);   
    pFile[3] = fopen(filename[3], "w"); 

    printf("\n\n Enter the transformation matrix filename \n");
    scanf("%s",filename[4]);   
    pFile[4] = fopen(filename[4], "w"); 
    
    printf("\n\n Enter the dof order vector filename \n");
    scanf("%s",filename[5]);   
    pFile[5] = fopen(filename[5], "w");   
*/


   strcpy(filename[4],"aK22.out");
   strcpy(filename[5],"aK21.out");
   strcpy(filename[6],"ainvK22.out");  
   strcpy(filename[7],"aCC.out");  
//   strcpy(filename[8],"KID.out");  
   
   pFile[4] = fopen(filename[4], "w"); 
   pFile[5] = fopen(filename[5], "w");
   pFile[6] = fopen(filename[6], "w");
   pFile[7] = fopen(filename[7], "w");
//   pFile[8] = fopen(filename[8], "w");      

// reading

    n=0;
    while(1)
    {
      if(fscanf(pFile[0],"%lf",&anum)<=0)
      {
         break;
      }    
      else
      {
//         printf(" n=%ld  anum=%8.4g \n",n,anum); 
      }           
      n++;
    }
    rewind(pFile[0]);
   
    n=long(sqrt(n));

    if( n<2 || n>MAX)
	{
		printf("\n\n Maximum number of dofs exceeded. \n");
		printf("\n Press any key to exit.");
		getch();
		exit(1);
	}  
	else
	{
        printf("\n n=%ld \n",n);
    }

    printf("\n reading mass \n");
    
//*****************************************   
    
    printf("\n");

    long jvm=0;

    long ijk=0;
    
    for(i=0; i< n; i++)
    {
			for(j=0; j<n; j++)
			{
				if( fscanf(pFile[0],"%lf",&anum) <=0 )
				{break;}
				
				jvm++;
				
				 if(fabs(anum)> 1.0e-12)
				 {
				   mass[ijk]=float(anum/mass_scale);
				   mass_index[ijk][0]=i;
				   mass_index[ijk][1]=j;
                   				   
//				   printf("mass_index[%ld][%ld]=%d   mass[%ld]=%8.4g\n",i,j,mass_index[i][j],ijk,mass[ijk]);
				   
				   ijk++;
                 }
			}
    }
    printf("\n");
    
    mnz=ijk-1;
    
    printf("\n reading stiffness \n");
    
    long jvk=0;
        
    for(i=0; i< n; i++)
    {
			for(j=0; j<n; j++)
			{
				if( fscanf(pFile[1],"%lf",&anum)<=0)
				{break;}
				stiff[i][j]=anum;
				
				jvk++;
			}
    }
    
   if(jvk!=jvm)
   {
      printf("\n read error:  jvm=%ld  jvk=%ld\n",jvm,jvk);
      getch();
      exit(1);
   }

//**********************************************************************

/*
     printf("\n mass matrix \n");
     PRINTMAT(mass,n,n)
          
     printf("\n stiffness matrix \n");
     PRINTMAT(stiff,n,n)
*/

//**********************************************************************
	
    fclose(pFile[0]);
    fclose(pFile[1]);  
/*
    for(i=0;i<n;i++)
    {
       ngw[i]=i+1;
    }    
 */ 
 /*   
     printf("\n Enter number of degrees-of-freedom \n");
     printf(" to retain in reduced model \n");

     scanf("%d",&nr);

     if(nr>n)
     {
       printf("\n Warning: nr reset to n-1 \n");     
       nr=n-1;
     }
*/
     nrg=6;
     
     d2=nrg;
     nmds=n-d2;
     
 //    printf("\n  n=%d  d2=%d  nmds=%d  \n",n,d2,nmds);
     
     printf("\n Enter retained node number:  \n");

     scanf("%d",&rnn);

     for(i=0;i<nrg;i++)
     {
        iret[i]=((rnn-1)*6)+i;
        
        if(  ((rnn-1)*6)+i > n)
        {
             printf("\n Input error. Retain dof number exceeds maximum input dof. \n");
             
             printf("\n Press any key to exit. \n");
                          
             getch();
             exit(1);
        }
     }

     printf("\n Retained dof \n");
     printf(" %d %d %d %d %d %d \n",iret[0],iret[1],iret[2],iret[3],iret[4],iret[5]);
         
        
}
void pivot(void)
{
     
// Diagonal Pivot

  
       for(i=0;i<6;i++)
       {
            k=iret[i];  
                       
            if(i!=k)           
            {           
                        
// Row switch
              for(j=0;j<n;j++)
              { 
                      SWITCH(stiff[i][j],stiff[k][j])                
              }
// Column switch
              for(j=0;j<n;j++)
              {                                             
                      SWITCH(stiff[j][i],stiff[j][k])                            
              }
          }
        }
     
   
//**********************************************************************************

     long ir,ic;

     for(i=0;i<6;i++)
     {
         k=iret[i];
         
         for(ijk=0;ijk<=mnz;ijk++)
         {
               ir=mass_index[ijk][0];
               ic=mass_index[ijk][1];
      
               if(ir==k)           
               {
                    mass_index[ir][0]=k;
                     mass_index[k][0]=ir;                                              
               }
               if(ic==k)           
               {
                    mass_index[ic][1]=k;
                     mass_index[k][1]=ic;                                              
               }             
               
         }            
     }

//**********************************************************************************     
     
     printf("\n\n Pivot \n");
     
/*     
     printf("\n switched mass matrix 1\n");
     PRINTMAT(mass,n,n)
     
     printf("\n switched stiffness matrix 1\n");
     PRINTMAT(stiff,n,n)
*/
         

}
void partition(void)
{
        
     
//     ZERO(K11,MAX,MAX);
     ZERO(K21,MAX,MAX);
     ZERO(K22,MAX,MAX);   
     
     ZERO(K22inv,n,n);
      
     ZERO(a,n,n);                       
     
/*
     for(i=0;i<d2;i++)
     {
        for(j=0;j<d2;j++)
        {
            K11[i][j]=stiff[i][j];
        }
     }
*/
     
     for(i=0;i<nmds;i++)
     {
        for(j=0;j<d2;j++)
        {
            K21[i][j]=stiff[i+d2][j];
        }
     }

     
     for(i=0;i<nmds;i++)
     {
       for(j=0;j<nmds;j++)
       {  
            K22[i][j]=stiff[i+d2][j+d2];
           
            if(i==j )
            {
                if( fabs(K22[i][i])<1.0e-200)
                {
                    printf("\n i=%ld  d2=%ld  i+d2=%ld   j+d2=%ld \n",i,d2,i+d2,j+d2);
                    
                    printf("\n Ref -1: error zero on diagonal  \n");
                    exit(1);
                }    
           }          
       }
    }

     
    for(i=0;i<nmds;i++)
    {
       for(j=0;j<d2;j++)
       {
           fprintf(pFile[4]," %10.4g",K22[i][j]);   
       }
       fprintf(pFile[4],"\n");
    }   
    for(i=0;i<nmds;i++)
    {
       for(j=0;j<d2;j++)
       {
           fprintf(pFile[5]," %10.4g",K21[i][j]);   
       }
       fprintf(pFile[5],"\n");
    }   
         
     
 //   printf("\n K21 \n");
 //   PRINTMAT(K21,nmds,nmds);
       
//    printf("\n K22 \n");
//    PRINTMAT(K22,nmds,nmds);
    
    
//   printf("\n nmds = %ld  n=%ld \n",nmds,n);
    
    for(i=0;i<nmds;i++)
    {
        if( fabs(K22[i][i])<1.0e-200)
        {
            printf("\n Ref 0: error zero on diagonal   i=%ld \n",i);
            exit(1);
        }
    }
    
/*    
    printf("\n K22 \n");
    PRINTMAT(K22,nmds,nmds)
*/
}
void transformation_matrix(void)
{ 
          
    inverse();
    
//    MATMUL(CC,K22inv,K21,nmds)   

/*
    ZERO(KID,nmds,nmds);  

    MATMUL(KID,K22inv,K22,nmds);  

    for(i=0;i<nmds;i++)
    {
       for(j=0;j<nmds;j++)
       {
           fprintf(pFile[8]," %10.5g \t",KID[i][j]);   
       }
       fprintf(pFile[8],"\n");
    }  
*/

    
    for(i=0;i<nmds;i++)
    {
       for(j=0;j<nmds;j++)
       {
           fprintf(pFile[6]," %10.4g \t",K22inv[i][j]);   
       }
       fprintf(pFile[6],"\n");
    }   
    
    ZERO(CC,nmds,nmds);  
    
//    printf("\n ** ref 1 ** \n");
    
    for(i=0; i<nmds; i++)
    {
       for(j=0; j<d2; j++)
       {
          for(k=0; k<nmds; k++)
          { 
              CC[i][j]+=K22inv[i][k]*K21[k][j];
          }
       }
    }
    
    for(i=0; i<nmds; i++)
    {
       for(j=0; j<d2; j++)
       {
              fprintf(pFile[7]," %10.4g",CC[i][j]);          
       }
       fprintf(pFile[7],"\n");      
    }  

    
//     printf("\n K21 \n");
//     PRINTMAT(K21,nmds,d2);  
     
//     printf("\n n=%ld nmds=%d\n",n);
     
//     printf("\n CC \n");
//     PRINTMAT(CC,nmds,d2);   
    

    IDENTITY(C,d2)

    for(i=0;i<nmds;i++)
    {
       for(j=0;j<d2;j++)
       {
         C[i+d2][j]=-CC[i][j];  // note negative polartiy
       }
    }
/*    
    printf("\n\n C \n");
    PRINTMAT(C,n,d2);  
*/    
}
void inverse(void)
{

     long i,ii,ik,imethod;
     long j,jj,k,kk,n,nv;
     
     double aa,ah,aq;
     
     n=nmds;
     
 //    printf("\n inverse \n");

//      K22inv    

/****** begin forward elimination  *********/

        MATEQUAL(a,K22)
                               

        IDENTITY(K22inv,n)


//        printf("\n a matrix \n");
//        PRINTMAT(a,n,n)


         for(i=0;i<n;i++)
         {
              if( fabs(a[i][i])<1.0e-200)
              {
					  printf("\n Ref 1:  Error:  zero in diagonal.  i=%ld\n",i);

					  exit(1);
              }	                
         }    



//   preliminary reorder


        for(i=0;i<n;i++)
        {
             aq=fabs(a[i][i]);

	          for(jj=i+1;jj<n;jj++)
	          {

	            ah=fabs(a[jj][i]);

	            if(aq<ah)
	            {
				    aq=fabs(a[jj][i]);

					for(kk=0;kk<n;kk++)
					{
						SWITCH(a[jj][kk],a[i][kk])
					}
					for(nv=0; nv<n; nv++)
                    {
						SWITCH(K22inv[jj][nv],K22inv[i][nv]);
					}	 
	            }
			  }                                          
        }


//  Begin Gaussian 

         for(i=0;i<n;i++)
         {

             aq=fabs(a[i][i]);

	          for(jj=i+1;jj<n;jj++)
	          {

	            ah=fabs(a[jj][i]);

	            if(aq<ah)
	            {
				    aq=fabs(a[jj][i]);

					for(kk=0;kk<n;kk++)
					{
						SWITCH(a[jj][kk],a[i][kk])
					}
					for(nv=0; nv<n; nv++)
                    {
						SWITCH(K22inv[jj][nv],K22inv[i][nv]);
					}	 
	            }
			  }

	          ik=i+1;

	          for(ii=ik;ii<n;ii++)
	          {
				  if( fabs(a[i][i])<1.0e-200)
				  {
					  printf("\n Ref 2: Error:  zero in diagonal.  i=%ld\n",i);

					  exit(1);
				  }

	              aa=a[ii][i]/a[i][i];

				  for(nv=0; nv<n; nv++)
				  {
	                 K22inv[ii][nv]+=-K22inv[i][nv]*a[ii][i]/a[i][i];
				  }
                  for(j=i;j<n;j++)
	              {
		             a[ii][j]+=-aa*a[i][j];
		             if( fabs(a[ii][j]) <1.0e-10)
		             {
		                 a[ii][j]=0.;
		             }
	              }
	          }
         }
/*         
     printf("\n a matrix  after fwd elim\n");
     PRINTMAT(a,n,n)   
*/


/********* back substitution ***********************************/

		 for(nv=0; nv<n; nv++)
		 { 
			   K22inv[n-1][nv]/=a[n-1][n-1];

			   for(i=n-2;i>=0;i=i-1)
			   {              
				  for(k=i+1;k<n;k++)
	              {
	                  K22inv[i][nv]+=(-K22inv[k][nv]*a[i][k]);
	              }
	              K22inv[i][nv]/=a[i][i];

			   }
		 }
		 
 
/*
         printf("\n x matrix \n");
		 PRINTMAT(x,n,n)
		 
         printf("\n K22inv matrix \n");
		 PRINTMAT(K22inv,n,n)		 
*/	
	 
}