//- ****************************************************************************
//- 
//- Copyright 2009 Sandia Corporation. Under the terms of Contract
//- DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government
//- retains certain rights in this software.
//- 
//- BSD Open Source License.
//- All rights reserved.
//- 
//- Redistribution and use in source and binary forms, with or without
//- modification, are permitted provided that the following conditions are met:
//- 
//-    * Redistributions of source code must retain the above copyright notice,
//-      this list of conditions and the following disclaimer.
//-    * Redistributions in binary form must reproduce the above copyright
//-      notice, this list of conditions and the following disclaimer in the
//-      documentation and/or other materials provided with the distribution.
//-    * Neither the name of Sandia National Laboratories nor the names of its
//-      contributors may be used to endorse or promote products derived from
//-      this software without specific prior written permission.
//- 
//- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
//- AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
//- IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
//- ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
//- LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
//- CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
//- SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
//- INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
//- CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
//- ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//- POSSIBILITY OF SUCH DAMAGE.
//-
//- ****************************************************************************

#include <sstream>
#include <climits>
#include <cmath>

// **** _LOCAL INCLUDES_ *******************************************************

#include "GeoTessPosition.h"
#include "GeoTessException.h"
#include "CpuTimer.h"
#include "InterpolatorType.h"
#include "GeoTessPositionLinear.h"
#include "GeoTessPositionNaturalNeighbor.h"
#include "Profile.h"

// **** _BEGIN GEOTESS NAMESPACE_ **********************************************

