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

#define MAX 300

void enter_data(void);
void files(void);
void nearest_node(void);
void bc(void);
void symmetry(void);
void global1(void);
void print_global_before_BC(void);
void local(void);

void inverse_iteration(void);
void iteration(void);

void interactive(void);
void inverse_iteration(void);
void kshift(void);

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


void matrix_times_vector(long nd, double aaa[], double w[][MAX],double bbb[] );
void vector_times_vector(long nd, double *csum, double aaa[], double bbb[]);

void copy_matrix(long nd, double aaa[][MAX], double bbb[][MAX]);
void zero_matrix(long nd, double aaa[][MAX]);

void zero_vector(long nd, double aaa[]);

void print_matrix(long nd, double aaa[][MAX]);
void print_vector(long nd, double aaa[]);

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


const double TPI = 2.*atan2(0.,-1);


long i,ii,ij,ik,j,jj,jk,k,kk;
long m,n,ne,nn;

int apply;
int iflag;
int nx;
int ipm;

double aa,ah,aj,aq;
double ajl;
double ccc;
double det;
double eigen;
double kd;

double mc;

double length,inertia;
double modulus,mpv,area,mpl,h,load;

double length_mass,cm;

double scale;
double shift=0.;

double x,xx;

double kl[5][5],ml[5][5];
double kg[MAX][MAX],mg[MAX][MAX],gmg[MAX][MAX];
double a[MAX][MAX];
double xi[MAX];
double bb[MAX];
double ac[MAX][MAX];
double ww[MAX];
double b[MAX];


FILE *pFile[20];
char filename[5][20];


void main()
{

// assume evenly spaced


    printf("\n\n cantilever_with_mass.cpp \n");
	printf("\n ver 1.0  by Tom Irvine   Email: tomirvine@aol.com \n");

	printf("\n This program calculates the natural frequency of a ");
	printf("\n cantilever beam with an added point mass at an ");
	printf("\n arbitary point along the length of the beam. \n\n");

 
	printf("\n enter data \n");

    enter_data();

	printf("\n files \n");

	files();

	printf("\n local \n");

	local();

	printf("\n global1 \n");

	global1();

//    axial_stiffness();

	printf("\n symmetry \n");

	symmetry();



	if( ipm==1)
	{	
		printf("\n cm=%12.4g \n",cm);

		mg[nx][nx]+=cm;
	}



	printf("\n print global before BC \n");

	print_global_before_BC();

	printf("\n bc \n");

	bc();

	printf("\n inverse iteration \n");

	inverse_iteration();

}
void print_global_before_BC(void)
{
	for(i=1; i<=nn; i++)
	{
		for(j=1; j<=nn; j++)
		{
			fprintf(pFile[2]," %11.4g",mg[i][j]);
		}
		fprintf(pFile[2],"\n");
	}

	for(i=1; i<=nn; i++)
	{
		for(j=1; j<=nn; j++)
		{
			fprintf(pFile[2]," %11.4g",kg[i][j]);
		}
		fprintf(pFile[2],"\n");
	}
}
void symmetry(void)
{

	//  symmetric

	for(i=1; i<=nn; i++)
	{
		for(j=i+1; j<=nn; j++)
		{
			kg[j][i]=kg[i][j];
			mg[j][i]=mg[i][j];
		}
	}

}
void bc(void)
{

	// cantilever bc


	for(i=3; i<=nn; i++)
	{
		for(j=3; j<=nn; j++)
		{
			fprintf(pFile[3]," %11.4g",mg[i][j]);
		}
		fprintf(pFile[3],"\n");
	}
	for(i=3; i<=nn; i++)
	{
		for(j=3; j<=nn; j++)
		{
			fprintf(pFile[3]," %11.4g",kg[i][j]);
		}
		fprintf(pFile[3],"\n");
	}

	fclose(pFile[3]);


	n=0;

	for(i=3; i<=nn; i++)
	{
		for(j=3; j<=nn; j++)
		{
			mg[i-3][j-3]=mg[i][j];
			kg[i-3][j-3]=kg[i][j];
		}
		n++;
	}


}

