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


/* globale Arbeitsvariablen */

#include "cube.h"


#define MD 10000L
#define MAXTMP MD*1100 
#define MIN_GPS_DATA 5
#define MAX_GPS_DATA 50
#define PERC_OUTLIER 30.0
#define QUALITY_OF_FIT 0.01

long fit(x,y,ndata,a,b,siga,sigb,chi2)
double x[],y[],*a,*b,*siga,*sigb,*chi2;
long ndata;
{
/* y = a + b * x */

 double sx,sy,st2,sxoss,ss,t,sigdat;
 long i;
 
 sx=0.0; sy=0.0; st2=0.0; *b=0.0;
 for (i=1; i<=ndata; i++) { sx=sx+x[i]; sy=sy+y[i]; }
 ss=1.0*ndata;
 sxoss=sx/ss;
 for (i=1; i<=ndata; i++) { t=x[i]-sxoss; st2=st2+t*t; *b=*b+t*y[i]; }
 
 *b=*b/st2;
 *a=(sy-sx**b)/ss;
 *siga=sqrt((1.0+sx*sx/(ss*st2))/ss);
 *sigb=sqrt(1.0/st2);
 *chi2=0.0;
 for (i=1; i<=ndata; i++) *chi2=*chi2+(y[i]-*a-*b*x[i])*(y[i]-*a-*b*x[i]);
 sigdat=sqrt(*chi2/(ndata-2.0));
 *siga=*siga*sigdat;
 *sigb=*sigb*sigdat;
  
}


/* robust fitting, assuming a small number of outliers */


long robust_fit(rx,ry,rndata,ra,rb,rsiga,rsigb,rchi2,fl)
double rx[],ry[],*ra,*rb,*rsiga,*rsigb,*rchi2;
long rndata,*fl;
{
	double xra,xrb,xrsiga,xrsigb,xrchi2;
	double rxtest[MD],rytest[MD],ytest,aytest,dmax;
	long k,ki,rndata_test,ktest,kcount,flag;

/* y = a + b * x */	
	for (k=1; k<=rndata; k++) {rxtest[k]=rx[k]; rytest[k]=ry[k];}
	rndata_test=rndata;
	flag=0;

	kcount=1;
	fit(rxtest,rytest,rndata_test,&xra,&xrb,&xrsiga,&xrsigb,&xrchi2); 
/*	fprintf(stderr,"starting with found before count = %ld %d %lf %lf %lf %lf %20.10lf %lf %lf\n",kcount,rndata_test,xra,xrb*1000000.0,xrsiga*1000000.0,xrsigb*1000000.0,xrchi2*1000000.0,rxtest[1],rytest[1]); */


/* loop until all outliers are removed */
	while (((xrchi2*1000000.0)>QUALITY_OF_FIT) && ((100.0*kcount/rndata)<PERC_OUTLIER) && (rndata_test>=MIN_GPS_DATA))
	{

		dmax=-1;

		for (k=1; k<=rndata_test; k++)
		{
			ytest=rytest[k]-(xrb*rxtest[k]+xra);
			aytest=sqrt(ytest*ytest);
			if (aytest>dmax) 
			{
				ktest=k;
				dmax=aytest;
			}
		}
		ki=0;
		for (k=1; k<=rndata_test; k++)
		{
			if (k!=ktest) 
			{	
				ki=ki+1;
				rxtest[ki]=rxtest[k];
				rytest[ki]=rytest[k];
			}		
		}

		rndata_test=ki;
		kcount=kcount+1;
		fit(rxtest,rytest,rndata_test,&xra,&xrb,&xrsiga,&xrsigb,&xrchi2); 
/*		fprintf(stderr,"found after  count = %ld %d %lf %lf %lf %lf %20.10lf\n",kcount,rndata_test,xra,xrb*1000000.0,xrsiga*1000000.0,xrsigb*1000000.0,xrchi2*1000000.0); 
*/

	}
/* to do flag setzen und unveraenderte werte mit f=-1 ausgeben */
	if ((100.0*kcount/rndata)>=PERC_OUTLIER) {flag=-1; /* fprintf(stderr,"PERC no outlier removal possible, skipped all %lf %ld %ld\n",100.0*kcount/rndata,rndata-kcount,rndata);*/}
	if (rndata_test<MIN_GPS_DATA) {flag=-1; /* fprintf(stderr,"MIND no outlier removal possible, skipped all %lf %ld %ld\n",100.0*kcount/rndata,rndata-kcount,rndata);*/}

/*	fprintf(stderr,"XXX found after  count = %ld %d %lf %lf %lf %lf %20.10lf %lf %lf\n",kcount,rndata_test,xra,xrb*1000000.0,xrsiga*1000000.0,xrsigb*1000000.0,xrchi2*1000000.0,rxtest[1],rytest[1]); 
*/

	*ra=xra;
	*rb=xrb;
	*rsiga=xrsiga;
	*rsigb=xrsigb;
	*rchi2=xrchi2;
	*fl=flag;
}





