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

#include <time.h>
#include <sys/types.h>
#include <sys/timeb.h>

#define MAX 1000000
#define MAXP 500


void sf(void);
void elapsed();
void srmany(void);



void header(void);


int kkj=1;

time_t ltime;
time_t ltimen;

long i,ie,ik,j,k,kk,l,n,nt,num,num1,num2,nwave;

double aaa,am,ave,sd,sq;
double error,errormax;

double x1,x2,x3;
double x1r,x2r,x3r;
double t[MAX],a[MAX],amp[MAX];
double f[MAXP],p[MAXP],fl[MAXP],fu[MAXP];
double fc,flow,fup;

double pl,pu;
double xab,xfb,xpb,y;

double tol,tstart,tt;



double tbegin,tend;

double sr,fmax;



double bbb;

long drate;



long nnn;

const double pi=atan2(0.,-1);
const double tp=2.*pi;
const double max=32767.;




FILE *pFile[20];
char filename[6][FILENAME_MAX];


void main()
{
		time( &ltime );
		tstart=double(ltime);

//		strcpy(filename[0],"sf.in");

		printf("\n sinefind.cpp  version 1.7  December 11, 2008 \n");
		printf("\n By Tom Irvine   Email:  tomirvine@aol.com \n");

		printf("\n This program performs a sine curve-fit for an input file. \n");

		printf("\n The input file must be: time(sec) and amplitude(units) ");
		printf("\n Format is free, but no header lines allowed.\n");
		printf("\n Please enter the input filename. \n");





		scanf("%s",filename[1]);

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

		while(pFile[1] == NULL )

		{

			printf("\n Failed to open file: %s \n", filename[1]);

				

			printf("\n Please enter the input filename: \n");



			scanf("%s",filename[1]);

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

		}

		printf(" File: %s opened. \n", filename[1]); 



		strcpy(filename[2],"b.out");

		printf("\n\n");

		for(i=2; i<=2; i++)
		{
			pFile[i]=fopen(filename[i],"w");

			printf(" File: %s opened. \n", filename[i]);
		}
		printf("\n");



		header();



		double tfirst;



		fscanf(pFile[1],"%lf %lf",&tfirst,&bbb);

		

		for(i=1; i<MAX; i++)

		{

			if( fscanf(pFile[1],"%lf %lf",&aaa,&bbb) <=0 ){break;}



		}

		rewind(pFile[1]);



		printf("\n  Data: Start time=%8.4g sec   End time=%8.4g sec \n\n",tfirst,aaa);





		printf("\n Enter the analysis starting time (sec)\n");

		scanf("%lf",&tbegin);



		printf("\n Enter the analysis ending time (sec)\n");

		scanf("%lf",&tend);



		if( tend <= tbegin)

		{

			printf("\n Error: end time must be > start time. \n");

			exit(1);

		}



		ik=0;
       
		fprintf(pFile[2],"%ld",ik);
	

		long jk=0;



		header();
       
		for(i=1; i<MAX; i++)
		{
			if( fscanf(pFile[1],"%lf %lf",&aaa,&bbb) <=0 ){break;}



			if(aaa>=tbegin && aaa<=tend)

			{

				t[jk]=aaa;

				a[jk]=bbb;

				amp[jk]=bbb;



				jk++;

			}
		}

		num2=jk-1;


		printf("\n number of input file points = %ld",num2);

        sr=(t[num2]-t[1])/(num2-1);
		sr=1./sr;



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



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

		

		x1r=0.;

		x2r=0.;

		x3r=0.;



		printf("\n\n Enter the number of trials per frequency. \n");

		scanf("%ld",&nt);

		

		long nfr;



		printf("\n\n Enter the number of frequencies. \n");

		scanf("%ld",&nfr);

      

		fmax=0.;



		for(i=1; i<=nfr; i++)

		{

		  printf("\n\n Enter frequency %ld \n",i);

		  scanf("%lf",&fc);



		  printf("\n\n Enter frequency tolerance %ld \n",i);

		  scanf("%lf",&tol);

          

		  fl[i]=fc-tol;

          fu[i]=fc+tol;



		  if(fc>fmax){fmax=fc;}



		  printf("\n");

		}



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

		drate=long(sr/(fmax*16.));

		if(drate <2){drate=1;}
		if(drate > 10){drate=10;}

//		printf("\n sample rate = %10.4g  dr= %ld \n",sr,drate);

		xab=0.;
		xfb=0.;
		xpb=0.;

       
	   for(ie=1; ie<=nfr; ie++)
	   {


          flow=fl[ie];
           fup=fu[ie];





          printf("\n frequency case %ld ",ie);
          printf("\n fup=%12.8g  flow=%12.8g \n",fup,flow);




			sf();

			elapsed();
          
			printf("\n");
		}
	   	


		for(i=1; i<=2; i++)
		{
			fclose(pFile[i]);
		}

	    srmany();

	printf("\n\n");
}