void files(void)
{
   
  
   strcpy(filename[1],"km.out");
   pFile[1]=fopen(filename[1], "w");

   strcpy(filename[2],"kmg.out");
   pFile[2]=fopen(filename[2], "w");

   strcpy(filename[3],"kmbc.out");
   pFile[3]=fopen(filename[3], "w");

   strcpy(filename[4],"eigen_data.out");
   pFile[4]=fopen(filename[4], "w");

   printf("\n");

   for(i=1; i<=4; i++)
   {
      if(pFile[i] == NULL )
      {
         printf(" Failed to open file: %s \n", filename[i]);
         fclose(pFile[i]);
      }
      else
      {
         printf(" File: %s opened. \n", filename[i]);
      }
   }

}
void nearest_node(void)
{

	double xr;

	long matrix_point=1;

	double min = 2*length;

    double hh=length/double(ne);

	for(i=0;i<=ne;i++)
	{
		x=i*hh;

		xx = fabs( x -length_mass);

//        printf(" x=%10.3g  xx=%10.3g  min=%10.3g\n",x,xx,min);

		if(xx < min )
		{
			min=xx;

			nx=matrix_point;

			xr=x;
		}

		matrix_point+=2;
	}

//	printf("\n xr= %12.4g   nx= %ld  min=%12.4g\n\n",xr,nx,min);

}
void global1(void)
{

	for(i=0; i<100; i++)
	{
		for(j=0; j<100; j++)
		{
			kg[i][j]=0.;
			mg[i][j]=0.;
		}
	}

//	printf("ref3b  k11=%lf \n",kl[1][1]);

	   for(m=1; m<=ne; m++  )
	{

		i=2*m-1;

		kg[i][i]  +=kl[1][1];
		kg[i][i+1]+=kl[1][2];
		kg[i][i+2]+=kl[1][3];
		kg[i][i+3]+=kl[1][4];
		
        j=i+1;

		kg[j][j]  +=kl[2][2];
		kg[j][j+1]+=kl[2][3];
		kg[j][j+2]+=kl[2][4];

        j=i+2;

		kg[j][j]  +=kl[3][3];
		kg[j][j+1]+=kl[3][4];

        j=i+3;

		kg[j][j]  +=kl[4][4];


		mg[i][i]  +=ml[1][1];
		mg[i][i+1]+=ml[1][2];
		mg[i][i+2]+=ml[1][3];
		mg[i][i+3]+=ml[1][4];
		
        j=i+1;

		mg[j][j]  +=ml[2][2];
		mg[j][j+1]+=ml[2][3];
		mg[j][j+2]+=ml[2][4];

        j=i+2;

		mg[j][j]  +=ml[3][3];
		mg[j][j+1]+=ml[3][4];

        j=i+3;

		mg[j][j]  +=ml[4][4];

    }
			
}
void enter_data(void)
{
	printf("\n Enter length (inch)\n");
	scanf("%lf",&length);

	printf("\n Enter area moment of inertia (inch^4)\n");
	scanf("%lf",&inertia);

	printf("\n Enter cross-section area (in^2) \n");
	scanf("%lf",&area);


	int imat;

	printf("\n Enter material ");
	printf("\n 1=aluminum ");
	printf("\n 2=steel ");
	printf("\n 3=other \n");

	scanf("%d",&imat);


	if( imat==1)
	{
        modulus = 1.0e+07;
		mpv = 0.1;
    }

	if( imat==2)
	{
        modulus = 3.0e+07;
		mpv = 0.28;
    }

	if( imat != 1 && imat !=2)
	{
		printf("\n Enter elastic modulus (lbf/in^2)\n");
		scanf("%lf",&modulus);

		printf("\n Enter beam mass per volume (lbm/in^3) \n");
		scanf("%lf",&mpv);
    }
	

    printf("\n Beam mass = %8.3g lbm \n",length*area*mpv);


	mpv/=386.;

//	printf("\n Enter compressive load (lbf) \n");
//	scanf("%lf",&load);

        load = 0.; 


   
        printf("\n Enter number of elements. (suggest 8 or more) \n");
		scanf("%ld",&ne);


        printf("\n\n Add point mass ? ");
        printf("\n 1=yes  2=no \n");
  

        scanf("%d",&ipm);

        if(ipm==1)
        {       

			printf("\n Enter distance from fixed end to concentrated mass (inch)\n");
			scanf("%lf",&length_mass);

			printf("\n Enter concentrated mass (lbm)\n");
			scanf("%lf",&cm);

        	cm/=386.;

			nearest_node();

        }

	
	mpl=mpv*area;

//	printf("\n mpl= %12.4g\n",mpl);

    h=length/double(ne);

//  printf("\n h= %12.4g\n",h);  

	mc=h*mpl/420.;

//  printf("\n mc= %12.4g\n",mc); 

	nn=2 + (ne*2);

	n=nn;

}
void local(void)
{
	

	ml[1][1]=156.*mc;
	ml[1][2]=22.*mc;
	ml[1][3]=54.*mc;
	ml[1][4]=-13.*mc;

	ml[2][2]=4.*mc;
	ml[2][3]=13.*mc;
	ml[2][4]=-3.*mc;

	ml[3][3]=156.*mc;
	ml[3][4]=-22.*mc;

	ml[4][4]=4.*mc;


	double kc=modulus*inertia/pow(h,3.);


	kl[1][1]=12.*kc;

//	printf("ref1  k11=%lf \n",kl[1][1]);


	kl[1][2]=6.*kc;
	kl[1][3]=-12.*kc;
	kl[1][4]=6.*kc;

	kl[2][2]=4.*kc;
	kl[2][3]=-6.*kc;
	kl[2][4]=2.*kc;

	kl[3][3]=12.*kc;
	kl[3][4]=-6.*kc;

	kl[4][4]=4.*kc;


	for(i=1; i<=4; i++)
	{
		for(j=1; j<=4; j++)
		{
			fprintf(pFile[1]," %12.4g ",ml[i][j]);
		}
		fprintf(pFile[1],"\n");
	}
	for(i=1; i<=4; i++)
	{
		for(j=1; j<=4; j++)
		{
			fprintf(pFile[1]," %12.4g ",kl[i][j]);
		}
		fprintf(pFile[1],"\n");
	}

	fclose(pFile[1]);
}

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