void main(argc,argv)
int argc;
char **argv;
{
  double		tx,t,T,rch;
  long			j,ch,k;
  long long		nsamp,nsamp0,i;
  long long		*nsample;
  int			**data,amp;
  long			tmks,gps_t,t0;
  long			sec,mksec,x1,x2,ff;
  long long		tsl,ts0l,txl;
  long			asec[MD],amksec[MD];
  long			aflag[MD];
  double xd[MD],yd[MD],ax,bx,sa,sb,chi,ytest,aytest;
  long long		samp_no[MD];
  long			flag,fl,ii;
  long			*aasec,*aamksec;

  char  		buf[1000];
  int			code;
  int			ampx[100];
extern int optind, opterr, optopt;
   int c,errflg; 
   time_t start_time;
   struct tm *tss;

   errflg=0;
   putenv ("TZ=GMT");
   tzset; 


  if (!(nsample = malloc (MAXTMP * sizeof(long long))))   { fprintf(stderr, "%s ERROR: malloc for nsample failed, not enough core memory\n",argv[0]); exit(0); }
  if (!(aasec = malloc (MAXTMP * sizeof(long))))   { fprintf(stderr, "%s ERROR: malloc for nsample failed, not enough core memory\n",argv[0]); exit(0); }
  if (!(aamksec = malloc (MAXTMP * sizeof(long))))   { fprintf(stderr, "%s ERROR: malloc for nsample failed, not enough core memory\n",argv[0]); exit(0); }

  gps_t=0;
  opterr=0; while ((c = getopt (argc, argv, "o:t")) != -1)
  switch (c)
      {
         case 't':        /* gps_t timing info explicite */
           gps_t=1;               
           break;
         case '?':
           fprintf(stderr,"%s ERROR: Unrecognized option: -%c\n",argv[0], optopt);
           errflg++;
      }


/* read config */
   if (fgets(buf,1000,stdin)) 
   {
	code=sscanf(buf,STR_FORMAT,(char *) SC_STR); 
   }

   if (gps_t==0) fprintf(stdout,STR_FORMAT"\n",PR_STR);


/* allocating arrays for the CH_NUM time series */
  if (!(data = malloc (CH_NUM * sizeof(int)))) { fprintf(stderr, "%s ERROR: malloc failed, not enough core memory\n",argv[0]); exit(0); }
  for (ch = 0; ch < CH_NUM; ch++) if (!(data[ch] = malloc (MAXTMP * sizeof(int)))) { fprintf(stderr, "%s ERROR: malloc failed, not enough core memory\n",argv[0]); exit(0); }


/* read til first time stamp */
  j=0;
  x1=-1;
  while (x1 < 0)
  {
       if (fgets(buf,1000,stdin))
       {
	switch (CH_NUM)
	{
		case 1: code=sscanf(buf,"%lld%d%ld%ld",&nsamp,&ampx[0],&x1,&x2); break;
		case 2: code=sscanf(buf,"%lld%d%d%ld%ld",&nsamp,&ampx[0],&ampx[1],&x1,&x2); break;
		case 3: code=sscanf(buf,"%lld%d%d%d%ld%ld",&nsamp,&ampx[0],&ampx[1],&ampx[2],&x1,&x2); break;
	}
/* no data treatment */
	if (nsamp==-1)
	{ 
		fprintf(stdout,"%ld ",-1L); 
  		for (ch=0; ch<CH_NUM; ch++) fprintf(stdout,"%d ",0); 
  		fprintf(stdout,"%ld %ld\n",0L,0L); 
		fprintf(stderr,"%s ERROR: No useful data in add_time!\n", argv[0]); 
		exit(77);
	}
       }
       j++;
  }


/* ini for segment */
  i=0;
  nsample[i]=nsamp;
  aasec[i]=x1;
  aamksec[i]=x2;
  for (ch=0; ch<CH_NUM; ch++) data[ch][i]=ampx[ch]; 		
  ts0l=x1*1000000+x2; 
  samp_no[1]=nsamp;
  nsamp0=nsamp;
  asec[1]=x1;
  amksec[1]=x2;
  t0=asec[1];
  xd[1]=1.0*(nsamp-nsamp0)/S_RATE;
  yd[1]=asec[1]+amksec[1]/1000000.0-xd[1]-t0;

  j=1;

/*  fprintf(stderr,"start with %d %d %ld %ld\n",nsamp,amp,x1,x2); */



/* main loop */     
  while (!feof(stdin))
  {
       if (fgets(buf,1000,stdin))
       {
	switch (CH_NUM)
	{
		case 1: code=sscanf(buf,"%lld%d%ld%ld",&nsamp,&ampx[0],&x1,&x2); break;
		case 2: code=sscanf(buf,"%lld%d%d%ld%ld",&nsamp,&ampx[0],&ampx[1],&x1,&x2); break;
		case 3: code=sscanf(buf,"%lld%d%d%d%ld%ld",&nsamp,&ampx[0],&ampx[1],&ampx[2],&x1,&x2); break;
	}
       }
/*	fprintf(stderr,"next %d %d %lf\n", nsamp,amp,x1); */
/* add data point */
	i++;	
	if (i>=(MAXTMP-1)) { fprintf(stderr,"%s ERROR: max number exceeded: increase MAXTMP in x.c and recompile\n",argv[0],MAXTMP); exit(200); }
	nsample[i]=nsamp;
	aasec[i]=x1;
 	aamksec[i]=x2;
/* fprintf(stdout,"i=%d %ld %ld \n",i,x1,x2); */
	for (ch=0; ch<CH_NUM; ch++) data[ch][i]=ampx[ch];
	if (( x1 > 0.0 ) || (i>=(MAXTMP-5)))
	{
/* add data point & GPS tag */
		if (( x1 > 0.0 ) && ((x1+x2/1000000.0-asec[j]-amksec[j]/1000000.0)<1.5) && (j<MAX_GPS_DATA))
		{
			j=j+1;
			samp_no[j]=nsamp; 
			asec[j]=x1;
			amksec[j]=x2;			
			xd[j]=1.0*(nsamp-nsamp0)/S_RATE;
			yd[j]=x1+x2/1000000.0-xd[j]-t0;
		}
		else
		{
/* evaluate segment of tags & out put of all data */
/* fprintf(stderr,"do check with %ld data points starting at %ld\n",j,x1);*/
/* fprintf(stdout,"do analysis with  %d GPS tags, data points %ld \n",j,i); */
			if (j<MIN_GPS_DATA) 
			{
/* ignore segments with only a few tags */ 
				flag=-1;
/*				for (k=1; k<j; k++)  fprintf(stdout,"%lld %d %d %ld %ld\n",samp_no[k],flag,0,asec[k],amksec[k]); */
				for (ii=0; ii<i; ii++)
				{
					if (gps_t==0)
					{
						fprintf(stdout,"%lld ",nsample[ii]);
						for (ch=0; ch<CH_NUM; ch++) fprintf(stdout,"%d ",data[ch][ii]);
						if (aasec[ii]==-1) fprintf(stdout,"%ld %ld \n",-1L,0); 
						else fprintf(stdout,"%ld %ld\n",-aasec[ii],aamksec[ii]); 
					}
					else
					{
						if (aasec[ii]!=-1)
						{
							fprintf(stdout,"%lld ",nsample[ii]);
							for (ch=0; ch<CH_NUM; ch++) fprintf(stdout,"%d ",data[ch][ii]);
							fprintf(stdout,"%ld %ld ",aasec[ii],aamksec[ii]); 
							start_time=aasec[ii];
  							tss = localtime(&start_time); 
							fprintf(stdout,"%4d-%02d-%02dT%02d:%02d:%02d %d\n",(int) (tss->tm_year)+1900,(int) (tss->tm_mon)+1,(int) (tss->tm_mday),(int) (tss->tm_hour),(int) (tss->tm_min),(int) (tss->tm_sec),-1);
						}
					}

				}

			}
			else
			{
/*	 			fprintf(stdout,"do check with nd=%ld %ld\n",i,j); */
				robust_fit(xd,yd,j,&ax,&bx,&sa,&sb,&rch,&fl); 
/* 				fprintf(stderr,"found %d %lf %lf %lf %lf %20.10lf %d\n",j,ax,bx*1000000.0,sa*1000000.0,sb*1000000.0,rch*1000000.0,fl); */
				for (k=1; k<=j;k++)
				{
					ytest=yd[k]-(bx*xd[k]+ax);
					aytest=sqrt(ytest*ytest);
					aflag[k]=0;
					if (aytest>0.000100) aflag[k]=-2;
	 				if (fl!=0) aflag[k]=-3;
/* if (aflag[k]!=0) fprintf(stderr,"K=%d flag=%d %lf %lf %lf %lf %d %ld %lf\n",k,aflag[k],ax,bx,xd[k],yd[k],j,aasec[k-1],aytest); */
				}
				k=1;
				for (ii=0; ii<i; ii++)		
				{

					if (gps_t==0)
					{
						fprintf(stdout,"%lld ",nsample[ii]);
						for (ch=0; ch<CH_NUM; ch++) fprintf(stdout,"%d ",data[ch][ii]);
						if (aasec[ii]==-1) fprintf(stdout,"%ld %ld \n",-1L,0); 
						if ((aasec[ii]!=-1) && (aflag[k]==0)) fprintf(stdout,"%ld %ld\n",aasec[ii],aamksec[ii]); 
						if ((aasec[ii]!=-1) && (aflag[k]!=0)) fprintf(stdout,"%ld %ld\n",-aasec[ii],aamksec[ii]); 
						if (aasec[ii]!=-1) k=k+1;
					}
					else
					{
						if (aasec[ii]!=-1)
						{
/*						if (aflag[k]!=0)
						{*/
							fprintf(stdout,"%lld ",nsample[ii]);
							for (ch=0; ch<CH_NUM; ch++) fprintf(stdout,"%d ",data[ch][ii]);
							fprintf(stdout,"%ld %ld ",aasec[ii],aamksec[ii]); 
							start_time=aasec[ii];
  							tss = localtime(&start_time);
							fprintf(stdout,"%4d-%02d-%02dT%02d:%02d:%02d %d\n",(int) (tss->tm_year)+1900,(int) (tss->tm_mon)+1,(int) (tss->tm_mday),(int) (tss->tm_hour),(int) (tss->tm_min),(int) (tss->tm_sec),aflag[k]);
							if (aasec[ii]!=-1) k=k+1;
/*						} */
						}
					}
				}
			}
			j=1;			
			asec[j]=x1;
			t0=x1;
 			amksec[j]=x2;
			samp_no[j]=nsamp;
			nsamp0=nsamp;
			xd[j]=1.0*(nsamp-nsamp0)/S_RATE;
			yd[j]=x1+x2/1000000.0-xd[j]-t0;
			i=0;	
			nsample[0]=nsamp;
			aasec[0]=x1;
 			aamksec[0]=x2;
			for (ch=0; ch<CH_NUM; ch++) data[ch][i]=ampx[ch];
		}
	}


  }

/* output remaining data with GPS flagged nonvalid */
/*  fprintf(stderr,"remaining %d samples\n",ii); */
  for (ii=0; ii<i; ii++)
  {
	if (gps_t==0)
	{
		fprintf(stdout,"%lld ",nsample[ii]); 
		for (ch=0; ch<CH_NUM; ch++) fprintf(stdout,"%d ",data[ch][ii]);
		if (aasec[ii]==-1) fprintf(stdout,"%ld %ld \n",-1L,0); 
		else fprintf(stdout,"%ld %ld \n",-aasec[ii],aamksec[ii]); 
	}
	else
	{
		if (aasec[ii]!=-1)
		{
			fprintf(stdout,"%lld ",nsample[ii]);
			for (ch=0; ch<CH_NUM; ch++) fprintf(stdout,"%d ",data[ch][ii]);
			fprintf(stdout,"%ld %ld ",aasec[ii],aamksec[ii]); 
			start_time=aasec[ii];
  			tss = localtime(&start_time); 
			fprintf(stdout,"%4d-%02d-%02dT%02d:%02d:%02d %d\n",(int) (tss->tm_year)+1900,(int) (tss->tm_mon)+1,(int) (tss->tm_mday),(int) (tss->tm_hour),(int) (tss->tm_min),(int) (tss->tm_sec),-4);
		}	

	}
  }

 }

