/*
 * \file Observations.c
 * \brief: implementation of the Observations class, derived from DataSet class
 */

/*Headers: {{{*/
#ifdef HAVE_CONFIG_H
	#include <config.h>
#else
#error "Cannot compile with HAVE_CONFIG_H symbol! run configure first!"
#endif

#include <vector>
#include <functional>
#include <algorithm>
#include <iostream>

#include "./DataSet.h"
#include "./Observations.h"
#include "../shared/shared.h"
#include "../include/include.h"
#include "../modules/modules.h"
#include "../EnumDefinitions/EnumDefinitions.h"
#include "../io/io.h"

using namespace std;
/*}}}*/

/*Object constructors and destructor*/
/*FUNCTION Observations::Observations(){{{*/
Observations::Observations(){
	this->quadtree = NULL;
	return;
}
/*}}}*/
/*FUNCTION Observations::Observations(IssmDouble* observations_list,IssmDouble* x,IssmDouble* y,int n,Options* options){{{*/
Observations::Observations(IssmDouble* observations_list,IssmDouble* x,IssmDouble* y,int n,Options* options){

	/*Intermediaries*/
	int          i,j,maxdepth,level,counter,index;
	int          xi,yi;
	IssmDouble       xmin,xmax,ymin,ymax;
	IssmDouble       offset,minlength,minspacing,mintrimming,maxtrimming;
	int         *indices     = NULL;
	Observation *observation = NULL;

	/*Get extrema*/
	xmin=x[0]; ymin=y[0];
	xmax=x[0]; ymax=y[0];
	for(i=1;i<n;i++){
		xmin=min(xmin,x[i]); ymin=min(ymin,y[i]);
		xmax=max(xmax,x[i]); ymax=max(ymax,y[i]);
	}
	offset=0.05*(xmax-xmin); xmin-=offset; xmax+=offset;
	offset=0.05*(ymax-ymin); ymin-=offset; ymax+=offset;

	/*Get trimming limits*/
	options->Get(&mintrimming,"mintrimming",-1.e+21);
	options->Get(&maxtrimming,"maxtrimming",+1.e+21);
	options->Get(&minspacing,"minspacing",0.01);
	if(minspacing<=0) _error2_("minspacing must > 0");

	/*Get Minimum box size*/
	if(options->GetOption("boxlength")){
		options->Get(&minlength,"boxlength");
		if(minlength<=0)_error2_("boxlength should be a positive number");
		maxdepth=int(log(max(xmax-xmin,ymax-ymin)/minlength +1)/log(2.0));
	}
	else{
		maxdepth = 30;
		minlength=max(xmax-xmin,ymax-ymin)/IssmDouble((1L<<maxdepth)-1);
	}

	/*Initialize Quadtree*/
	_pprintString_("Generating quadtree with a maximum box size " << minlength << " (depth=" << maxdepth << ")... ");
	this->quadtree = new Quadtree(xmin,xmax,ymin,ymax,maxdepth);

	/*Add observations one by one*/
	counter = 0;
	for(i=0;i<n;i++){

		/*First check limits*/
		if(observations_list[i]>maxtrimming) continue;
		if(observations_list[i]<mintrimming) continue;

		/*First check that this observation is not too close from another one*/
		this->quadtree->ClosestObs(&index,x[i],y[i]);
		if(index>=0){
			observation=(Observation*)this->GetObjectByOffset(index);
			if(pow(observation->x-x[i],2)+pow(observation->y-y[i],2) < minspacing) continue;
		}

		this->quadtree->IntergerCoordinates(&xi,&yi,x[i],y[i]);
		this->quadtree->QuadtreeDepth2(&level,xi,yi);
		if((int)level <= maxdepth){
			observation = new Observation(x[i],y[i],xi,yi,counter++,observations_list[i]);
			this->quadtree->Add(observation);
			this->AddObject(observation);
		}
		else{
			/*We need to average with the current observations*/
			this->quadtree->AddAndAverage(x[i],y[i],observations_list[i]);
		}
	}
	_pprintLine_("done");
	_pprintLine_("Initial number of observations: " << n);
	_pprintLine_("  Final number of observations: " << this->quadtree->NbObs);
}
/*}}}*/
/*FUNCTION Observations::~Observations(){{{*/
Observations::~Observations(){
	delete quadtree;
	return;
}
/*}}}*/

