//   SRS Synthesis using Damped Sinusoids

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

#define MAX 400
#define FMAX 400
#define TMAX 100000

void interp_ref(void);
void th_syn(void);
void srs(void);
void find_srs_scale(void);
void scale_th(void);
void srs_error(void);
void check(void);

void datafile(void);
void rescale(void);

void sintime(void);

void printresults(void);

const double max=32767.;
const double pi=atan2(0.,-1.);
const double tpi=2.*pi;
const double octave=pow(2.,(1./12.));


double flast;



double sym;

float dampt[FMAX];

float delay[FMAX];



float best_dampt[FMAX];
double errlit=1.0e+90;
float ft,tt;

double error_before;

float f[MAX],a[MAX],slope[MAX];

float freq[FMAX],amp[FMAX];
float rf[FMAX],ra[FMAX];
float best_amp[FMAX];

float best_delay[FMAX];


float sss[FMAX][TMAX],sum[TMAX];


float sr,dur,dwn,dt;


long ntt,iamax;


long i,ijk,j,k,last,n,ns,nss;

long icase,iacase,ia;

float a1[FMAX],a2[FMAX];
float b1[FMAX],b2[FMAX];
float cosd;
float damp;
float domegadt;



double last_f,last_a;
    
double error;


double fe,fer;


float omega, omegad;
    
float sind;
   
float xb[FMAX], xbb[FMAX];
float x[FMAX],xmax[FMAX],xmin[FMAX];
float y;
float yb[FMAX];

long jnum;


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


int main()
{

	
	printf("\n\n\n damped_sine_syn.cpp    ver 3.0    April 5, 2010");
	printf("\n\n by Tom Irvine ");
	printf("\n Email:  tomirvine@aol.com \n\n\n");

	printf("\n This program synthesizes a time history to satisfy a shock ");
	printf("\n response spectrum specification.  Damped sinusoids are used ");
	printf("\n for the synthesis. \n\n");

	for(i=0; i<FMAX; i++)
	{
		ra[i]=0.00001;
	}

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

		f[i]=0.;
		a[i]=0.;

		slope[i]=0.;
	}

	strcpy(filename[1],"accel.out");
	pFile[1]=fopen(filename[1],"w");

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

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



	strcpy(filename[5],"srs_spec.out");
	pFile[5]=fopen(filename[5],"w");



	datafile();


	long seed;

	double dr;



	printf("\n Enter an arbitary integer. \n");

    scanf("%s",&seed);



    for(i=1;i<seed;i++)
	{

	   dr=rand()/max;

    }

	for(i=0; i<(n-1); i++)
	{
		slope[i]=log(a[i+1]/a[i])/log(f[i+1]/f[i]);
	}

	printf("\n Enter duration (sec). \n");
	scanf("%f",&dur);



	sr=10*last_f;



	printf("\n\n Recommend sample rate >= %12.6g samples/sec\n",sr);

    printf("\n Enter sample rate \n");

	scanf("%f",&sr);




	dt=1./sr;
	ns=long(sr*dur)+20;

	if(ns > TMAX)
	{
		printf("\n Excessive number of samples. Reduce duration.\n");

	    printf("\n\n\n Press any key to exit.");

	    getch();


		exit(1);
	}

	printf("\n\n sample rate = %12.4g samples/sec\n",sr);


	long idamp;



	printf("\n Enter damping format. \n 1= damping ratio   2= Q \n");

	scanf("%ld",&idamp);



	if(idamp == 1 )
	{

	     printf("\n Enter damping ratio (typically 0.05) ");

		 scanf("%f",&damp);

	}

	else
	{

	     printf("\n Enter amplification factor Q (typically 10) ");

	     scanf("%f",&damp);

	     damp = 1./(2.*damp);

	}


	if(damp > 0.9 || damp < 0.)
	{
		printf("\n Damping input error.  damp=%8.4g ",damp);
		

		printf("\n\n\n Press any key to exit.");

	    getch();

		

		exit(1);
	}

//	printf("\n Enter damping value for time history sinusoids \n (suggest value between 0.00 and 0.10). \n ");
//	scanf("%f",&dampt[][]);


	printf("\n Enter number of iterations for outer loop (min=10 max=5000). \n ");
	scanf("%ld",&iamax);

	if(iamax < 10 ){iamax=10;}
	if(iamax > 5000 ){iamax=5000;}

//  	printf("\n Enter number of iterations for inner loop (min=5 max=100). \n ");
//	scanf("%ld",&ntt);



	ntt=80;

