/**------------------------------------------------------------------------
 * @file
 *
 * Copyright 2001 by the University of Toronto (UT).
 *
 *                       All Rights Reserved
 * 
 * Permission to copy and modify this software and its documentation 
 * only for internal use in your organization is hereby granted, 
 * provided that this notice is retained thereon and on all copies.  
 * UT makes no representations as to the suitability and operability 
 * of this software for any purpose.  It is provided "as is" without 
 * express or implied warranty.
 *
 * UT DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING
 * ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT
 * SHALL UT BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
 * DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
 * OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
 * TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
 * PERFORMANCE OF THIS SOFTWARE.
 *
 * No other rights, including for example, the right to redistribute 
 * this software and its documentation or the right to prepare 
 * derivative works, are granted unless specifically provided in a 
 * separate license agreement.
 *
 * Author: Diego Macrini
 * 
 * $Log: ApproxPoly.cpp,v $
 *
 *
 *-----------------------------------------------------------------------*/
#include <stdlib.h>
#include <iostream>
#include "ApproxBezier.h"
#include "HelperFunctions.h"
#include "BasicUtils.h"

#define GROW_FACT 10

#ifndef PI
#define PI 3.14159265
#endif

using namespace dml;

void SetColinearCtrlPt(const POINT* vertices, double& x1, double& y1, double& x2, double& y2);

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

ApproxBezier::ApproxBezier(int nMaxRetry, double dMinError, double dMinSlope, 
	double dMaxSegments, double dMaxYDiff) : ApproxPoly(dMinError, dMinSlope, dMaxSegments, dMaxYDiff)
{
	Clear();
	m_nMaxRetry = nMaxRetry;
}

ApproxBezier::~ApproxBezier()
{

}

void ApproxBezier::Clear()
{
	x0 = y0 = x1 = y1 = x2 = y2 = x3 = y3 = 0;
	a0 = b0 = a1 = b1 = a2 = b2 = a3 = b3 = 0;
	x1g = y1g = 0;
}

void ApproxBezier::CalcError(double x4, double y4, const double& u, double* s, double* z)
{
	double x, y, dx4, dy4, dx, dy;

	x = a0 + u * (a1 + u *(a2 + u * a3));
	y = b0 + u * (b1 + u *(b2 + u * b3));
	
	dx4 = x - x4;
	dy4 = y - y4;

	dx = a1 + u *(2 * a2 + u * 3 * a3);
	dy = b1 + u *(2 * b2 + u * 3 * b3);

	*z = dx * dx4 + dy * dy4;
	*s = dx4 * dx4 + dy4 * dy4;
}

double ApproxBezier::GetDistance(const POINT* vertices, int n)
{
	double totalerror = 0, stepsize, s, z, u, u1 = 0, s1 = 0, z1 = 0, u2, z2, temp;
	double x4, y4;

	a3 = (x3 - x0 + 3 * (x1 - x2)) / 8;
	b3 = (y3 - y0 + 3 * (y1 - y2)) / 8;
	a2 = (x3 + x0 - x1 - x2) * 3 / 8;
	b2 = (y3 + y0 - y1 - y2) * 3 / 8;
	a1 = (x3 - x0) / 2 - a3;
	b1 = (y3 - y0) / 2 - b3;
	a0 = (x3 + x0) / 2 - a2;
	b0 = (y3 + y0) / 2 - b2;

	for (int I = 2; I <= n - 2; I += 2)
	{
		x4 = vertices[I].x;
		y4 = vertices[I + 1].y;

		stepsize = 2.0 / (n + 1);

		for (u = -1; u <= 1.01; u += stepsize)
		{
			CalcError(x4, y4, u, &s, &z);

			if (s == 0)
			{
				u1 = u;
				z1 = z;
				s1 = s;
				break;
			}

			if (u == -1)
			{
				u1 = u;
				z1 = z;
				s1 = s;
			}

			if (s < s1)
			{
				u1 = u;
				z1 = z;
				s1 = s;
			}
		}

		if (s1 != 0.0)
		{
			u = u1 + stepsize;

			if (u > 1)
				u =1 - stepsize;

			while (true)
			{
				CalcError(x4, y4, u, &s, &z);

				if (s == 0 || z == 0) break;
				
				u2 = u;
				z2 = z;
				temp = z2 - z1;

				if (temp != 0.0)
					u = (z2 * u1 -z1 * u2) / temp;
				else
					u =(u1 + u2) / 2.0;

				if (u > 1)
					u = 1;
				else if (u < -1)
					u = -1;

				if (fabs(u - u2) < .0001) break;

				u1 = u2;
				z1 = z2;
			}
		}

		totalerror = totalerror + s;
	}

	//PRINT totalerror;
	//PRINT #1, USING "####.### ####.### ####.### ####.### ######.###"; x1; y1; x2; y2; totalerror
	
	return totalerror;
}

