/*
*  Copyright (C) 2007 Jolien Creighton, Julien Sylvestre
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with with program; see the file COPYING. If not, write to the
*  Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
*  MA  02110-1301  USA
*/

#include <lal/CoherentEstimation.h>
#include <lal/DetResponse.h>
#include <lal/TimeDelay.h>
#include <lal/AVFactories.h>
#include <strings.h>
#include <math.h>

#define EPS 1.0e-7

#define Cosh(x) cosh(x)
#define ACosh(x) acosh(x)

double cosh(double);
double acosh(double);

static INT4 jacobi(float **a, int n, float d[], float **v, int *nrot);

void
LALDoCoherentEstimation ( LALStatus          *stat,
			REAL4TimeSeries *output,
			CoherentEstimation *params,
			DetectorsData      *in) {
  /*
     NOTES:
     o destroys input (in)
     o order of in must be same as order of params->filters
     o output time wrt center of Earth
  */

  INT4 /* Sret, */
    i, j, k, /* counters */
    iPad, ePad, /* used for padding */
    del;  /* integer time delay */
  REAL4 y; /* dummy */
  REAL8 p1,p2; /* weights in interpolation */
  REAL8 *tDelays; /* time delays wrt Earth center */
  REAL8 mDelay; /* max of time delays */
  REAL8 tmid; /* mid time point */
  LALDetAMResponse *F; /* response functions */
  LALPlaceAndGPS pGPS; /* for time delays */
  LALDetAndSource dAs; /* for responses */
  LALSource source; /* for responses */
  LIGOTimeGPS t0; /* start time of data */

  REAL8 *alpha; /* scale factor */
  INT4 nrot;
  REAL8 maxLambda, tmpLambda;
  REAL4 *lambda;
  REAL4 **Hmat;
  REAL4 **v;
  REAL8 **Cmat;

  /*
    {REAL8 tmp = Cosh(ACosh((double)3.02993)/3.0);
    printf("%g\n",tmp);}
  */

  /***********************************************************************/
  /* initialize status & validate input                                  */
  /***********************************************************************/
  INITSTATUS(stat);
  ATTATCHSTATUSPTR( stat );


  ASSERT ( in, stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  ASSERT ( in->data, stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  ASSERT ( in->Ndetectors > 0 && in->Ndetectors < 10, stat, COHERENTESTIMATIONH_E0DEC, COHERENTESTIMATIONH_MSGE0DEC );

  ASSERT ( in->Ndetectors == 3, stat, COHERENTESTIMATIONH_EUIMP, COHERENTESTIMATIONH_MSGEUIMP );

  for(i=0;i<(INT4)in->Ndetectors;i++) {
    ASSERT ( in->data[i].data, stat, COHERENTESTIMATIONH_E0DEC, COHERENTESTIMATIONH_MSGE0DEC );
  }

  t0 = in->data[0].epoch;

  for(i=1;i<(INT4)in->Ndetectors;i++) {
    ASSERT ( in->data[i].epoch.gpsSeconds == t0.gpsSeconds &&
	     in->data[i].epoch.gpsNanoSeconds == t0.gpsNanoSeconds, stat, COHERENTESTIMATIONH_EDST, COHERENTESTIMATIONH_MSGEDST );
  }

  ASSERT ( params, stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  ASSERT ( params->detectors, stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  ASSERT ( params->filters, stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  ASSERT ( params->position, stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  ASSERT ( params->Ndetectors == in->Ndetectors, stat, COHERENTESTIMATIONH_EICE, COHERENTESTIMATIONH_MSGEICE );

  /***********************************************************************/
  /* if params->preProcessed is 0, need to pre-processed by applying the */
  /* IIR filters twice                                                   */
  /***********************************************************************/
  if(!(params->preProcessed) && params->nPreProcessed) {

    REAL8Vector *tmpR8 = NULL;
    UINT4 jind;

    for(i=0;i<(INT4)in->Ndetectors;i++) {
      ASSERT ( params->filters[i], stat, COHERENTESTIMATIONH_EICE, COHERENTESTIMATIONH_MSGEICE );
    }

    /* loop over all detectors, and filter twice */
    for(i=0; i<(INT4)params->Ndetectors; i++) {

      TRY ( LALDCreateVector( stat->statusPtr,
			      &tmpR8,
			      in->data[i].data->length ), stat );

      for(jind = 0; jind<in->data[i].data->length; jind++) {
	tmpR8->data[jind] = (REAL8)(in->data[i].data->data[jind]);
      }

      for(j=0; j<params->nPreProcessed; j++) {
        TRY ( LALIIRFilterREAL8Vector( stat->statusPtr,
	       			       tmpR8,
				       params->filters[i]), stat );
      }

      for(jind = 0; jind<in->data[i].data->length; jind++) {
	in->data[i].data->data[jind] = (REAL4)(tmpR8->data[jind]);
      }

      TRY ( LALDDestroyVector (  stat->statusPtr,
				 &tmpR8 ) , stat );

      /* set first 1/16 s to zero to avoid transient */
      memset(in->data[i].data->data, 0, sizeof(REAL4) * (UINT4)ceil(0.0635 / in->data[i].deltaT));

    }

    params->preProcessed = 1;

  }


  /* update output parameters */
  output->epoch = in->data[0].epoch;
  output->deltaT = in->data[0].deltaT;
  output->f0 = in->data[0].f0;
  memcpy(&(output->sampleUnits), &(in->data[0].sampleUnits), sizeof(LALUnit));

  /* make sure output is zero */
  memset(output->data->data, 0, output->data->length * sizeof(REAL4));



  /***********************************************************************/
  /* Compute time delays and response functions                          */
  /***********************************************************************/
  if(!(tDelays = (REAL8 *)LALMalloc(params->Ndetectors * sizeof(REAL8)))) {
    ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
  }

  /* delays are computed wrt to center of data stretch */
  pGPS.p_gps = &t0;
  tmid = 0.5 * (REAL8)(in->data[0].data->length) * in->data[0].deltaT;
  pGPS.p_gps->gpsSeconds += (INT4)floor(tmid);
  pGPS.p_gps->gpsNanoSeconds += (INT4)floor(1E9*(tmid-floor(tmid)));
  if(pGPS.p_gps->gpsNanoSeconds >= 1000000000) {
    pGPS.p_gps->gpsSeconds += (INT4)floor((REAL8)(pGPS.p_gps->gpsNanoSeconds) / 1E9);
    pGPS.p_gps->gpsNanoSeconds -= 1000000000 * (INT4)floor((REAL8)(pGPS.p_gps->gpsNanoSeconds) / 1E9);
  }


  if(!(F = (LALDetAMResponse *)LALMalloc(params->Ndetectors * sizeof(LALDetAMResponse)))) {
    ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
  }

  dAs.pSource = &source;
  source.equatorialCoords = *(params->position);
  source.orientation = params->polAngle;

  for(i=0; i<(INT4)params->Ndetectors; i++) {

    pGPS.p_detector = dAs.pDetector = params->detectors + i;

    /* tDelays = arrival time at detector - arrival time a center of Earth */
    tDelays[i] = XLALTimeDelayFromEarthCenter(params->detectors[i].location, params->position->longitude, params->position->latitude, &t0);

    /* JC: isnan is not allowed
    if(isnan(tDelays[i])) {
      ABORT ( stat, COHERENTESTIMATIONH_ENUM, COHERENTESTIMATIONH_MSGENUM );
    }
    */

    TRY ( LALComputeDetAMResponse ( stat->statusPtr,
				    F + i,
				    &dAs,
				    pGPS.p_gps ), stat );

    /* JC: isnan is not allowed
    if(isnan(F[i].cross) || isnan(F[i].plus)) {
      ABORT ( stat, COHERENTESTIMATIONH_ENUM, COHERENTESTIMATIONH_MSGENUM );
    }
    */
  }

  /***********************************************************************/
  /* Compute and store estimated data                                    */
  /***********************************************************************/
  /* set time origine on detector with largest delay */
  /*
  mDelay = tDelays[0];
  for(i=1; i<params->Ndetectors; i++) {
	if(tDelays[i] > mDelay) {
		mDelay = tDelays[i];
	}
  }
  */
  mDelay = tDelays[0];

  for(i=0; i<(INT4)params->Ndetectors; i++) {
    tDelays[i] -= mDelay;
  }


  alpha = (REAL8 *)LALMalloc(params->Ndetectors * sizeof(REAL8));
  if(!alpha) {
    ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
  }

  lambda = (REAL4 *)LALMalloc(params->Ndetectors * sizeof(REAL4));
  if(!lambda) {
    ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
  }

  Hmat = (REAL4 **)LALMalloc(params->Ndetectors * sizeof(REAL4 *));
  if(!Hmat) {
    ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
  }
  for(i=0; i<(INT4)params->Ndetectors; i++) {
    Hmat[i] = (REAL4 *)LALMalloc(params->Ndetectors * sizeof(REAL4));
    if(!(Hmat[i])) {
      ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
    }
  }

  v = (REAL4 **)LALMalloc(params->Ndetectors * sizeof(REAL4 *));
  if(!v) {
    ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
  }
  for(i=0; i<(INT4)params->Ndetectors; i++) {
    v[i] = (REAL4 *)LALMalloc(params->Ndetectors * sizeof(REAL4));
    if(!(v[i])) {
      ABORT ( stat, COHERENTESTIMATIONH_EMEM, COHERENTESTIMATIONH_MSGEMEM );
    }
  }

  Cmat = params->CMat;

  for(i=0; i<(INT4)params->Ndetectors; i++) {
    for(j=0; j<(INT4)params->Ndetectors; j++) {

	Hmat[i][j] = (F[i].plus*F[j].plus*params->plus2cross + (F[i].plus*F[j].cross + F[j].plus*F[i].cross)*params->plusDotcross + F[i].cross*F[j].cross/params->plus2cross) / Cmat[i][i];

    }
  }

  if(jacobi(Hmat, params->Ndetectors, lambda, v, &nrot)) {
    ABORT ( stat, COHERENTESTIMATIONH_ENUM, COHERENTESTIMATIONH_MSGENUM );
  }
  /*
 printf("%g\t%g\t%g\n",lambda[0],lambda[1],lambda[2]);
 */
  maxLambda = -1e30;
  for(k=0;k<(INT4)params->Ndetectors;k++) {

    tmpLambda = 0.0;

    for(i=0; i<(INT4)params->Ndetectors; i++) {
      for(j=0; j<(INT4)params->Ndetectors; j++) {
	tmpLambda += v[i][k]*v[j][k]*(F[i].plus*F[j].plus*params->plus2cross + (F[i].plus*F[j].cross + F[j].plus*F[i].cross)*params->plusDotcross + F[i].cross*F[j].cross/params->plus2cross);
      }
    }

    if(tmpLambda > maxLambda) {
      for(i=0; i<(INT4)params->Ndetectors; i++) {
	alpha[i] = v[i][k];
      }
      maxLambda = tmpLambda;
    }
/*
    printf("%u\t%g\t%g\t%g\t%g\n",k,lambda[k],alpha[0],alpha[1],alpha[2]);
    */
  }

  /* loop */
  for(i=0; i<(INT4)params->Ndetectors; i++) {

    /* setup padding and weights */
    if(tDelays[i] < 0.0) {
      /* need padding at beginning */
      iPad = (INT4)floor(-tDelays[i]/output->deltaT);
      ePad = 0;

      /* set integer delay (for p1 weight) */
      del = -iPad;

      /* set weights */
      p1 = ceil(tDelays[i] / output->deltaT) - tDelays[i] / output->deltaT;
      p2 = 1.0 - p1;
    } else {
      /* need padding at end */
      iPad = 0;
      ePad = (INT4)ceil(tDelays[i]/output->deltaT);

      /* integer delay */
      del = ePad;

      /* weights */
      p1 = ceil(tDelays[i] / output->deltaT) - tDelays[i] / output->deltaT;
      p2 = 1.0 - p1;
    }


    /* interpolate using time delays */
    for(j=iPad+1; j<(INT4)output->data->length - (INT4)ePad; j++) {

      y = p1 * in->data[i].data->data[del+j-1] + p2 * in->data[i].data->data[del+j];

      output->data->data[j] += y * alpha[i];
    }
  }

  /*
  {
    FILE *out;
    out = fopen("test.dat","w");
    for(j=0;j<output->data->length;j++) {
      fprintf(out,"%g\t%g\n",output->data->data[j].re,output->data->data[j].im);
    }
    fclose(out);

    exit(0);
  }
  */

  /***********************************************************************/
  /* clean up and normal return                                          */
  /***********************************************************************/
  LALFree(tDelays);
  LALFree(F);

  LALFree(alpha);
  LALFree(lambda);

  for(i=0; i<(INT4)params->Ndetectors; i++) {
    LALFree(Hmat[i]);
    LALFree(v[i]);
  }
  LALFree(Hmat);
  LALFree(v);

  DETATCHSTATUSPTR( stat );
  RETURN( stat );

}


void
LALClearCoherentData (
		      LALStatus     *stat,
		      DetectorsData *dat
		      ) {

  UINT4 i;

  INITSTATUS(stat);
  ATTATCHSTATUSPTR( stat );

  if(!dat) {
    ABORT ( stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  }

  for(i=0; i<dat->Ndetectors; i++) {
    if(dat->data[i].data) {
      TRY ( LALDestroyVector(stat->statusPtr, &(dat->data[i].data)), stat );
    }
  }

  LALFree(dat->data);

  DETATCHSTATUSPTR( stat );
  RETURN( stat );

}



void
LALClearCoherentInfo (
		      LALStatus     *stat,
		      CoherentEstimation *dat
		      ) {

  UINT4 i;

  INITSTATUS(stat);
  ATTATCHSTATUSPTR( stat );

  if(!dat) {
    ABORT ( stat, COHERENTESTIMATIONH_ENULL, COHERENTESTIMATIONH_MSGENULL );
  }

  LALFree(dat->detectors);

  for(i=0; i<dat->Ndetectors; i++) {
    if(dat->filters[i]) {
      TRY ( LALDestroyREAL8IIRFilter(stat->statusPtr, dat->filters + i), stat );
    }
    LALFree(dat->CMat[i]);
  }

  LALFree(dat->filters);

  LALFree(dat->CMat);

  DETATCHSTATUSPTR( stat );
  RETURN( stat );

}


#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);\
	a[k][l]=h+s*(g-h*tau);

int jacobi(float **a, int n, float d[], float **v, int *nrot)
{
	int j,iq,ip,i/*,k*/;
	float tresh,theta,tau,t,sm,s,h,g,c,*b,*z;

	b = (REAL4 *)LALMalloc(n * sizeof(REAL4));
	if(!b) {
	  return 1;
	}
	b -= 1;

	z = (REAL4 *)LALMalloc(n * sizeof(REAL4));
	if(!z) {
	  return 1;
	}
	z -= 1;

	d -= 1;

	for(i=0;i<n;i++) {
	  v[i] -= 1;
	  a[i] -= 1;
	}
	v -= 1;
	a -= 1;

	for (ip=1;ip<=n;ip++) {
		for (iq=1;iq<=n;iq++) v[ip][iq]=0.0;
		v[ip][ip]=1.0;
	}
	for (ip=1;ip<=n;ip++) {
		b[ip]=d[ip]=a[ip][ip];
		z[ip]=0.0;
	}
	*nrot=0;
	for (i=1;i<=50;i++) {
		sm=0.0;
		for (ip=1;ip<=n-1;ip++) {
			for (iq=ip+1;iq<=n;iq++)
				sm += fabs(a[ip][iq]);
		}
		if (sm == 0.0) {
		  LALFree(z + 1);
		  LALFree(b + 1);
		  d += 1;
		  a += 1;
		  v += 1;
		  for(i=0;i<n;i++) {
		    v[i] += 1;
		    a[i] += 1;
		  }
		  return 0;
		}
		if (i < 4)
			tresh=0.2*sm/(n*n);
		else
			tresh=0.0;
		for (ip=1;ip<=n-1;ip++) {
			for (iq=ip+1;iq<=n;iq++) {
				g=100.0*fabs(a[ip][iq]);
				if (i > 4 && (float)(fabs(d[ip])+g) == (float)fabs(d[ip])
					&& (float)(fabs(d[iq])+g) == (float)fabs(d[iq]))
					a[ip][iq]=0.0;
				else if (fabs(a[ip][iq]) > tresh) {
					h=d[iq]-d[ip];
					if ((float)(fabs(h)+g) == (float)fabs(h))
						t=(a[ip][iq])/h;
					else {
						theta=0.5*h/(a[ip][iq]);
						t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
						if (theta < 0.0) t = -t;
					}
					c=1.0/sqrt(1+t*t);
					s=t*c;
					tau=s/(1.0+c);
					h=t*a[ip][iq];
					z[ip] -= h;
					z[iq] += h;
					d[ip] -= h;
					d[iq] += h;
					a[ip][iq]=0.0;
					for (j=1;j<=ip-1;j++) {
						ROTATE(a,j,ip,j,iq)
					}
					for (j=ip+1;j<=iq-1;j++) {
						ROTATE(a,ip,j,j,iq)
					}
					for (j=iq+1;j<=n;j++) {
						ROTATE(a,ip,j,iq,j)
					}
					for (j=1;j<=n;j++) {
						ROTATE(v,j,ip,j,iq)
					}
					++(*nrot);
				}
			}
		}
		for (ip=1;ip<=n;ip++) {
			b[ip] += z[ip];
			d[ip]=b[ip];
			z[ip]=0.0;
		}
	}

	fprintf(stderr,"Too many iterations in routine jacobi");
	return 1;
}
#undef ROTATE

