//
//	Copyright © 2005 Nemetschek North America 
//	All Rights Reserved.
//
// Created by Bozhidar Stanchev

#include "StdAfx.h"
//#include "VWCubicInterpolation.h"

using namespace VWFC::Math;

VWCubicInterpolation::VWCubicInterpolation( const VWCubicInterpolation& cubicBezier )
{
	*this = cubicBezier;
}

VWCubicInterpolation::VWCubicInterpolation( const VWPoint2D&	point0, 
											const VWPoint2D&	point1, 
											const VWPoint2D&	point2, 
											const VWPoint2D&	point3 )
{
	this->fPoint0		= point0;
	this->fPoint1		= point1;
	this->fPoint2		= point2;
	this->fPoint3		= point3;

	// valid intervals
	this->FindIntervals();
}

void	VWCubicInterpolation::Set( const VWPoint2D& point0,
								   const VWPoint2D& point1,
								   const VWPoint2D& point2,
								   const VWPoint2D& point3 )
{
	this->fPoint0		= point0;
	this->fPoint1		= point1;
	this->fPoint2		= point2;
	this->fPoint3		= point3;

	// valid intervals
	this->FindIntervals();
}

VWPoint2D	VWCubicInterpolation::Pt0() const
{ 
	return ( this->fPoint0 );
}

VWPoint2D	VWCubicInterpolation::Pt1() const
{ 
	return ( this->fPoint1 );
}

VWPoint2D	VWCubicInterpolation::Pt2() const
{
	return ( this->fPoint2 );
}

VWPoint2D	VWCubicInterpolation::Pt3() const
{
	return ( this->fPoint3 );
}

VWPoint2D VWCubicInterpolation::StartPt() const
{
	return ( this->fPoint0 );
}

VWPoint2D VWCubicInterpolation::EndPt() const
{
	return ( this->fPoint3 );
}

bool VWCubicInterpolation::PtOn( double tX, double tY,
								 const VWPoint2D& prevAddedPt, const double& addInterval,
								 const VWPoint2D& nextPt, VWPoint2D& result )
{
	double tolerance	= addInterval * 0.2;
	double ctrlDist		= 1.5 * prevAddedPt.DistanceTo( nextPt );

	double a0, a1, a2, a3;

	VWPoint2D	res1,	res2;
	double		dist1,	dist2;

	bool		bCorrect1	= true;
	bool		bCorrect2	= true;

	// find the XtoY polynomial
	if ( FindPolynomialXtoY( a0, a1, a2, a3 ) ) {
		// prevent incorrect parameter value
		if		( tX < fMinX )		tX	= fMinX;
		else if ( tX > fMaxX )		tX	= fMaxX;

		res1.x	= tX;
		res1.y	= a0 + a1 * tX + a2 * tX * tX + a3 * tX * tX * tX;
		double toPrevDist	= res1.DistanceTo( prevAddedPt );
		if ( Abs( toPrevDist - addInterval ) < tolerance ) {
			dist1	= toPrevDist + res1.DistanceTo( nextPt );
			if ( dist1 > ctrlDist )	bCorrect1	= false;
		}
		else	bCorrect1	= false;
	}
	else	bCorrect1	= false;

	// find the YtoX polynomial
	if ( FindPolynomialYtoX( a0, a1, a2, a3 ) )
	{
		// prevent incorrect parameter value
		if		( tY < fMinY )		tY	= fMinY;
		else if ( tY > fMaxY )		tY	= fMaxY;

		res2.y	= tY;
		res2.x	= a0 + a1 * tY + a2 * tY * tY + a3 * tY * tY * tY;
		double toPrevDist	= res2.DistanceTo( prevAddedPt );
		if ( Abs( toPrevDist - addInterval ) < tolerance ) {
			dist2	= toPrevDist + res2.DistanceTo( nextPt );
			if ( dist2 > ctrlDist )	bCorrect2	= false;
		}
		else	bCorrect2	= false;
	}
	else
		bCorrect2	= false;

	// find correct result
	if ( bCorrect1 && bCorrect2 ) {
		if ( dist1 < dist2 )	result = res1;
		else					result = res2;
	}
	else if ( bCorrect1 ) {
		result = res1;
	}
	else if ( bCorrect2 ) {
		result = res2;
	}
	else {
		return false;
	}

	return true;
}

bool	VWCubicInterpolation::FindPolynomialXtoY( double& a0, double& a1, double& a2, double& a3 )
{
	// linear system matrix
	double LSystem[4][5];
	
	// initialization of this matrix
	// row 1
	LSystem[0][0] = 1;			LSystem[0][4] = fPoint0.y;
	LSystem[0][1] = fPoint0.x;	LSystem[0][2] = fPoint0.x * fPoint0.x;	LSystem[0][3] = fPoint0.x * fPoint0.x * fPoint0.x;

	// row 2
	LSystem[1][0] = 1;			LSystem[1][4] = fPoint1.y;
	LSystem[1][1] = fPoint1.x;	LSystem[1][2] = fPoint1.x * fPoint1.x;	LSystem[1][3] = fPoint1.x * fPoint1.x * fPoint1.x;

	// row 3
	LSystem[2][0] = 1;			LSystem[2][4] = fPoint2.y;
	LSystem[2][1] = fPoint2.x;	LSystem[2][2] = fPoint2.x * fPoint2.x;	LSystem[2][3] = fPoint2.x * fPoint2.x * fPoint2.x;

	// row 3
	LSystem[3][0] = 1;			LSystem[3][4] = fPoint3.y;
	LSystem[3][1] = fPoint3.x;	LSystem[3][2] = fPoint3.x * fPoint3.x;	LSystem[3][3] = fPoint3.x * fPoint3.x * fPoint3.x;

	// solve the linear system
	return	SolveLSystem( LSystem, a0, a1, a2, a3 );
}