void inverse_iteration(void)
{

/*
	printf("\n inverse_iteration.cpp, ver 1.1, February 19, 2004");
	printf("\n\n by Tom Irvine ");
	printf("\n Email:  tomirvine@aol.com ");


    printf("\n\n This program uses inverse iteration to determine the lowest ");
	printf("\n eigenvalue and vector of the generalized eigen problem: \n\n");
    printf("\n     K - lambda M = 0");
	printf("\n\n where K and M are symmetric matrices.");

*/
	interactive();

    if(apply==2)
	{
		kshift();
	}

	iteration();


}

void iteration(void)
{

	double ddd=0.;

// printf("\n initialization" );

       
//       printf("\n n= %ld \n",n);

       for(i=0;i<MAX;++i)
       {
		  xi[i]=double(i);
       }

	   zero_vector(MAX, bb);
	   zero_vector(MAX, ww);

	   zero_matrix(MAX, ac);

/****  copy data into reserve matrices  **/


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

	long ijkl;

	for(ijkl=0; ijkl<40; ijkl++)
	{

	   copy_matrix(n, a, kg);
	   copy_matrix(n, ac, kg);
	   copy_matrix(n, gmg, mg);
     
	   matrix_times_vector(n, b, gmg, xi );
 

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

//       printf("\n begin forward elimination \n" );

       for(i=0;i<n;++i)
       {
//	       printf(" %d ",i );
	       aq=fabs(a[i][i]);

	       for(jj=i+1;jj<n;++jj)
	       {
//	          printf("\n switching");
	          ah=fabs(a[jj][i]);

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

		         for(kk=0;kk<n;++kk)
		         {
		             ww[kk]=a[jj][kk];
		              a[jj][kk]=a[i][kk];
		              a[i][kk]=ww[kk];
		         }
		         aj=b[jj];
		         b[jj]=b[i];
		         b[i]=aj;
	          }
	        }


	       for(j=i+1;j<n;++j)
	       {
	           if(  fabs( a[i][i] ) < 1.0e-20  )
	           {
		           printf("\n diagonal term  %d %d equals zero ",i,i);
		           exit(1);
	            }
	        }

	        ik=i+1;
	        for(ii=ik;ii<n;++ii)
	        {
	            aa=a[ii][i]/a[i][i];
	            b[ii]+=-b[i]*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.;
		           }
	            }

			}

       }

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