//	if(ntt < 5 ){ntt=5;}
//	if(ntt > 100 ){ntt=100;}

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

	printf("\n Interpolating reference. \n\n");
	interp_ref();



	for(ia=1; ia<=iamax; ia++)
	{
//		printf("\n Calculating damped sine parameters. ");
		sintime();

//		printf("\n Synthesizing initial time history. \n\n");
		th_syn();
		srs();



		for(ijk=0; ijk < ntt; ijk++)
		{
			find_srs_scale();
			scale_th();
			srs();
			srs_error();
			check();

			printf("%ld %ld  error= %12.4g   best= %12.4g  \n",ia,ijk,error,errlit);


            if(ijk>2 && error > 1.001*error_before){break;}
            if(ijk>1 && error > 1.0e+90){break;}


			error_before=error;
		}
		printf("\n");
	

	}


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

	printf("\n\n\n Best case is %ld %ld \n",iacase,icase);

	printf("\n\n Output files for best case: ");
	printf("\n\n  %s - positive and negative shock response spectrum ",filename[3]);
	printf("\n  %s - time history ",filename[1]);



	strcpy(filename[5],"srs_spec.out");

	printf("\n\n  %s - specification",filename[5]);

	printf("\n\n Please call the output files into your own graphics program. \n\n");

	printf("\n\n\n Press any key to exit.");
	getch();


}

void interp_ref()
{
	rf[0]=f[0];
	ra[0]=a[0];

	for(i=1; i<FMAX; i++)
	{
		rf[i]=rf[i-1]*octave;


		if( rf[i] == f[n-1] ){break;}
		if( rf[i] > f[n-1] )
		{

			rf[i]=f[n-1];

			break;

		}
	}
	last=i;


	printf("\n\n last = %ld   flast = %12.4g\n",last,f[n-1]);

	for(i=1; i<last; i++ )
	{
		for(j=0; j<n; j++)
		{
		   if( rf[i] > f[j] && rf[i] <f[j+1])
		   {
			   ra[i]=a[j]*pow( (rf[i]/f[j]),slope[j] );

			   break;
		   }

		   if( rf[i]==f[j])
		   {

			   ra[i]=a[j];

               break;

		   }

		   if( rf[i]==f[j+1])
		   {

			   ra[i]=a[j+1];

		       break;	

		   }
		}
	}


	rf[last]=last_f;

	ra[last]=last_a;

	for(i=0; i<=last; i++)
	{
		fprintf(pFile[2],"%15.5e %15.5e\n",rf[i],ra[i]);
	}

	fclose(pFile[2]);

//	exit(1);



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

}
void th_syn()
{

	 double big=0.;
	 double small=0.;


	 for(j=0; j<ns; j++)
	 {
		 sum[j]=0.;



		 if(j>=20)
		 {
			for(k=0; k<=last; k++)
			{		
				sum[j]+=amp[k]*sss[k][j-20];
			}
		 }


		 if( sum[j] < small ){small=sum[j];}

		 if( sum[j] > big   ){  big=sum[j];}
	 }
     sum[0]=0.;


	 sym = fabs(20*log10( big/fabs(small)));

	 if(sym ==0)
	 {

		 printf("\n error: sym=%8.4g \n",sym);

		 exit(1);

	 }
}
void srs(void)
{

/******************************** initialize arrays ******************/
   for(i=0; i< FMAX; ++i)
   {
      xmax[i]=0.;
      xmin[i]=0.;
      
	     x[i]=0.;
	    xb[i]=0.;
       xbb[i]=0.;
	    yb[i]=0.;
   }
    
/********************* intialize frequency array *********************/
  
   jnum=j;

/*********************** initialize filter coefficients ***************/

   for(j=0;j<=last;++j)
   {
      omega=freq[j];  // already has tpi;

      omegad=omega*sqrt(1.-pow(damp,2));
      cosd=cos(omegad*dt);
      sind=sin(omegad*dt);
      domegadt=damp*omega*dt;
      a1[j]=2.*exp(-domegadt)*cosd;
      a2[j]=-exp(-2.*domegadt);
      b1[j]=2.*domegadt;
      b2[j]=omega*dt*exp(-domegadt);
      b2[j]*=( (omega/omegad)*(1.-2.*pow(damp,2))*sind -2.*damp*cosd );

   }


//   strcpy(filename[7],"ct.out");

//   pFile[7]=fopen(filename[7],"w");


/********************** response calculation *******************/
   
   long nsrs = ns + long((0.8/rf[0])/dt);

   for(i=0; i<nsrs; i++)
   {
      if(i<ns)
	  {
	      y=sum[i];
      }
	  else
	  {
		  y=0.;
	  }

//	  fprintf(pFile[7]," %12.5e %12.5e \n",(i-20)*dt,y);

	  for(j=0; j<=last; ++j)
	  {
	    x[j]=  a1[j]* xb[j]
		  +a2[j]*xbb[j]
		  +b1[j]*y
		  +b2[j]*yb[j];

	    if(x[j] > xmax[j])
	    {
	       xmax[j]=x[j];
	     
	    }
	    if(x[j] < xmin[j])
	    {
	       xmin[j]=x[j];
	      
	    }
	    xbb[j]=xb[j];
	     xb[j]= x[j];
	     yb[j]= y;
	  }

   }


//   fclose(pFile[7]);
   
/**************************** print response spectra *****************/
   
   for(j=0; j<=last; ++j)
   {
	  xmin[j]=fabs(xmin[j]);
	  xmax[j]=fabs(xmax[j]);
	  


		if(fabs(xmin[j]) <1.0e-90)
		{

			printf("\n error fabs(xmin[%ld])=%8.4g \n",j,xmin[j]);

			exit(1);

		}

		if(fabs(xmax[j]) <1.0e-90)
		{

			printf("\n error fabs(xmax[%ld])=%8.4g \n",j,xmax[j]);

			exit(1);

		}

   }
  
}
void find_srs_scale(void)
{


        double xx; 

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


            xx = (xmax[i] + fabs(xmin[i]))/2.; 


			if(xx <1.0e-90)
			{

				printf("\n error xx=%8.4g \n",xx);

				exit(1);

			}


			amp[i]*=pow((ra[i]/xx),0.25);
		}

		
}