void sf(void)
{
		ave=0.;
		sq=0.;

		for(i=1; i<=num2; i++)
		{
          ave+=a[i];
          sq+=pow(a[i],2.);
		}

		ave/=double(num2);
	   
		sd=sqrt( (sq/num2) -pow(ave,2.));

		printf("\n ave=%12.4g  sd=%12.4g",ave,sd);

		am=2.*sd;
		n=num2;
		pu=1.;
		pl=0.;
       
 //       printf("\n n=%ld  nt=%ld  num2=%ld \n",n,nt,num2);

		errormax=1.0e+53;

		printf("\n\n  Trial     Error      Amplitude   Freq(Hz)   Phase(rad) \n");


		double c1=0.;

		double c2=0.;

       

		double deltaf = (fup-flow)/2.;



		double rn;

		double rm;

		double rp;



		x1r=0.;

		x2r=0.;

		x3r=0.;



		nnn= long(nt*0.20);


		for(j=1; j<=nt; j+=drate)
		{

			rn=rand()/max;

			rm=rand()/max;

			rp=rand()/max;



//			if(j==1){printf(" %ld  %8.3f  %8.3f \n",j,fup,flow);}



			x1=rand()/max;

			x2=rand()/max;

			x3=rand()/max;



			x1=am*x1;

			x2=((fup-flow)*x2+flow)*tp;

			x3*=tp;





			if(j>nnn)

			{ 

				c1=double(j-nnn)/double(nt-nnn);



				c2=(1.-c1);



				c2=pow(c2,2.4);



				deltaf = c2*(fu[ie]-fl[ie])/2.;



//				printf(" deltaf=%8.3f  (x2r/tp)=%8.3f \n",deltaf,(x2r/tp));



//				if(deltaf<0.01){break;}



				fup = (x2r/tp)+deltaf;

				flow= (x2r/tp)-deltaf;



				if(flow<fl[ie]){flow=fl[ie];}

				if(fup >fu[ie]){ fup=fu[ie];}

			}

			

			if(j>100)

			{



			if(rm>0.4 && rm<0.8)

			{

				x1=x1r*(0.85+0.45*rand()/max);   // 0.85 to 1.25				

            }

			if(rm>0.8 && rm<1.0)

			{

				x1=x1r*(0.99+0.02*rand()/max);   // 0.85 to 1.25				

            }



			if(rp>0.8)

			{

				x2=x2r*(0.99+0.02*rand()/max);

			}





			if(rn>0.4 && rn<=0.6)

			{ 

				x3=(x3r+tp)*(0.8+0.4*rand()/max);

			}

			if(rn>0.6 && rn<=0.8)

			{ 

				x3=(x3r+tp)*(0.9+0.2*rand()/max);

			}

			if(rn>0.8 && rn<=1.0)

			{

				x3=(x3r+tp)*(0.95+0.1*rand()/max);			

			}



			double rq=rand()/max;



			if(rq>0.90 && rq <=0.925)

            {

				x1=x1r*(0.995+0.01*rand()/max);

				x2=x2r;

				x3=x3r;

			}

			if(rq>0.925 && rq <=0.95)

            {

				x1=x1r;

				x2=x2r*(0.995+0.01*rand()/max);

				x3=x3r;

			}

			if(rq>0.95 && rq <=0.975)

			{

   				x1=x1r;

				x2=x2r;

				x3=(x3r+tp)*(0.995+0.01*rand()/max);         

			}

			if(rq>0.975 && rq <=1.0)

            {

				x1=x1r*(0.995+0.01*rand()/max);

				x2=x2r*(0.995+0.01*rand()/max);

				x3=(x3r+tp)*(0.995+0.01*rand()/max);

			}

			}



			if(x3>tp){x3-=tp;}

			if(x3>tp){x3-=tp;}


       

			if(j==1)

			{

				x1=0.;

			}

			if(j<5)

			{

				x2=0.5*(fup-flow)*tp;

			}

			error=0.;
          
			for(i=1; i<=n; i++)
			{
				tt=t[i];
				y=x1*sin(x2*tt-x3);
				error+=pow((a[i]-y),2.);
			} 
			error=sqrt(error);
		  
			if(error<errormax)
			{
				x1r=x1;
				x2r=x2;
				x3r=x3;

				printf("\n %6ld  %13.4e  %8.3g %8.3f %8.3f ",j,error,x1,x2/tp,x3);


				errormax=error;

				if(j> nnn)
				{

					fu[ie]=fup;

					fl[ie]=flow;


				}

			}
       }

       ave=0.;
       sq=0.;

    
		for(i=1; i<=n; i++)
		{
          tt=t[i];      
          a[i]=a[i]-x1r*sin(x2r*tt-x3r);
          ave+=a[i];
          sq+=pow(a[i],2.);
		}

		ave=ave/n;   
		sd=sqrt( (sq/n) -pow(ave,2.));
	
		printf("\n %12.4g %12.4g",ave,sd);

		fprintf(pFile[2],"\n %14.7g %14.7g %14.7g",x1r,x2r,x3r);


		xab=x1r;
		xfb=x2r;
		xpb=x3r;

}
void elapsed()
{

	double tnow;
   
	time( &ltimen );

	tnow=double(ltimen);

	double ttt=tnow-tstart;
				
	printf("\n\n elapsed time = %12.5g sec\n",ttt);
}