double ApproxBezier::LeastSquares(const POINT* vertices, int n, SEGMENT& s)
{
	//static int example = 1;
	double e1, e2, e3;
	double x1a, y1a, x2a, y2a;
	bool bSlopeConstr = (x1g == 0 && y1g == 0) ? false:true;

	if (n < 4)
		return -1; // real fit error will never be negative.

	n--; // can this be changed to look better?

	x0 = vertices[0].x;
	y0 = vertices[1].y;
	x3 = vertices[n - 1].x;
	y3 = vertices[n].y;
	
	if (n == 3) // just 2 points
	{
		x1 = (3 * x0 + x3) / 4;
		y1 = (3 * y0 + y3) / 4;
		x2 = (3 * x3 + x0) / 4;
		y2 = (3 * y3 + y0) / 4;

		//SetColinearCtrlPt(vertices, x1, y1, x2, y2);
		return 0.0;
	}
	else
	{
		x1 = vertices[2].x;
		y1 = vertices[3].y;
		x2 = vertices[n - 3].x;
		y2 = vertices[n - 2].y;
	}

	/*if (example++ == 3)
	{
		// special guess for loop
		x1 = 8 * x1 - 7 * x0;
		y1 = 8 * y1 - 7 * y0;
		x2 = 8 * x2 - 7 * x3;
		y2 = 8 * y2 - 7 * y3;

		example = 1;
	}
	else
	{*/
		if (bSlopeConstr)
		{
			x1 = x1g;
			y1 = y1g;
		}
		else
		{
			x1 = 2 * x1 - x0;
			y1 = 2 * y1 - y0;
		}
		x2 = 2 * x2 - x3;
		y2 = 2 * y2 - y3;
	//}

	e1 = GetDistance(vertices, n);
	
	for (int Retry = 1; Retry <= m_nMaxRetry; Retry++)
	{
		e3 = .5;
		x1a = x1;
		
		if (!bSlopeConstr)
		{
			while (true)
			{
				x1 = x1 + (x1 - x0) * e3;
				e2 = GetDistance(vertices, n);

				if (e2 == e1) 
					break;
				else if (e2 > e1)
				{
					x1 = x1a;
					e3 = -e3 / 3;

					if (fabs(e3) < .001)
						break;
				}
				else
				{
					e1 = e2;
					x1a = x1;
				}
			}

			e3 = .5;
			y1a = y1;

			while (true)
			{
				y1 = y1 +(y1 - y0) * e3;
				e2 = GetDistance(vertices, n);

				if (e2 == e1)
					break;
				else if (e2 > e1)
				{
					y1 = y1a;
					e3 = -e3 /3;

					if (fabs(e3) < .01) 
						break;
				}
				else
				{
					e1 = e2;
					y1a = y1;
				}
			}
		}
		else
		{
			double delta;

			while (true)
			{
				delta = (x1 - x0) * e3;
				
				y1 = y1 + (y1 * delta) / x1;
				x1 = x1 + delta;

				e2 = GetDistance(vertices, n);

				if (e2 == e1) 
					break;
				else if (e2 > e1)
				{
					x1 = x1a;
					e3 = -e3 / 3;

					if (fabs(e3) < .001)
						break;
				}
				else
				{
					e1 = e2;
					x1a = x1;
				}
			}
		}

		e3 = .5;
		x2a = x2;

		while (true)
		{
			x2 = x2 + (x2 - x3) * e3;
			e2 = GetDistance(vertices, n);

			if (e2 == e1)
				break;
			else if (e2 > e1)
			{
				x2 = x2a;
				e3 = -e3 / 3;

				if (fabs(e3) < .01)
					break;
			}
			else
			{
				e1 = e2;
				x2a = x2;
			}
		}

		e3 =.5;
		y2a = y2;
		
		while(true)
		{
			y2 = y2 + (y2 - y3) * e3;
			e2 = GetDistance(vertices, n);

			if (e2 == e1)
				break;
			else if (e2 > e1)
			{
				y2 = y2a;
				e3 = -e3 /3;

				if (fabs(e3) < .01)
					break;
			}
			else
			{
				e1 = e2;
				y2a = y2;
			}
		}
	}
	
	// Set the parameters of the curve segment
	CBSegment& cbs = static_cast<CBSegment&>(s);
	cbs.p0.Set(x0,y0);
	cbs.p1.Set(x1,y1);
	cbs.p2.Set(x2,y2);
	cbs.p3.Set(x3,y3);
	
	return e1;
}