void srs_error(void)
{
	error=0.;

	float db;


	float dbmax=0.;


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

       if( xmax[i] <=0 || xmin[i] <=0)
	   {

           printf(" %ld  %lf %lf \n",i,xmax[i],xmin[i]);



		   error=1.0e+99;

		   break;

	   }


	   db=fabs(20.*log10(xmin[i]/ra[i]));

//      error+=db;



	    if(db>error)
	    {

		   error=db;

	       fe=rf[i];

	   }   



	   db=fabs(20.*log10(xmax[i]/ra[i]));

//	   error+=db;  

	   
	   if(db>error)
	   {

		   error=db;

	       fe=freq[i];

	   }   
	}


}
void scale_th(void)
{
	 double big=0.;

	 double small=0.;


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

		 sum[j]=0.;


		 if(j>=20)
		 {


			for(k=0; k<=last; k++)
			{		
				sum[j]+=amp[k]*sss[k][j-20];
			}

		 }


		 if( sum[j] < small ){small=sum[j];}

		 if( sum[j] > big   ){  big=sum[j];}

	 }

     sum[0]=0.;



	 sym = fabs(20*log10( big/fabs(small)));


}
void check(void)
{

	if( error < errlit && sym <= 2.5)
	{
	
		iacase =ia;
		icase = ijk;

		errlit = error;


		printf("\n\n %ld %ld  best= %12.4g  sym= %12.4g\n",ia,ijk,error,sym);

		for( k=0; k<=last; k++)
		{
			best_amp[k]=amp[k];

            best_dampt[k] = dampt[k];

			best_delay[k]=delay[k];
		}



		fer=fe;



		printresults();
	}
}