//       printf("\n begin back substitution ");

       b[n-1]/=a[n-1][n-1];

       for(i=n-2;i>=0;i=i-1)
       {
	        for(k=i+1;k<n;++k)
	        {
//	             printf("\n %d %d",i,k);
	             b[i]+=(-b[k]*a[i][k]);
	        }
	        b[i]/=a[i][i];
       }

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


// xt M x


	   matrix_times_vector(n, bb, gmg, b );


	   vector_times_vector(n, &ccc, bb, b);

	   ccc =sqrt(ccc);


	   for(i=0;i<n;++i)
       {
		   b[i]/=ccc; 
		   xi[i]=b[i];
	   }

	   matrix_times_vector(n, bb, ac, b );

	   vector_times_vector(n, &ccc, bb, b);

	   matrix_times_vector(n, bb, gmg, b );


	   ddd=0.;

	   vector_times_vector(n, &ddd, bb, b );

//	   printf(" ccc = %lf   ddd = %lf \n",ccc,ddd);

	   ccc/=ddd;

 

   }


// eigenvector 

   printf("\n\n Fundamental frequency \n\n");
	fprintf(pFile[4],"\n\n Eigenvalue \n\n");

	   ccc+=shift;

//	   printf("   lambda = %12.5g \n",ccc);
//	   printf("    omega = %12.5g \n",sqrt(ccc));
	   printf("       fn = %12.5g Hz\n",sqrt(ccc)/TPI);


	fprintf(pFile[4],"     lambda = %12.5g \n",ccc);
	fprintf(pFile[4],"      omega = %12.5g \n",sqrt(ccc));
	fprintf(pFile[4],"         fn = %12.5g Hz \n",sqrt(ccc)/TPI);


//   printf("\n\n Eigenvector \n\n");
fprintf(pFile[4],"\n\n Eigenvector \n\n");

       h=length/double(ne);

	   fprintf(pFile[4]," 0. \t  0.  \t 0. \n");

	   k=1;
       for(ij=0;ij<n;ij+=2)
       {
//	         printf("  %d %12.5e \n",ij,b[ij]);
	        fprintf(pFile[4]," %12.5e \t %12.5e \t %12.5e \n",k*h,b[ij],b[ij+1]);
       
			 k++;
	   
	   }
	  

}
void interactive(void)
{

  apply = 1;
  shift = 0.;

}
void kshift(void)
{
		for(i=0; i< n; i++)
		{

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

               kg[i][j]-= shift*mg[i][j];

			}

		}
}

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

void matrix_times_vector(long nd, double aaa[], double bbb[][MAX],double ccc[] )
{
	   long i,j;

	   for(i=0;i<nd;++i)
       {
		    aaa[i]=0.;

	        for(j=0;j<n;++j)
	        {
	             aaa[i]+=bbb[i][j]*ccc[j];
	        }
       }
}
void vector_times_vector(long nd, double *csum, double aaa[], double bbb[])
{
	   long i;

	   *csum=0.;

	   for(i=0;i<nd;++i)
       {
            *csum+=aaa[i]*bbb[i];
	   }


}
void copy_matrix(long nd, double aaa[][MAX], double bbb[][MAX])
{

	   long i,j;

       for(i=0;i<nd;++i)
       {
	      for(j=0;j<nd;++j)
	      {
	           aaa[i][j]=bbb[i][j];
	      }
       }
}
void zero_matrix(long nd, double aaa[][MAX])
{
	   long i,j;

       for(i=0;i<nd;++i)
       {
			for(j=0;j<nd;++j)
			{

	           aaa[i][j]=0.;
			}
       }
}
void zero_vector(long nd, double aaa[])
{
	   long i;

       for(i=0;i<nd;++i)
       {
			aaa[i]=0.;
       }
}
void print_matrix(long nd, double aaa[][MAX])
{
	   long i,j;

	   printf("\n");

	   for(i=0;i<nd;++i)
       {
			for(j=0;j<nd;++j)
			{
	            printf(" %8.3g",aaa[i][j]);
			}
			printf("\n");
       }	  
}
void print_vector(long nd, double aaa[])
{
	   long i;

	   printf("\n");

	   for(i=0;i<nd;++i)
       {
	      printf(" %8.3g \n",aaa[i]);

       }	  
}