bool	VWCubicInterpolation::FindPolynomialYtoX( double& a0, double& a1, double& a2, double& a3 )
{
	// linear system matrix
	double LSystem[4][5];
	
	// initialization of this matrix
	// row 1
	LSystem[0][0] = 1;			LSystem[0][4] = fPoint0.x;
	LSystem[0][1] = fPoint0.y;	LSystem[0][2] = fPoint0.y * fPoint0.y;	LSystem[0][3] = fPoint0.y * fPoint0.y * fPoint0.y;

	// row 2
	LSystem[1][0] = 1;			LSystem[1][4] = fPoint1.x;
	LSystem[1][1] = fPoint1.y;	LSystem[1][2] = fPoint1.y * fPoint1.y;	LSystem[1][3] = fPoint1.y * fPoint1.y * fPoint1.y;

	// row 3
	LSystem[2][0] = 1;			LSystem[2][4] = fPoint2.x;
	LSystem[2][1] = fPoint2.y;	LSystem[2][2] = fPoint2.y * fPoint2.y;	LSystem[2][3] = fPoint2.y * fPoint2.y * fPoint2.y;

	// row 3
	LSystem[3][0] = 1;			LSystem[3][4] = fPoint3.x;
	LSystem[3][1] = fPoint3.y;	LSystem[3][2] = fPoint3.y * fPoint3.y;	LSystem[3][3] = fPoint3.y * fPoint3.y * fPoint3.y;

	// solve the linear system
	return	SolveLSystem( LSystem, a0, a1, a2, a3 );
}

void	VWCubicInterpolation::FindIntervals()
{
	fMinX	= fPoint0.x;
	if ( fMinX > fPoint1.x )	fMinX	= fPoint1.x;
	if ( fMinX > fPoint2.x )	fMinX	= fPoint2.x;
	if ( fMinX > fPoint3.x )	fMinX	= fPoint3.x;

	fMaxX	= fPoint0.x;
	if ( fMaxX < fPoint1.x )	fMaxX	= fPoint1.x;
	if ( fMaxX < fPoint2.x )	fMaxX	= fPoint2.x;
	if ( fMaxX < fPoint3.x )	fMaxX	= fPoint3.x;

	fMinY	= fPoint0.y;
	if ( fMinY > fPoint1.y )	fMinY	= fPoint1.y;
	if ( fMinY > fPoint2.y )	fMinY	= fPoint2.y;
	if ( fMinY > fPoint3.y )	fMinY	= fPoint3.y;

	fMaxY	= fPoint0.y;
	if ( fMaxY < fPoint1.y )	fMaxY	= fPoint1.y;
	if ( fMaxY < fPoint2.y )	fMaxY	= fPoint2.y;
	if ( fMaxY < fPoint3.y )	fMaxY	= fPoint3.y;
}

bool	VWCubicInterpolation::SolveLSystem( double LSystem[4][5], double& a0, double& a1, double& a2, double& a3 )
{
	double epsilon = 1e-10;

	size_t iRows	= 4;
	size_t iColumns	= 5;

	// for any Xi eliminate
	for ( size_t i = 0 ; i < iColumns - 1 ; i ++ ) {
		// find non-zero coeficient for Xi
		bool	bFound	= false;
		size_t	j		= i;

		while ( ! bFound && j < iRows ) {
			if ( !MathUtils::Equalish( LSystem[j][i], 0.0, epsilon ) )	bFound = true;
			else														j++;
		}

		if ( !bFound )	return false;
		
		// exchange i and j rows
		for ( size_t k = 0 ; k < iColumns && i != j ; k ++ ) {
			VWFC::Tools::SwapVariables( LSystem[i][k], LSystem[j][k] );
		}

		double dNumber = -1.0 / LSystem[i][i];
		
		for ( size_t k = 0 ; k < iRows; k ++ ) {
			if ( k != i ) {
				// multiply column i and add to column k
				double *pS		= LSystem[ i ];
				double *pD		= LSystem[ k ];
				double coeff	= dNumber * LSystem[ k ][ i ];

				for ( size_t l = 0 ; l < iColumns ; l ++, pS ++, pD ++ )	*pD += (*pS) * coeff;

				LSystem[ k ][ i ] = 0.0;
			}
		}

	}

	// find the result
	double result[4];
	for ( size_t i = 0 ; i < iRows ; i ++ ) {
		if ( MathUtils::Equalish( LSystem[i][i], 0.0, epsilon ) )	return false;
		result[ i ] = LSystem[ i ][ iColumns-1 ] / LSystem[ i ][ i ];
	}

	a0 = result[ 0 ];	a1 = result[ 1 ];	
	a2 = result[ 2 ];	a3 = result[ 3 ];

	return true;
}