void datafile(void)
{
	      long imethod;		  
		  float ff,aa;

	      printf("\n Select data input method.");
          printf("\n 1=keyboard  2=file \n     ");
	      scanf("%ld",&imethod);
		  
		  if(imethod==1)
		  {    
              printf("\n How Many Breakpoints (min=2)? \n");
          	  scanf("%ld",&n);

              if(n < 2)
			  {
				  printf( "\n Input error \n");
				  exit(1);
			  }
              else
			  {
                  printf("\n Enter Points (free-format) ");
                  printf("\n Natural Freq(Hz)  SRS(G)   \n\n ");
          
                  for( i=0; i<n; i++)
			      {
				     scanf("%f %f",&f[i],&a[i]);

					 flast=f[i];

					 last_f=f[i];

					 last_a=a[i];

					 fprintf(pFile[5],"%f %f \n",f[i],a[i]);
			      }
			  }
			 
          }
          else
		  {

			 printf( "\n The file must contain two columns: \n");
             printf( " Natural Freq(Hz)  SRS(G)    \n");

			 printf("\n Enter filename.\n ");
			 scanf("%s",filename[0]);
			 pFile[0]=fopen(filename[0], "rb");

			 if(pFile[0] == NULL)
			 {
				 printf( "\n Input file error \n");				 

	             printf("\n\n\n Press any key to exit.");

	             getch();


				 exit(1);
			 }

			 i=0;

			 while( fscanf(pFile[0], "%f %f", &ff, &aa) > 0 )
			 {
				 if( ff > 0. )
				 {
				 	 f[i]=ff;
					 a[i]=aa;



					 fprintf(pFile[5],"%f %f \n",f[i],a[i]);


					 flast = ff;

					 last_f=ff;

					 last_a=aa;


					 i++;

				 }
                 if(i>=MAX){ break; }  
			 }
			 fclose( pFile[0]);

			 n=i;
			 printf("\n n=%d \n", n);
          }

		  fclose(pFile[5]);

//		  printf("\n Ref 2 ");
}

void sintime(void)
{

	 if( ia < long(1.*iamax/4.) ||  (rand()/max) < 0.6 )
	 {

		for(i=0; i<=last; i++)
		{
			freq[i]=rf[i]*tpi;

			amp[i]=ra[i]/10.*(0.5-rand()/max);

			delay[i] = 0.015*dur*rand()/max;

			dampt[i] = 0.003 + 0.035*rand()/max;
		}

	 }
	 else
	 {

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

			amp[i]= best_amp[i]*(0.99+0.02*rand()/max);

			delay[i]= best_delay[i];

			dampt[i]= best_dampt[i]*(0.99+0.02*rand()/max);

		}
	 }
	 
	 for(k=0; k<=last; k++)
	 {
        for(j=0; j<ns; j++)
	    {
			tt=dt*j;
		
            sss[k][j]=0.;
		
			if( tt > delay[k])
			{

			    tt-=delay[k];

				ft=freq[k]*tt;

				sss[k][j]=exp(-dampt[k]*ft)*sin(ft);

			}
		}
	 }
}
void rescale(void)
{
    double least=-1.0e+90;

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


		if(fabs(xmax[i])<1.0e-90)
		{

			printf("\n error: xmax[%ld]=%8.4g \n",i,xmax[i]);

			exit(1);

		}

		if(fabs(xmin[i])<1.0e-90)
		{

			printf("\n error: xmin[%ld]=%8.4g \n",i,xmin[i]);

			exit(1);

		}


	     if( ( ra[i]/xmax[i] ) > least)
		 { 
			 least = ( ra[i]/xmax[i] );
		 }
		 if( ( ra[i]/xmin[i] ) > least)
		 { 
			 least = ( ra[i]/xmin[i] );
		 }
	}

	for(i=0; i<=last; i++)
	{
	     best_amp[i]*=least;
	}

}

void printresults(void)
{

		strcpy(filename[1],"accel.out");

	pFile[1]=fopen(filename[1],"w");



	strcpy(filename[3],"srs_syn.out");

	pFile[3]=fopen(filename[3],"w");




//	rescale();

//	regen_best();

//	srs();

    float xx,ll;

	for(j=0; j<ns; j++)
	{
       if( j > (0.98*ns) )
	   {
		   xx = float(j) - (0.98*ns);
		   ll  = 0.02*ns;
		   
		   sum[j]*= (1.- (xx/ll) );
	   }                
		fprintf(pFile[1],"%16.5e \t %16.5e\n",(j-20)*dt,sum[j]);
	}


	srs();

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

		if( rf[i] > flast){ break; }

		fprintf(pFile[3],"%15.5e %15.5e %15.5e\n",rf[i],xmax[i],xmin[i]);

	}

	fclose(pFile[1]);

	fclose(pFile[3]);



    strcpy(filename[4],"dsine_table.out");

	pFile[4]=fopen(filename[4],"w");



//	print(" k= %ld  last= %ld \n",k,last);


	for( k=0; k<=last; k++)
	{

		fprintf(pFile[4]," %12.4lf %12.4lf %12.4lf %12.5lf\n",rf[k],best_amp[k],best_dampt[k],best_delay[k]);

//		printf(" %12.4lf %12.4lf %12.4lf %12.4lf %12.5lf\n",rf[k],best_amp[k],best_phase[k],best_dampt[k],best_delay[k]);

	}
	fclose(pFile[4]);

}