namespace geotess {

// **** _EXPLICIT TEMPLATE INSTANTIATIONS_ *************************************

// **** _STATIC INITIALIZATIONS_************************************************

const double GeoTessPosition::TWALK_TOLERANCE = -1e-15;

// **** _FUNCTION IMPLEMENTATIONS_ *********************************************

/**
 * Constructor that takes a reference to the 3D model that is to be interrogated by this
 * GeoTessPosition3D object.
 *
 * @param model
 * @throws GeoTessException
 */
GeoTessPosition::GeoTessPosition(GeoTessModel* m, const InterpolatorType& radialType,
		vector< vector<double> >& c) :
		refCount(0), maxTessLevel(NULL), tessLevels(NULL), triangle(NULL),
		errorValue(NaN_DOUBLE),
		layerId(-1), tessid(-1),
		hCoefficients(c),
		radialInterpolatorType(radialType),
		model(m),
		grid(m->getGrid()), modlProfiles(m->getProfiles()),
		gridVertices(grid.getVertices()),
		gridTriangles(grid.getTriangles()),
		gridNeighbors(grid.getNeighbors()),
		gridEdges(grid.getEdges()),
		layerTessIds(m->getMetaData().getLayerTessIds()),
		nLayers(m->getNLayers()),
		radiusOutOfRangeAllowed(true)
{
	unitVector[0] = unitVector[1] = unitVector[2] = 0.0;

	radius = -1.0;
	tessid = -1;

	layerRadii.resize(nLayers + 1);
	for (int i = 0; i < nLayers + 1; ++i) layerRadii[i] = -1.0;

	int ntess = grid.getNTessellations();
	triangle = new int [ntess];
	CPPUtils::resetArray<int>(ntess, triangle, -1);
	tessLevels = new int [ntess];
	maxTessLevel = new int [ntess];
	CPPUtils::resetArray<int>(ntess, maxTessLevel, INT_MAX);

	vertices.resize(ntess);
	nnCoefficients.resize(ntess);
	linearCoefficients.resize(ntess);
	for (int i = 0; i < ntess; ++i) linearCoefficients[i].resize(3);
}

/**
 * Destructor.
 */
GeoTessPosition::~GeoTessPosition()
{
	if (refCount > 0)
	{
		ostringstream os;
		os << endl << "ERROR in GeoTessPosition::~GeoTessPosition()" << endl
			 << "Reference count (" << refCount << ") is not zero." << endl;
		throw GeoTessException(os, __FILE__, __LINE__, 3001);
	}

	if (maxTessLevel != NULL) delete [] maxTessLevel;
	if (tessLevels != NULL) delete [] tessLevels;
	if (triangle != NULL) delete [] triangle;
}

/**
 * Static factory method to create a new interpolator.
 */
GeoTessPosition* GeoTessPosition::getGeoTessPosition(GeoTessModel* model)
{
	return new GeoTessPositionLinear(model, InterpolatorType::LINEAR);
}

/**
 * Static factory method to create a new interpolator.
 */
GeoTessPosition*	GeoTessPosition::getGeoTessPosition(GeoTessModel* model,
		const InterpolatorType& horizontalType)
{
	switch (horizontalType.ordinal())
	{
	case 0: // LINEAR
		return new GeoTessPositionLinear(model, InterpolatorType::LINEAR);

	case 1: // NATURAL_NEIGHBOR
		return new GeoTessPositionNaturalNeighbor(model, InterpolatorType::CUBIC_SPLINE);

	default:
		ostringstream os;
		os << endl << "ERROR in GeoTessPosition::getGeoTessPosition" << endl
				<< "Unsupported InterpolatorType " << horizontalType.name() << endl
				<< "Must specify either LINEAR or NATURAL_NEIGHBOR." << endl;
		throw GeoTessException(os, __FILE__, __LINE__, 3002);
	}
}

/**
 * Static factory method to create a new interpolator.
 */
GeoTessPosition* GeoTessPosition::getGeoTessPosition(GeoTessModel* model,
		const InterpolatorType& horizontalType, const InterpolatorType& radialType)
{
	switch (horizontalType.ordinal())
	{
	case 0: // LINEAR
		return new GeoTessPositionLinear(model, radialType);

	case 1: // NATURAL_NEIGHBOR
		return new GeoTessPositionNaturalNeighbor(model, radialType);

	default:
		ostringstream os;
		os << endl << "ERROR in GeoTessPosition::getGeoTessPosition" << endl
				<< "Unsupported InterpolatorType " << horizontalType.name() << endl
				<< "Must specify either LINEAR or NATURAL_NEIGHBOR." << endl;
		throw GeoTessException(os, __FILE__, __LINE__, 3003);
	}
}

/**
 * Replace the model that currently supports this GeoTessPosition object
 * with a new model. For this to work, the new model and the current model
 * must use the same grid.
 *
 * <p>
 * The benefit of calling this method is that if the application needs to
 * interpolate a value at the same position in multiple models
 * that share the same grid, then the walking triangle algorithm and the
 * calculation of geographic interpolation coefficients do not have to be
 * repeated.  The radial interpolation information is updated by this
 * method as well so after the call to this method, GeoTessPosition is ready
 * to interpolate values from the new model.
 *
 *  * @param newModel
 *            model that is to replace the currently supported model.
 * @throws GeoTessException
 *             if the new model and current model do not have GeoTessGrids
 *             that have the same gridID.
 */
bool GeoTessPosition::setModel(GeoTessModel* newModel)
{
	if (newModel->getGrid().getGridID() != grid.getGridID())
	{
		ostringstream os;
		os << endl << "ERROR in GeoTessPosition::setModel" << endl
				<< "Specified model and current model use different grids." << endl;
		throw GeoTessException(os, __FILE__, __LINE__, 3004);
	}

	model = newModel;
	modlProfiles = model->getProfiles();

	layerTessIds = model->getMetaData().getLayerTessIds();
	nLayers = model->getMetaData().getNLayers();

	double r = radius;

	layerRadii.clear();
	radius = -1.;
	for (int i = 0; i <= nLayers; ++i)
		layerRadii.push_back(-1);

	updateRadius(layerId, r);

	return true;
}

/**
 * Returns true if none of the current interpolation vertices have empty profiles.
 */
bool GeoTessPosition::noEmptyProfiles()
{
	vector<int>& v = vertices[tessid];
	for (int i = 0; i < (int) v.size(); ++i)
		if (modlProfiles[v[i]][layerId]->getType() == ProfileType::EMPTY)
			return false;
	return true;
}

/**
 * Set the 2D position and ensure that the triangle, vertices and 2D
 * interpolation coefficients for the tessellation that supports layerid
 * have been updated. Sets radius = -1.  Must call updateRadius after
 * this call to ensure that radius is set properly.
 *
 * @param lid
 * @param uVector
 * @throws GeoTessException
 */
void GeoTessPosition::updatePosition2D(int lid, const double* const uVector)
{
	tessid  = layerTessIds[lid];

	if ((triangle[tessid] < 0) || (unitVector[0] != uVector[0]) ||
			(unitVector[1] != uVector[1]) || (unitVector[2] != uVector[2]))
	{
		// the vector position has changed. Update everything.

		// nullify the triangle index for all tessellations other than the
		// current one.
		int ntess = grid.getNTessellations();
		for (int tess = 0; tess < ntess; ++tess)
			if (tess != tessid)
				triangle[tess] = -1;

		// 0.961261696 is cos(16 degrees)
		// if new position is more than 16 degrees away from current
		// position then start walk from triangle zero, otherwise,
		// start walk from current triangle
		if ((triangle[tessid] < 0) || GeoTessUtils::dot(uVector, unitVector) < 0.961261696)
		{
			triangle[tessid] = grid.getTriangle(tessid, 0, 0);
			tessLevels[tessid] = 0;
		}

		unitVector[0] = uVector[0];
		unitVector[1] = uVector[1];
		unitVector[2] = uVector[2];

		// perform walking triangle algorithm, which will set
		// triangle[tessid], lienarCoefficients[tessid] and tessLevels[tessid]
		getContainingTriangle(tessid);

		// determine vertices and coefficients for interpolation in geographic
		// dimensions. This is an abstract method so the results depend on the
		// interpolator type.
		update2D(tessid);

		// nullify previously computed earth radius.
		earthRadius = -1;

		// set current radius to -1, forcing recalculation of radial
		// interpolation coefficients after this method is done.
		radius = -1.;

		// nullify all previously computed layer radii (layer boundaries).
		for (int i = 0; i < (int) layerRadii.size(); ++i) layerRadii[i] = -1.0;

		clearRadialCoefficients();
		if (radialIndexes.size() < vertices[tessid].size())
		{
			radialIndexes.resize(vertices[tessid].size());
			radialCoefficients.resize(vertices[tessid].size());
		}

	}
	else
		// the 2D position did not change but the layerId/tessid might have.
		checkTessellation(tessid);
}

/**
 * Find the index of the triangle that contains position. Also computes the 3 interpolation
 * coefficients that can be used to interpolate data stored on the vertices of the returned
 * triangle.
 * <p>
 * A GeoTessPosition object has an attribute maxTessLevel that defaults to Integer.maxValue. The
 * search is limited to that tessellation level. So the triangle identified by this method will
 * reside either on x.maxTessLevel, or the largest tessellation level of the tessellation,
 * whichever is larger.
 */
void GeoTessPosition::getContainingTriangle(int tid)
{
	int t = triangle[tid];
	int tessLevel = tessLevels[tid];
	vector<double>& c = linearCoefficients[tid];
	int maxTess = maxTessLevel[tid];

	if (gridEdges == NULL)
	{
		while (true)
		{
			c[0] = GeoTessUtils::scalarTripleProduct(
					gridVertices[gridTriangles[t][2]],
					gridVertices[gridTriangles[t][1]], unitVector);
			if (c[0] > GeoTessPosition::TWALK_TOLERANCE)
			{
				c[1] = GeoTessUtils::scalarTripleProduct(
						gridVertices[gridTriangles[t][0]],
						gridVertices[gridTriangles[t][2]], unitVector);
				if (c[1] > GeoTessPosition::TWALK_TOLERANCE)
				{
					c[2] = GeoTessUtils::scalarTripleProduct(
							gridVertices[gridTriangles[t][1]],
							gridVertices[gridTriangles[t][0]],
							unitVector);
					if (c[2] > GeoTessPosition::TWALK_TOLERANCE)
					{
						if ((gridNeighbors[t][3] < 0) || (tessLevel >= maxTess))
						{
							// the correct triangle has been found.
							// Normalize the x.coefficients
							// such that they sum to one.
							double sum =	c[0] + c[1] + c[2];
							c[0] /= sum;
							c[1] /= sum;
							c[2] /= sum;
							triangle[tid] = t;
							tessLevels[tid] = tessLevel;
							return;
						}
						else
						{
							++tessLevel;
							t = gridNeighbors[t][3];
						}
					}
					else
						t = gridNeighbors[t][2];
				}
				else
					t = gridNeighbors[t][1];
			}
			else
				t = gridNeighbors[t][0];
		}
	}
	else
	{
		while (true)
		{
			//cout << "GeoTessPosition::getContainingTriangle t=" << t << endl;
			c[0] = GeoTessUtils::dot(gridEdges[t][0], unitVector);
			if (c[0] > GeoTessPosition::TWALK_TOLERANCE)
			{
				c[1] = GeoTessUtils::dot(gridEdges[t][1], unitVector);
				if (c[1] > GeoTessPosition::TWALK_TOLERANCE)
				{
					c[2] = GeoTessUtils::dot(gridEdges[t][2], unitVector);
					if (c[2] > GeoTessPosition::TWALK_TOLERANCE)
					{
						if ((gridNeighbors[t][3] < 0) || (tessLevel >= maxTess))
						{
							// the correct triangle has been found.
							// Normalize the coefficients
							// such that they sum to one.
							double sum =	c[0] + c[1] + c[2];
							c[0] /= sum;
							c[1] /= sum;
							c[2] /= sum;
							triangle[tid] = t;
							tessLevels[tid] = tessLevel;
							return;
						}
						else
						{
							++tessLevel;
							t = gridNeighbors[t][3];
						}
					}
					else
						t = gridNeighbors[t][2];
				}
				else
					t = gridNeighbors[t][1];
			}
			else
				t = gridNeighbors[t][0];
		}
	}
}

/**
 * Retrieve an interpolated value of the specified model attribute.
 *
 * @param attribute
 * @return interpolated value of the specified model attribute.
 * @throws GeoTessException
 */
double GeoTessPosition::getValue(int attribute)
{
	vector<int>& v = vertices[tessid];
	vector<double>& h = hCoefficients[tessid];

	double value = 0;
	if (&radialInterpolatorType == &InterpolatorType::CUBIC_SPLINE)
		for (int i = 0; i < (int)v.size(); ++i)
			value += modlProfiles[v[i]][layerId]->getValue(radialInterpolatorType, attribute,
					radius, radiusOutOfRangeAllowed) * h[i];
	else
	{
		updateRadialCoefficients();
		for (int i = 0; i < (int)v.size(); ++i)
			value += modlProfiles[v[i]][layerId]->getValue(radialIndexes[i], radialCoefficients[i], attribute)
			* h[i];
	}

	return isnan(value) ? getErrorValue() : value;
}

/**
 * Retrieve an interpolated value of the radius of the top of the specified layer, in km.
 *
 * @param layer
 * @return interpolated value of the radius of the top of the specified layer, in km.
 * @throws GeoTessException
 */
double GeoTessPosition::getRadiusTop(int layer)
{
	if (layerRadii[layer + 1] < 0)
	{
		int tid = layerTessIds[layer];

		if (layer < nLayers-1 && layerTessIds[layer+1] != tid)
		{
			// the next layer above the current layer is in a different
			// multi-level tessellation.  The containing triangle in the next layer
			// may be smaller and provide a more accurate estimate of
			// the radius at current position.  Have to evaluate this.

			int tid2 = layerTessIds[layer+1];

			int t1 = getTriangle(tid);
			int t2 = getTriangle(tid2);
			if (biggerTriangle(t1, t2) == t1)
				// triangle on next layer is smaller than triangle on current layer
				tid = tid2;
		}

		vector<int>& v = vertices[tid];
		vector<double>& c = hCoefficients[tid];
		layerRadii[layer + 1]=0.;
		for (int i = 0; i < (int) v.size(); ++i)
			layerRadii[layer + 1] += modlProfiles[v[i]][layer]->getRadiusTop() * c[i];
	}
	return isnan(layerRadii[layer + 1]) ? getErrorValue() : layerRadii[layer + 1];
}

/**
 * Retrieve an interpolated value of the radius of the bottom of the specified layer, in km.
 *
 * @param layer
 * @return interpolated value of the radius of the bottom of the specified layer, in km.
 * @throws GeoTessException
 */
double GeoTessPosition::getRadiusBottom(int layer)
{
	if (layerRadii[layer] < 0)
	{
		int tid = layerTessIds[layer];

		if (layer > 0 && layerTessIds[layer-1] != tid)
		{
			// the layer below the current layer is in a different
			// multi-level tessellation.  The containing triangle in the previous layer
			// may be smaller and provide a more accurate estimate of
			// the radius at current position.  Have to evaluate this.

			int tid2 = layerTessIds[layer-1];

			int t1 = getTriangle(tid);
			int t2 = getTriangle(tid2);
			if (biggerTriangle(t1, t2) == t1)
				// triangle on previous layer is smaller than triangle on current layer
				tid = tid2;
		}

		vector<int>& v = vertices[tid];
		vector<double>& c = hCoefficients[tid];
		layerRadii[layer]=0.;
		for (int i = 0; i < (int) v.size(); ++i)
			layerRadii[layer] += modlProfiles[v[i]][layer]->getRadiusBottom() * c[i];
	}
	return isnan(layerRadii[layer]) ? getErrorValue() : layerRadii[layer];
}

/**
 * Retrieve the index of the vertex with the highest interpolation
 * coefficient.
 */
int	GeoTessPosition::getIndexOfClosestVertex() const
{
	const vector<double>& c = hCoefficients[tessid];
	int vertex = 0;
	for (int i = 1;  i < (int) c.size(); ++i)
		if (c[i] > c[vertex])	vertex = i;
	return vertices[tessid][vertex];
}

/**
 * Output this GeoTessPosition as a string.
 */
string GeoTessPosition::toString()
{
	char s[300];
	sprintf(s, "Triangle %7d layer %2d  lat, lon, depth: %1.6f, %1.6f, %1.3f",
			triangle[tessid], layerId,
			GeoTessUtils::getLatDegrees(unitVector),
			GeoTessUtils::getLonDegrees(unitVector), getDepth());
	ostringstream os;
	os << endl << s << endl << endl;
	os << "  Node       Lat        Lon      Coeff    Dist" << endl;

	vector<int>& v = vertices[tessid];
	vector<double>& c = hCoefficients[tessid];
	for (int i = 0; i < (int) v.size(); ++i)
	{
		string lls = GeoTessUtils::getLatLonString(grid.getVertex(v[i]));
		sprintf(s, "%6d %s %10.6f %7.3f", v[i], lls.c_str(),
				c[i], CPPUtils::toDegrees(GeoTessUtils::angle(unitVector,
						grid.getVertex(v[i]))));
		os << s << endl;
	}
	return os.str();
}

} // end namespace geotess