/*Methods*/
/*FUNCTION Observations::ClosestObservation{{{*/
void Observations::ClosestObservation(IssmDouble *px,IssmDouble *py,IssmDouble *pobs,IssmDouble x_interp,IssmDouble y_interp,IssmDouble radius){

	/*Output and Intermediaries*/
	bool         stop;
	int          nobs,i,index;
	IssmDouble       h2,hmin2,radius2;
	int         *indices      = NULL;
	Observation *observation  = NULL;

	/*If radius is not provided or is 0, return all observations*/
	if(radius==0) radius=this->quadtree->root->length;

	/*Compute radius square*/
	radius2 = radius*radius;

	/*Find all observations that are in radius*/
	this->quadtree->RangeSearch(&indices,&nobs,x_interp,y_interp,radius);
	for (i=0;i<nobs;i++){
		observation=(Observation*)this->GetObjectByOffset(indices[i]);
		h2 = (observation->x-x_interp)*(observation->x-x_interp) + (observation->y-y_interp)*(observation->y-y_interp);

		if(i==0){
			hmin2 = h2;
			index = i;
		}
		else{
			if(h2<hmin2){
				hmin2 = h2;
				index = i;
			}
		}
	}  

	/*Assign output pointer*/
	if(!nobs){
		*px=UNDEF;
		*py=UNDEF;
		*pobs=UNDEF;
	}
	else{
		observation=(Observation*)this->GetObjectByOffset(indices[index]);
		*px=observation->x;
		*py=observation->y;
		*pobs=observation->value;
	}
	xDelete<int>(indices);

}/*}}}*/
/*FUNCTION Observations::ObservationList(IssmDouble **px,IssmDouble **py,IssmDouble **pobs,int* pnobs,IssmDouble x_interp,IssmDouble y_interp,IssmDouble radius,int maxdata){{{*/
void Observations::ObservationList(IssmDouble **px,IssmDouble **py,IssmDouble **pobs,int* pnobs,IssmDouble x_interp,IssmDouble y_interp,IssmDouble radius,int maxdata){

	/*Output and Intermediaries*/
	bool         stop;
	int          nobs,tempnobs,i,j,k,n,counter;
	IssmDouble       h2,radius2;
	int         *indices      = NULL;
	int         *tempindices  = NULL;
	IssmDouble      *dists        = NULL;
	IssmDouble      *x            = NULL;
	IssmDouble      *y            = NULL;
	IssmDouble      *obs          = NULL;
	Observation *observation  = NULL;

	/*If radius is not provided or is 0, return all observations*/
	if(radius==0) radius=this->quadtree->root->length;

	/*Compute radius square*/
	radius2 = radius*radius;

	/*Find all observations that are in radius*/
	this->quadtree->RangeSearch(&tempindices,&tempnobs,x_interp,y_interp,radius);
	if(tempnobs){
		indices = xNew<int>(tempnobs);
		dists   = xNew<IssmDouble>(tempnobs);
	}
	nobs = 0;
	for (i=0;i<tempnobs;i++){
		observation=(Observation*)this->GetObjectByOffset(tempindices[i]);
		h2 = (observation->x-x_interp)*(observation->x-x_interp) + (observation->y-y_interp)*(observation->y-y_interp);

		if(nobs==maxdata && h2>radius2) continue;
		if(nobs<=maxdata){
			indices[nobs]   = tempindices[i];
			dists[nobs]     = h2;
			nobs++;
		}
		if(nobs==1) continue;

		/*Sort all dists up to now*/
		n=nobs-1;
		stop = false;
		for(k=0;k<n-1;k++){
			if(h2<dists[k]){
				counter=1;
				for(int jj=k;jj<n;jj++){
					j  = n-counter;
					dists[j+1]   = dists[j];
					indices[j+1] = indices[j];
					counter++;
				}
				dists[k]   = h2;
				indices[k] = tempindices[i];
				stop = true;
				break;
			}
			if(stop) break;
		}
	}  
	xDelete<IssmDouble>(dists);
	xDelete<int>(tempindices);

	if(nobs){
		/*Allocate vectors*/
		x   = xNew<IssmDouble>(nobs);
		y   = xNew<IssmDouble>(nobs);
		obs = xNew<IssmDouble>(nobs);

		/*Loop over all observations and fill in x, y and obs*/
		for (i=0;i<nobs;i++){
			observation=(Observation*)this->GetObjectByOffset(indices[i]);
			observation->WriteXYObs(&x[i],&y[i],&obs[i]);
		}
	}

	/*Assign output pointer*/
	xDelete<int>(indices);
	*px=x;
	*py=y;
	*pobs=obs;
	*pnobs=nobs;
}/*}}}*/
/*FUNCTION Observations::ObservationList(IssmDouble **px,IssmDouble **py,IssmDouble **pobs,int* pnobs){{{*/
void Observations::ObservationList(IssmDouble **px,IssmDouble **py,IssmDouble **pobs,int* pnobs){

	/*Output and Intermediaries*/
	int          nobs;
	IssmDouble      *x            = NULL;
	IssmDouble      *y            = NULL;
	IssmDouble      *obs          = NULL;
	Observation *observation  = NULL;

	nobs = this->Size();

	if(nobs){
		x   = xNew<IssmDouble>(nobs);
		y   = xNew<IssmDouble>(nobs);
		obs = xNew<IssmDouble>(nobs);
		for(int i=0;i<this->Size();i++){
			observation=(Observation*)this->GetObjectByOffset(i);
			observation->WriteXYObs(&x[i],&y[i],&obs[i]);
		}
	}

	/*Assign output pointer*/
	*px=x;
	*py=y;
	*pobs=obs;
	*pnobs=nobs;
}/*}}}*/
/*FUNCTION Observations::InterpolationIDW{{{*/
void Observations::InterpolationIDW(IssmDouble *pprediction,IssmDouble x_interp,IssmDouble y_interp,IssmDouble radius,int mindata,int maxdata,IssmDouble power){

	/*Intermediaries*/
	int    i,n_obs;
	IssmDouble prediction;
	IssmDouble numerator,denominator,h,weight;
	IssmDouble *x   = NULL;
	IssmDouble *y   = NULL;
	IssmDouble *obs = NULL;

	/*Some checks*/
	_assert_(maxdata>0);
	_assert_(pprediction);
	_assert_(power>0);

	/*If radius is not provided or is 0, return all observations*/
	if(radius==0) radius=this->quadtree->root->length;

	/*Get list of observations for current point*/
	this->ObservationList(&x,&y,&obs,&n_obs,x_interp,y_interp,radius,maxdata);

	/*If we have less observations than mindata, return UNDEF*/
	if(n_obs<mindata){
		prediction = UNDEF; 
	}
	else{
		numerator   = 0.;
		denominator = 0.;
		for(i=0;i<n_obs;i++){
			h = sqrt( (x[i]-x_interp)*(x[i]-x_interp) + (y[i]-y_interp)*(y[i]-y_interp));
			if (h<0.0000001){
				numerator   = obs[i];
				denominator = 1.;
				break;
			}
			weight = 1./pow(h,power);
			numerator   += weight*obs[i];
			denominator += weight;
		}
		prediction = numerator/denominator; 
	}

	/*clean-up*/
	*pprediction = prediction;
	xDelete<IssmDouble>(x);
	xDelete<IssmDouble>(y);
	xDelete<IssmDouble>(obs);
}/*}}}*/
/*FUNCTION Observations::InterpolationKriging{{{*/
void Observations::InterpolationKriging(IssmDouble *pprediction,IssmDouble *perror,IssmDouble x_interp,IssmDouble y_interp,IssmDouble radius,int mindata,int maxdata,Variogram* variogram){

	/*Intermediaries*/
	int           i,j,n_obs;
	IssmDouble        prediction,error;
	IssmDouble        numerator,denominator,ratio;
	IssmDouble       *x            = NULL;
	IssmDouble       *y            = NULL;
	IssmDouble       *obs          = NULL;
	IssmDouble       *Gamma        = NULL;
	IssmDouble       *GinvG0       = NULL;
	IssmDouble       *Ginv1        = NULL;
	IssmDouble       *GinvZ        = NULL;
	IssmDouble       *gamma0       = NULL;
	IssmDouble       *ones         = NULL;

	/*Some checks*/
	_assert_(mindata>0 && maxdata>0);
	_assert_(pprediction && perror);

	/*If radius is not provided or is 0, return all observations*/
	if(radius==0) radius=this->quadtree->root->length;

	/*Get list of observations for current point*/
	this->ObservationList(&x,&y,&obs,&n_obs,x_interp,y_interp,radius,maxdata);

	/*If we have less observations than mindata, return UNDEF*/
	if(n_obs<mindata){
		*pprediction = -999.0; 
		*perror      = -999.0; 
		return;
	}

	/*Allocate intermediary matrix and vectors*/
	Gamma  = xNew<IssmDouble>(n_obs*n_obs);
	gamma0 = xNew<IssmDouble>(n_obs);
	ones   = xNew<IssmDouble>(n_obs);

	/*First: Create semivariogram matrix for observations*/
	for(i=0;i<n_obs;i++){
		for(j=0;j<=i;j++){
			//Gamma[i*n_obs+j] = variogram->SemiVariogram(x[i]-x[j],y[i]-y[j]);
			Gamma[i*n_obs+j] = variogram->Covariance(x[i]-x[j],y[i]-y[j]);
			Gamma[j*n_obs+i] = Gamma[i*n_obs+j];
		}
	}
	for(i=0;i<n_obs;i++) ones[i]=1;

	/*Get semivariogram vector associated to this location*/
	//for(i=0;i<n_obs;i++) gamma0[i] = variogram->SemiVariogram(x[i]-x_interp,y[i]-y_interp);
	for(i=0;i<n_obs;i++) gamma0[i] = variogram->Covariance(x[i]-x_interp,y[i]-y_interp);

	/*Solve the three linear systems*/
#if _HAVE_GSL_
	SolverxGsl(&GinvG0,Gamma,gamma0,n_obs); // Gamma^-1 gamma0
	SolverxGsl(&Ginv1, Gamma,ones,n_obs);   // Gamma^-1 ones
	SolverxGsl(&GinvZ, Gamma,obs,n_obs);    // Gamma^-1 Z
#else
	_error2_("GSL is required");
#endif

	/*Prepare predictor*/
	numerator=-1.; denominator=0.;
	for(i=0;i<n_obs;i++) numerator  +=GinvG0[i];
	for(i=0;i<n_obs;i++) denominator+=Ginv1[i];
	ratio=numerator/denominator;

	prediction = 0.;
	error      = - numerator*numerator/denominator;
	for(i=0;i<n_obs;i++) prediction += (gamma0[i]-ratio)*GinvZ[i];
	for(i=0;i<n_obs;i++) error += gamma0[i]*GinvG0[i];

	/*clean-up*/
	*pprediction = prediction;
	*perror = error;
	xDelete<IssmDouble>(x);
	xDelete<IssmDouble>(y);
	xDelete<IssmDouble>(obs);
	xDelete<IssmDouble>(Gamma);
	xDelete<IssmDouble>(gamma0);
	xDelete<IssmDouble>(ones);
	xDelete<IssmDouble>(GinvG0);
	xDelete<IssmDouble>(Ginv1);
	xDelete<IssmDouble>(GinvZ);

}/*}}}*/
/*FUNCTION Observations::InterpolationNearestNeighbor{{{*/
void Observations::InterpolationNearestNeighbor(IssmDouble *pprediction,IssmDouble x_interp,IssmDouble y_interp,IssmDouble radius){

	/*Intermediaries*/
	IssmDouble        x,y,obs;

	/*Get clostest observation*/
	this->ClosestObservation(&x,&y,&obs,x_interp,y_interp,radius);

	/*Assign output pointer*/
	*pprediction = obs;
}/*}}}*/
/*FUNCTION Observations::QuadtreeColoring{{{*/
void Observations::QuadtreeColoring(IssmDouble* A,IssmDouble *x,IssmDouble *y,int n){

	int xi,yi,level;

	for(int i=0;i<n;i++){
		this->quadtree->IntergerCoordinates(&xi,&yi,x[i],y[i]);
		this->quadtree->QuadtreeDepth(&level,xi,yi);
		A[i]=(IssmDouble)level;
	}

}/*}}}*/
/*FUNCTION Observations::Variomap{{{*/
void Observations::Variomap(IssmDouble* gamma,IssmDouble *x,int n){

	/*Output and Intermediaries*/
	int          i,j,k;
	IssmDouble       distance;
	Observation *observation1 = NULL;
	Observation *observation2 = NULL;

	IssmDouble *counter = xNew<IssmDouble>(n);
	for(j=0;j<n;j++) counter[j] = 0.0;
	for(j=0;j<n;j++) gamma[j]   = 0.0;

	for(i=0;i<this->Size();i++){
		observation1=(Observation*)this->GetObjectByOffset(i);

		for(j=i+1;j<this->Size();j++){
			observation2=(Observation*)this->GetObjectByOffset(j);

			distance=sqrt(pow(observation1->x - observation2->x,2.) + pow(observation1->y - observation2->y,2.));
			if(distance>x[n-1]) continue;

			int index = int(distance/(x[1]-x[0]));
			if(index>n-1) index = n-1;
			if(index<0)   index = 0;

			gamma[index]   += 1./2.*pow(observation1->value - observation2->value,2.);
			counter[index] += 1.;
		}
	}

	/*Normalize semivariogram*/
	gamma[0]=0.;
	for(k=0;k<n;k++){
		if(counter[k]) gamma[k] = gamma[k]/counter[k];
	}

	/*Assign output pointer*/
	xDelete<IssmDouble>(counter);
}/*}}}*/
