#ifndef GRID_MULTILIN_INTERPOLATOR_DEF
#define GRID_MULTILIN_INTERPOLATOR_DEF
#include "GridMultilinearInterpolator.h"



double GridMultilinearInterpolator::get_grid_z_value(const vector<long> &point) const{
	long index = 0, indexFactor = 1;
	for(long d=grid_points.size()-1; d>=0; --d){
		index += indexFactor * point[d];
		indexFactor *= grid_points[d].size();
	}
	return z_values[index];
}




void GridMultilinearInterpolator::string2numvector(const string &s, vector<double> &v){
	istringstream ss(s);
	v.clear();
	double d;
	do{
		ss >> d;
		if(ss.fail()) break;
		v.push_back(d);
	}while(true);
}


string GridMultilinearInterpolator::vector2string(const vector<double> &v, string separator){
	ostringstream ss;
	for(long n=0; n<v.size(); ++n){
		ss << (n==0 ? "" : separator) << v[n];
	}
	return ss.str();
}


void GridMultilinearInterpolator::explodeString(vector<string> &parts, const string &haystack, const string &separator, int maxPartsCount){
	parts.clear();
	int previousPos = 0, nextPos, separatorLen = separator.length();
	while(((nextPos = haystack.find(separator, previousPos)) != string::npos) && ((maxPartsCount>1) || (maxPartsCount<0))){
		parts.push_back(haystack.substr(previousPos, nextPos - previousPos));
		previousPos = nextPos + separatorLen;
		-- maxPartsCount;
	}
	parts.push_back(haystack.substr(previousPos));
}


bool GridMultilinearInterpolator::load(long D, const string &s){
	vector<string> parts;
	explodeString(parts, s, ",", D+1);
	if(parts.size()<D+1) return false;
	grid_points.resize(D);
	long totalGridSize = 1;
	for(long d=0; d<D; ++d){
		string2numvector(parts[d], grid_points[d]);
		totalGridSize *= grid_points[d].size();
	}
	string2numvector(parts[D], z_values);
	if(z_values.size() != totalGridSize) return false;
	return true;
}




string GridMultilinearInterpolator::getGridAsString() const{
	string s;
	const long D = grid_points.size();
	for(long d=0; d<D; ++d){
		s += (d==0 ? "" : ",") + vector2string(grid_points[d]," ");
	}
	s += "," + vector2string(z_values," ");
	return s;
}


bool GridMultilinearInterpolator::isWithinGrid(const vector<double> &location) const{
	const long D = grid_points.size();
	for(long d=0; d<D; ++d){
		if((grid_points[d].size()<2) || (grid_points[d].size()<2)) return false;
		if(location[d]+abs(location[d])*EPSILON<grid_points[d].front() || location[d]-abs(location[d])*EPSILON>grid_points[d].back()) return false;
	}
	return true;
}



long GridMultilinearInterpolator::findOn1DGrid(const vector<double> &gridValues, double value){
	if(gridValues.empty()) return -1;
	if(value<gridValues.front()) return -1;
	if(value>gridValues.back()) return gridValues.size();
	const double value_du = value + abs(value)*EPSILON;
	const double value_dd = value - abs(value)*EPSILON;
	for(long i=1; i<gridValues.size(); ++i){
		if(gridValues[i-1]<=value_du && gridValues[i]>=value_dd){ return i-1; }
	}
	return 0; // this should not happen
}




long GridMultilinearInterpolator::numberOfVertices(long D){
	long V = 1;
	for(long d=0; d<D; ++d){ V *= 2; }
	return V;
}