void ApproxBezier::Fit(const POINTS vertices)
{
	ApproxPoly poly(*this);
	
	poly.Fit(vertices);
	// Find meaningful vertices
	ApproxPoly::Fit(vertices);
}

void ApproxBezier::Draw(/*CDC* pDC, int nWidth, COLORREF color*/)
{
	/*if (x0 == 0 && y0 == 0) return;

	CPen pen(PS_SOLID, nWidth, color);
	POINT points[4] = {{x0, y0}, {x1, y1}, {x2, y2}, {x3, y3}};
	
	CPen* pOldPen = pDC->SelectObject(&pen);
	pDC->PolyBezier(points, 4);

	pDC->SelectObject(pOldPen);*/
}

void CBSegment::Draw(/*CDC* pDC, int nWidth, COLORREF color*/)
{
	/*CPen pen(PS_SOLID, nWidth, color);
	
	CPen* pOldPen = pDC->SelectObject(&pen);
	
	pDC->PolyBezier(m_points, 4);

	pDC->SelectObject(pOldPen);

	CPoint pt(m_points[3]);
	pDC->Rectangle(pt.x - 4, pt.y - 4, pt.x + 4, pt.y + 4);*/
}

void SetColinearCtrlPt(const POINT* vertices, double& x1, double& y1, double& x2, double& y2)
{
	double x0 = vertices[0].x;
	double y0 = vertices[1].y;
	double x3 = vertices[2].x;
	double y3 = vertices[3].y;

	double x02 = x0 * x0;
	double x32 = x3 * x3;
	
	double m = (y3 - y0) / double(x3 - x0);
	double m2 = m * m;

	double d2 = 1; // square of the target distance
	                   
	x1 = (-((-2 - 2 * m2) * x0) + sqrt(pow((-2 - 2 * m2 ), 2) * x02 - 
		4 * (1 + m2 ) * (-d2  + x02  + m2 * x02 ))) / (2 * (1 + m2));

	y1 = m * x1 + y0;

	x2 = (-((-2 - 2 * m2) * x3) - sqrt(pow((-2 - 2 * m2 ), 2) * x32 - 
		4 * (1 + m2 ) * (-d2  + x32  + m2 * x32 ))) / (2 * (1 + m2));
 
	y2 = m * x2 + y3;
}



