/************************************************************************
* editcm reads a file in cm format and edits outliers, add     		*
* uncertainties, and optionally applies a regression model to depth     *
* 		                                                        *
************************************************************************/
/************************************************************************
* Creator: David Sandwell                                               *
* Date   : March 26, 2008					        *
************************************************************************/
/************************************************************************
* Modification History: Seung Hee Kim					* 
* Date	:	June 27, 2008				                *
*                                                                       * 
************************************************************************/ 

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
void fit();

int main(int argc, char **argv) {
	
#define NUM_REC (1000*5000)
	int i, imode, nrec, ndata;
	float *x, *y, *sigma;
	float a, b, siga, sigb, chi2, dd;
	
	struct cmRecType {
		int    time;
		double lon;
		double lat;
		int    depth;
		int    sig_h;
		int    sig_d;
		int    id;
		int    pred;
	} *jjPtr, *scan;
	
	scan = jjPtr = malloc(sizeof(struct cmRecType)*NUM_REC);
	x = malloc(sizeof(float)*NUM_REC);
	y = malloc(sizeof(float)*NUM_REC);
	sigma = malloc(sizeof(float)*NUM_REC);
	
	if(argc < 2){
		fprintf(stderr,"usage: %s imode < file.cm > file.cm\n\n",argv[0]);
		fprintf(stderr,"          imode:\n");
		fprintf(stderr,"          0 - dump measured and pred depth \n");
		fprintf(stderr,"          1 - calculate depth uncertainty \n");
		fprintf(stderr,"          2 - edit outliers and add depth uncertainty \n");
		fprintf(stderr,"          3 - edit outliers, add depth uncertainty, and remove depth regression model \n\n");
		exit(-1);
	}
	imode = atoi(argv[1]);
	if(imode < 0 || imode > 3) {
		printf(" imode out of range \n");
		exit(1);
	}
	
	/* read the data */
	
	nrec=0;
	scan = jjPtr;
	while (fscanf(stdin,"%d %lf %lf %d %d %d %d %d", 
				  &scan->time, &scan->lat, &scan->lon, &scan-> depth, &scan->sig_h, &scan->sig_d, &scan->id, &scan->pred)
		   != EOF) {
		
		nrec++;
		if(nrec > NUM_REC) {
			printf(" increase NUM_REC \n");
			exit(1);
		}
		scan++;
	}
	
	/* dump the measured and pred depth */
	
	scan = jjPtr;
	
	if (imode == 0) {
		for(i=0;i<nrec;i++) {
			if(scan->sig_d < 9999) 
				printf(" %d %d \n", scan->pred, scan->depth);
			scan++;
		}
		exit(1);
	}
	
	if (imode > 0) {
		
		/* use IHO formulation sig = a + b * depth */
		/* use a new IHO standard sig = sqrt(a^2 + (b * depth)^2) */
		
		ndata = 0;
		scan = jjPtr;
		for(i=0;i<nrec;i++) {
			if(scan->sig_d != 9999) {
				dd=fabsf(scan->pred - scan->depth);
				dd=fminf(dd,fabsf(scan->pred));
				x[ndata] = scan->pred;
				y[ndata] = dd;
				sigma[ndata] = 0.;
				ndata++;
			}
			scan++;
		}
		fit(x, y, ndata, sigma, 0, &a, &b, &siga, &sigb, &chi2);
		fprintf(stderr," %f %f %f %f %f \n",  a, b, siga, sigb, chi2);
		
		/* add the sigmas to the record */	
		scan = jjPtr;
		for(i=0;i<nrec;i++) {
			if(scan->sig_d != 9999) scan->sig_d = sqrt(a*a + b*b * (scan->pred)*(scan->pred));
			scan++;
		}
		
	} 
	
	if(imode > 1){
		
		/* now flag the outliers where abs(dd) > 8 rms */
		
		scan = jjPtr;
		for(i=0;i<nrec;i++) {
			if(scan->sig_d != 9999) {
				dd=fabs(scan->pred -scan->depth);
				if(dd > 8.*scan->sig_d) scan->sig_d = 9999;
			}
			scan++;
		}
	}
	if(imode > 2){
		
		/* do linear fit */
		
		ndata = 0;
		scan = jjPtr;
		for (i=0;i<nrec;i++){
			if(scan->sig_d > 0. && scan->sig_d < 9998.) {
				x[ndata] = scan->pred;
				y[ndata] = scan->depth - scan->pred;
				sigma[ndata] = scan->sig_d;
				ndata++;
			}
			scan++;
		}
		fit(x, y, ndata, sigma, 1, &a, &b, &siga, &sigb, &chi2);
		fprintf(stderr," %f %f %f %f %f \n",  a, b, siga, sigb, chi2);
		
		/* remove the trend from the measured depth */
		
		scan = jjPtr;
		for (i=0;i<nrec;i++){
			scan->depth = (int) floor(scan->depth - (a + b * scan->pred) + 0.5);
			scan++;
		}
	}
	
	/* write all the data records */
	
	scan = jjPtr;
	for(i=0;i<nrec;i++) {
		printf("%d %.5f %.5f %d %d %d %d %d \n", scan->time, scan->lat, 
			   scan->lon, scan-> depth, scan->sig_h, scan->sig_d, scan->id, scan->pred);
		scan++;
	}
	return 0;
}

void fit(x,y,ndata,sig,mwt,a,b,siga,sigb,chi2)
float *a,*b,*chi2,*siga,*sigb,sig[],x[],y[];
int mwt,ndata;
{
	int i;
	float wt,t,sxoss,sx=0.0,sy=0.0,st2=0.0,ss,sigdat;
	
	
	*b=0.0;
	if (mwt) {
		ss=0.0;
		for (i=0;i<ndata;i++) {
			wt=1.0/(sig[i]*sig[i]);
			ss += wt;
			sx += x[i]*wt;
			sy += y[i]*wt;
		}
	} else {
		for (i=0;i<ndata;i++) {
			sx += x[i];
			sy += y[i];
		}
		ss=ndata;
	}
	sxoss=sx/ss;
	if (mwt) {
		for (i=0;i<ndata;i++) {
			t=(x[i]-sxoss)/sig[i];
			st2 += t*t;
			*b += t*y[i]/sig[i];
		}
	} else {
		for (i=0;i<ndata;i++) {
			t=x[i]-sxoss;
			st2 += t*t;
			*b += t*y[i];
		}
	}
	*b /= st2;
	*a=(sy-sx*(*b))/ss;
	*siga=sqrt((1.0+sx*sx/(ss*st2))/ss);
	*sigb=sqrt(1.0/st2);
	*chi2=0.0;
	if (mwt == 0) {
		for (i=0;i<ndata;i++)
			*chi2 += (y[i]-(*a)-(*b)*x[i])*(y[i]-(*a)-(*b)*x[i]);
		sigdat=sqrt((*chi2)/(ndata-2));
		*siga *= sigdat;
		*sigb *= sigdat;
	} else {
		for (i=0;i<ndata;i++)
			*chi2 += ((y[i]-(*a)-(*b)*x[i])/sig[i])*((y[i]-(*a)-(*b)*x[i])/sig[i]);
	}
}