// do multilinear interpolation
// see:  http://bmia.bmt.tue.nl/people/BRomeny/Courses/8C080/Interpolation.pdf
double GridMultilinearInterpolator::interpolate(const vector<double> &location,
												double default_z_value, 
												bool flatExtrapolate) const{
	//find grid box
	const long D = grid_points.size();
	vector<long> box(D);
	vector<double> boxVolumeFraction(D);
	vector<long> N(D);
	
	//find and evaluate appropriate grid box (linear search)
	for(long d=0; d<D; ++d){
		N[d] = grid_points[d].size();
		if(N[d]==0) return (flatExtrapolate ? numeric_limits<double>::quiet_NaN() : default_z_value);
		box[d] = findOn1DGrid(grid_points[d], location[d]);
		if(box[d]<0 || box[d]>=N[d]){
			// not within s-grid
			if(!flatExtrapolate) return default_z_value;
			boxVolumeFraction[d] = 0;
		}else{
			// within s-grid
			boxVolumeFraction[d] = (location[d]-grid_points[d][box[d]])/(grid_points[d][box[d]+1] - grid_points[d][box[d]]);
		}
	}
	
	double z=0;
	long d;
	int s;
	const long Nvertices = numberOfVertices(D);
	long vertex_index, vertex_index_factor, remainder;
	double vertex_weight;
	for(long v=0; v<Nvertices; ++v){ // v enumerates all 2^D vertices of the focal box
		vertex_index=0; vertex_weight=1; vertex_index_factor=1;
		remainder = v;
		for(d=D-1; d>=0; --d){	// d enumerates the D grid dimensions
			s = remainder % 2;	// s identifies the vertex side (left or right) along the d-th dimension
			remainder = (remainder-s)/2;
			vertex_index += (box[d]<0 ? 0 : (box[d]>=N[d] ? N[d]-1 : (s==0 ? box[d] : box[d]+1))) * vertex_index_factor;
			vertex_index_factor *= N[d];
			vertex_weight *= (s==0 ? 1-boxVolumeFraction[d] : boxVolumeFraction[d]);
		}
		z += z_values[vertex_index] * vertex_weight;
	}
	return z;
}



double GridMultilinearInterpolator::interpolateWithDefault(const vector<double> &location, double default_z_value) const{
	return interpolate(	location,
						default_z_value, 
						false);
}


double GridMultilinearInterpolator::interpolate(const vector<double> &location) const{
	return interpolate(	location,
						0, 
						true);
}


double GridMultilinearInterpolator::interpolate(int dummy, ...) const{
	vector<double> location(grid_points.size());
	va_list args;
	va_start(args, dummy);
	for(long d=0; d<grid_points.size(); ++d){
		location[d] = va_arg(args, double);
	}
	va_end(args);
	return interpolate(location, 0, true);
}


double GridMultilinearInterpolator::interpolateWithDefault(double default_z_value, ...) const{
	vector<double> location(grid_points.size());
	va_list args;
	va_start(args, default_z_value);
	for(long d=0; d<grid_points.size(); ++d){
		location[d] = va_arg(args, double);
	}
	va_end(args);
	return interpolate(location, default_z_value, false);
}

void GridMultilinearInterpolator::setLayerPoints(const vector<vector<double> > &_layer_points){
	grid_points.resize(_layer_points.size()+1);
	grid_points[0].clear();
	for(long d=1; d<grid_points.size(); ++d){ grid_points[d] = _layer_points[d-1]; }
	z_values.clear();
}



void GridMultilinearInterpolator::setLayerPoints(long D, ...){
	grid_points.resize(D);
	grid_points[0].clear();
	va_list args;
	va_start(args, D);
	const char *s;
	for(long d=1; d<D; ++d){ 
		s = va_arg(args, const char*);
		string2numvector(s, grid_points[d]); 
	}
	va_end(args);
	z_values.clear();
}



bool GridMultilinearInterpolator::appendLayer(double location, const vector<double> &new_z_values){
	grid_points[0].push_back(location);
	z_values.insert(z_values.end(), new_z_values.begin(), new_z_values.end());
	return true;
}



long GridMultilinearInterpolator::getGridSize() const{
	if(grid_points.empty()) return 0;
	long S=1;
	for(long d=0; d<grid_points.size(); ++d){
		S *= grid_points[d].size();
	}
	return S;
}




void GridMultilinearInterpolator::printGridScann(ostream &ss) const{
	const long gridSize = getGridSize();
	const long D = grid_points.size();
	vector<double> point(D);
	if(gridSize==0) return;
	long remainder, index1D, index, indexFactor, d;
	for(long p=0; p<gridSize; ++p){
		indexFactor = 1; index = 0; remainder = p;
		for(d=D-1; d>=0; --d){
			index1D = remainder % grid_points[d].size();
			remainder = (remainder - index1D)/grid_points[d].size();
			index += indexFactor * index1D;
			indexFactor *= grid_points[d].size();
			point[d] = grid_points[d][index1D];
		}
		for(d=0; d<D; ++d){ ss << (d==0 ? "" : "\t") << point[d]; }
		ss << "\t" << z_values[index] << "\n";
		
	}
}



//returns a one-line summary of the grid setup
string GridMultilinearInterpolator::getShortDescription() const{
	ostringstream ss;
	const long D = grid_points.size();
	ss << D << "-dimensional, multilinear grid interpolator: ";
	for(long d=0; d<D; ++d){
		ss << (d==0 ? "" : " x ") << grid_points[d].size();
	}
	ss << " grid";
	return ss.str();
}

#endif