void srmany(void)
{
		strcpy(filename[0],"b.out");
		strcpy(filename[2],"error.out");
		strcpy(filename[3],"synth.out");

		printf("\n\n");

		for(i=0; i<=3; i++)
		{
			if(i>=2)
			{
				pFile[i]=fopen(filename[i],"w");
			}
			else
			{
				pFile[i]=fopen(filename[i],"rb");
			}

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

		kk=0;
      
		fscanf(pFile[0],"%ld",&ik);

		for(i=1; i<=1000; i++)
		{
			if(fscanf(pFile[0],"%lf %lf %lf",&a[i],&f[i],&p[i]) <=0 ){break;}

           printf("\n amp=%10.4f   freq=%10.3f   phase=%10.3f",a[i],f[i]/tp,p[i]);

		}
		num=i-1;

		fclose(pFile[0]);

//		printf("\n\n reference 1 ");



		long limit=num2;

//		printf("\n reference 2 ");

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

		for(k=1; k<=limit; k++)
		{
				aaa=0.;
              
				for(l=1; l<=num; l++)
				{
					aaa+=a[l]*sin(f[l]*t[k]-p[l]); 
					amp[k]+=-a[l]*sin(f[l]*t[k]-p[l]);
				}

			  fprintf(pFile[2],"\n %14.7e \t %14.7e",t[k],amp[k]);
		      fprintf(pFile[3],"\n %14.7e \t %14.7e",t[k],aaa);

		}
		fclose(pFile[2]);
		fclose(pFile[3]);

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


		printf("\n\n Output Files \n");

		printf("\n  Best trial function: synth.out");
		printf("\n  Error file:          error.out");

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

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

void header(void)

{

	long numBytes=300;



	char string[300];



	int nmax=0;

	long j=1;



	while(fgets(string,numBytes,pFile[1] )>0)

	{

//		printf("%ld.  %s\n",j,string);



		if( strrchr(string,'A' ) != NULL ){nmax=j;}

		if( strrchr(string,'B' ) != NULL ){nmax=j;}

		if( strrchr(string,'C' ) != NULL ){nmax=j;}

		if( strrchr(string,'D' ) != NULL ){nmax=j;}

		if( strrchr(string,'E' ) != NULL && strstr(string,"E+")==NULL && strstr(string,"E-")==NULL ){nmax=j;}

		if( strrchr(string,'F' ) != NULL ){nmax=j;}

		if( strrchr(string,'G' ) != NULL ){nmax=j;}

		if( strrchr(string,'H' ) != NULL ){nmax=j;}

		if( strrchr(string,'I' ) != NULL ){nmax=j;}

		if( strrchr(string,'J' ) != NULL ){nmax=j;}

		if( strrchr(string,'K' ) != NULL ){nmax=j;}

		if( strrchr(string,'L' ) != NULL ){nmax=j;}

		if( strrchr(string,'M' ) != NULL ){nmax=j;}

		if( strrchr(string,'N' ) != NULL ){nmax=j;}

		if( strrchr(string,'O' ) != NULL ){nmax=j;}

		if( strrchr(string,'P' ) != NULL ){nmax=j;}

		if( strrchr(string,'Q' ) != NULL ){nmax=j;}

		if( strrchr(string,'R' ) != NULL ){nmax=j;}

		if( strrchr(string,'S' ) != NULL ){nmax=j;}

		if( strrchr(string,'T' ) != NULL ){nmax=j;}

		if( strrchr(string,'U' ) != NULL ){nmax=j;}

		if( strrchr(string,'V' ) != NULL ){nmax=j;}

		if( strrchr(string,'W' ) != NULL ){nmax=j;}

		if( strrchr(string,'X' ) != NULL ){nmax=j;}

		if( strrchr(string,'Y' ) != NULL ){nmax=j;}

		if( strrchr(string,'Z' ) != NULL ){nmax=j;}

//

		if( strrchr(string,'a' ) != NULL ){nmax=j;}

		if( strrchr(string,'b' ) != NULL ){nmax=j;}

		if( strrchr(string,'c' ) != NULL ){nmax=j;}

		if( strrchr(string,'d' ) != NULL ){nmax=j;}

		if( strrchr(string,'e' ) != NULL  && strstr(string,"e+")==NULL && strstr(string,"e-")==NULL ){nmax=j;}

		if( strrchr(string,'f' ) != NULL ){nmax=j;}

		if( strrchr(string,'g' ) != NULL ){nmax=j;}

		if( strrchr(string,'h' ) != NULL ){nmax=j;}

		if( strrchr(string,'i' ) != NULL ){nmax=j;}

		if( strrchr(string,'j' ) != NULL ){nmax=j;}

		if( strrchr(string,'k' ) != NULL ){nmax=j;}

		if( strrchr(string,'l' ) != NULL ){nmax=j;}

		if( strrchr(string,'m' ) != NULL ){nmax=j;}

		if( strrchr(string,'n' ) != NULL ){nmax=j;}

		if( strrchr(string,'o' ) != NULL ){nmax=j;}

		if( strrchr(string,'p' ) != NULL ){nmax=j;}

		if( strrchr(string,'q' ) != NULL ){nmax=j;}

		if( strrchr(string,'r' ) != NULL ){nmax=j;}

		if( strrchr(string,'s' ) != NULL ){nmax=j;}

		if( strrchr(string,'t' ) != NULL ){nmax=j;}

		if( strrchr(string,'u' ) != NULL ){nmax=j;}

		if( strrchr(string,'v' ) != NULL ){nmax=j;}

		if( strrchr(string,'w' ) != NULL ){nmax=j;}

		if( strrchr(string,'x' ) != NULL ){nmax=j;}

		if( strrchr(string,'y' ) != NULL ){nmax=j;}

		if( strrchr(string,'z' ) != NULL ){nmax=j;}

//

		if( strrchr(string,'!' ) != NULL ){nmax=j;}

		if( strrchr(string,'@' ) != NULL ){nmax=j;}

		if( strrchr(string,'#' ) != NULL ){nmax=j;}

		if( strrchr(string,'$' ) != NULL ){nmax=j;}

		if( strrchr(string,'%' ) != NULL ){nmax=j;}

		if( strrchr(string,'^' ) != NULL ){nmax=j;}

		if( strrchr(string,'&' ) != NULL ){nmax=j;}

		if( strrchr(string,'*' ) != NULL ){nmax=j;}

		if( strrchr(string,'/' ) != NULL ){nmax=j;}

		if( strrchr(string,'>' ) != NULL ){nmax=j;}

		if( strrchr(string,'<' ) != NULL ){nmax=j;}

		if( strrchr(string,'?' ) != NULL ){nmax=j;}

		if( strrchr(string,'=' ) != NULL ){nmax=j;}

		if( strrchr(string,';' ) != NULL ){nmax=j;}



		j++;



		if(j==100){break;}

	}

	rewind(pFile[1]);



	if(kkj==1){printf("\n %ld header lines detected \n\n",nmax);}



	for(j=0;j<nmax;j++)

	{

		fgets(string,numBytes,pFile[1] );

		if(kkj==1){printf(" %s ",string);}

	}

	kkj=2;

}