/**------------------------------------------------------------------------
 * @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 "ApproxPoly.h"
#include "HelperFunctions.h"
#include "BasicUtils.h"

#define GROW_FACT 10

#ifndef PI
#define PI 3.14159265
#endif

using namespace dml;

ApproxPoly::ApproxPoly(double dMinError, double dMinSlope, double dMaxSegments, double dMaxYDiff)
{
	m_dMinError = dMinError;
	m_dMinSlope = dMinSlope;
	m_dMaxSegments = dMaxSegments;
	m_dMaxYDiff = dMaxYDiff;
	m_bDbgMode = false;
}

ApproxPoly::ApproxPoly(const ApproxPoly& poly)
{
	m_dMinError = poly.m_dMinError;
	m_dMinSlope = poly.m_dMinSlope;
	m_dMaxSegments = poly.m_dMaxSegments;
	m_dMaxYDiff = poly.m_dMaxYDiff;
	m_bDbgMode = poly.m_bDbgMode;
}

ApproxPoly::~ApproxPoly()
{
}

double ApproxPoly::LeastSquares(const POINT* vertices, int fromIdx, int toIdx, SEGMENT& s)
{
	return LeastSquares(vertices + fromIdx, toIdx - fromIdx + 1, s);
}

/*!
@brief Finds the best fit line mx + b, where

 m = (n Sum(xy) - Sum(x) Sum(y)) / (n Sum(x^2) - (Sum(x))^2)
 b = mean(y) - m mean(x)

  we could also find it by the projecton matrix: P = A (A^t A)^-1 A^t
*/
double ApproxPoly::LeastSquares(const POINT* vertices, int n, SEGMENT& s)
{
	int i;
	double x, y, sumx = 0, sumy = 0, sumx2 = 0, sumxy = 0, sum2x = 0;

	ASSERT(n > 1);
	
	for (i = 0; i < n; i++)
	{
		x = vertices[i].x;
		y = vertices[i].y;
		
		sumx += x;
		sumy += y;
		sumx2 += x * x;
		sumxy += x * y;
	}
	
	sum2x = sumx * sumx;
	
	// Set the parameters of the segment
	s.m = (n * sumxy - sumx * sumy) / (n * sumx2 - sum2x);
	s.b = (sumy - s.m * sumx) / n; // == (sumx2 * sumy - sumxy * sumx) / double(n * sumx2 - sum2x);
	s.p0 = vertices[0];
	s.p1 = vertices[n - 1];

	ASSERT_VALID_NUM(s.m);
	ASSERT_VALID_NUM(s.b);
	
	// Compute the squared error
	double e, sume2 = 0;
	
	for(i = 0; i < n; i++)
	{
		e = s.m * vertices[i].x + s.b - vertices[i].y;
		sume2 += e * e;
	}
	
	return sume2;
}

bool ApproxPoly::IsRelativeSmall(double x0, double xn, double m, double b)
{
	double mx, dx, r;
	
	mx = (x0 + xn) / 2;
	dx = fabs(x0 - xn);
		
	r = fabs(m * dx) / (m * mx + b); // dy / mean y
	
	ASSERT(r >= 0.0);
		
	return r < m_dMinSlope;
}

double ApproxPoly::FindMaxYDiff(int seg_num, int s, int e) const
{
	const SEGMENT *pFirstSeg, *pLastSeg;
	double  dMaxYDiff;
	
	FindMaxYDiff(seg_num, s, e, pFirstSeg, pLastSeg, dMaxYDiff);
	
	return dMaxYDiff;
}

void ApproxPoly::FindMaxYDiff(int seg_num, int s, int e, const SEGMENT*& pFirstSeg, const SEGMENT*& pLastSeg, double& dMaxYDiff) const
{
	const MEMDATA& d = m_minerrors[seg_num][s][e];
	int j = d.GetIdx();
	
	if (seg_num > 1)
	{
		const SEGMENT *pFirstLeft, *pLastLeft, *pFirstRight, *pLastRight;
		double dMaxLeftYDiff, dMaxRightYDiff;
		
		FindMaxYDiff(seg_num / 2, s, j, pFirstLeft, pLastLeft, dMaxLeftYDiff);
		FindMaxYDiff(seg_num / 2 + seg_num % 2, j, e, pFirstRight, pLastRight, dMaxRightYDiff);
		
		double diff = fabs(pLastLeft->Y1() - pFirstRight->Y0()) / MAX(pLastLeft->Y1(), pFirstRight->Y0());
		
		if (diff > dMaxLeftYDiff && diff > dMaxRightYDiff)
			dMaxYDiff = diff;
		else if (dMaxLeftYDiff > dMaxRightYDiff)
			dMaxYDiff = dMaxLeftYDiff;
		else
			dMaxYDiff = dMaxRightYDiff;
		
		pFirstSeg = pFirstLeft;
		pLastSeg = pLastRight;
	}
	else if (seg_num == 1)
	{
		pFirstSeg = &m_segments[s][e];
		pLastSeg = &m_segments[s][e];
		dMaxYDiff = 0;
	}
}

/*!
	The MaxYDiff is only check when m_dMaxYDiff > 0
*/
void ApproxPoly::Fit(const POINTS pts)
{
	int seg_num, n = pts.GetSize();
	MEMDATA d;
	double dMaxYDiff = m_dMaxYDiff; // init val allows us to ignore check (see <= below)
	
	ASSERT(n > 1);
	
	m_points = pts;
	m_segments.ReSize(n, n);
	m_minerrors.ReSize(n);
	m_knots.Clear();
	
	//SetYMinMax(pts, n);
	
	for (seg_num = 1; seg_num < n; seg_num++)
	{
		d = Min(seg_num, 0, n - 1);
		
		if (m_dMaxYDiff > 0)
			dMaxYDiff = FindMaxYDiff(seg_num, 0, n - 1);

		if ((d.GetMinError() <= m_dMinError && dMaxYDiff <= m_dMaxYDiff) || seg_num >= m_dMaxSegments)
			break;
	}
	
	WARNING(seg_num == m_dMaxSegments, "Max num segments reached. Consider changing this threshold.");
	
	if (seg_num == n)
		seg_num--;
		
	AddKnots(seg_num, 0, n - 1);
	
	if (m_bDbgMode)
		PlotKnots(seg_num);
}

MEMDATA ApproxPoly::Min(int seg_num, int s, int e)
{	
	if (m_minerrors[seg_num].GetSize() == 0)
		m_minerrors[seg_num].ReSize(m_points.GetSize(), m_points.GetSize());
		
	if (m_minerrors[seg_num][s][e].IsEmpty())
	{
		double minerror = 99999;
		int minpt = 0;
		
		if (seg_num == 1)
		{
			SEGMENT* pSegParams = NewSegment();
			minerror = LeastSquares(m_points, s, e, *pSegParams);
			m_segments[s][e].Set(*pSegParams);
			delete pSegParams;
			minpt = s;
		}
		else
		{	
			double m;
			int n_left = seg_num / 2;
			int n_right = seg_num / 2 + seg_num % 2;
		
			for (int i = s + 1; i <= e - 1 ; i++)
			{
				m = Min(n_left, s, i) + Min(n_right, i, e);
			
				if (m < minerror)
				{
					minerror = m;
					minpt = i;
				}
			}
		}
		
		m_minerrors[seg_num][s][e].Set(minerror, minpt);
	}
	
	return m_minerrors[seg_num][s][e];
}

int ApproxPoly::AddKnots(int seg_num, int s, int e)
{
	const MEMDATA& d = m_minerrors[seg_num][s][e];
	int j = d.GetIdx();
	
	if (seg_num > 1)
	{
		/*int i = */AddKnots(seg_num / 2, s, j);
		int k = AddKnots(seg_num / 2 + seg_num % 2, j, e);
		return k;
	}
	else if (seg_num == 1)
	{
		KNOT knot(e, m_segments[s][e], d.GetMinError());
		SEGMENT& s = knot.seg;
		
		knot.dir = IsRelativeSmall(s.p0.x, s.p1.x, s.m, s.b) ? 0:SIGN(s.m);
		m_knots.AddTail(knot);
	}
	
	return j;
}

void ApproxPoly::PlotKnots(int seg_num)
{
	static int dataidx = 0;	
		
	std::cout << "\n% DATA " << ++dataidx << "...\na = ";
	m_points.Print(std::cout, "\n");
	std::cout << ";\n% Fitting data " << dataidx << "...\n\n";
	
	MEMDATA d = m_minerrors[seg_num][0][m_points.GetSize() - 1];
	
	std::cout << "\n% For " << seg_num << " lines: \n";
	std::cout << "plot(a(:,1),a(:,2),'x');\nhold on;\n";
		
	for (int j = 0; j < m_knots.GetSize(); j++)
	{
		const KNOT& k = m_knots[j];
		const char* c = k.dir == 0 ? "g":(j % 2) ? "b":"r";
			
		std::cout << "x = " << k.seg.p0.x << ':' << k.seg.p1.x << ";\n";
		std::cout << "y = " << k.seg.m << " * x + " << k.seg.b << ";\n";
		std::cout << "plot(x,y,'" << c << "');\n";
		
		std::cout << "% p0.y = " << k.seg.p0.y << " and p1.y = " << k.seg.p1.y << ";\n";
		
		//if (j < m_knots.GetSize() - 1)
		//	std::cout << "disp('Angle: " << CompAcuteAngle(j, j + 1) << "');\n";			
	}
	
	std::cout << "title('Num of segments: " << seg_num
		<< ", error: " << d.GetMinError() << ",  min error: " << m_dMinError
		<< ", min slope: " << m_dMinSlope << ", max segs: " << m_dMaxSegments
		<< "');\n";

	std::cout << "ylabel('Radius');\n";
	std::cout << "xlabel('Shock point index');\n";
				
	std::cout << "hold off;\npause;\n\n";
	std::cout << "% End fitting...\n";
}

void ApproxPoly::SetYMinMax()
{
	double y;
	
	m_ymin = -1;
	m_ymax = -1;
	
	for (int i = 0; i < m_points.GetSize(); i++)
	{
		y = m_points[i].y;
		
		if (m_ymin == -1 || y < m_ymin)
			m_ymin = y;
			
		if (m_ymax == -1 || y > m_ymax)
			m_ymax = y;
	}
}

/*!
	@brief Finds the acute angle between the two line segmets
*/
double ApproxPoly::CompAcuteAngle(int seg1, int seg2) const
{
	ASSERT(seg1 < seg2);
	
	const SEGMENT& s1 = m_knots[seg1].seg;
	const SEGMENT& s2 = m_knots[seg2].seg;
	
	double s = (s2.p1.x - s1.p0.x) / (m_ymax - m_ymin);
	
	std::cout << "disp('scaling: " << m_ymin << "," << m_ymax << "," << s << "');\n";	
	
	//find vectors
	POINT v1(s1.p0.x - s1.p1.x, (s1.p0.y - s1.p1.y) * s);
	POINT v2(s2.p1.x - s2.p0.x, (s2.p1.y - s2.p0.y) * s);
	
	std::cout << "disp('v1: " << v1 << " v2: " << v2 << "');\n";	
	
	// compute angle
	return acos(v1.Dot(v2) / (v1.L2() * v2.L2())) * (180 / PI);
	
	/*const SEGMENT& s1 = m_knots[seg1].seg;
	const SEGMENT& s2 = m_knots[seg2].seg;
		
	double d = (s2.m > s1.m) ? s2.m - s1.m:s1.m - s2.m;
	
	return atan(d / (1 + s1.m * s2.m)) * (180 / PI);*/
}

/*!
	@brief Finds the obtuse angle between the two line segmets
*/
double ApproxPoly::CompObtuseAngle(int seg1, int seg2) const
{
	return 180 - CompAcuteAngle(seg1, seg2);
}

/*
1.517,  0.187
1.200,  2.923
1.353,  7.465
3.444,  9.187
6.017,  9.933
1.464,  14.00
7.320,  16.26
9.478,  17.15
7.472,  22.14
13.48,  23.18
*/
void ApproxPoly::Test()
{
	static POINT testData[] =
	{
		POINT(1.517,  0.187),
		POINT(1.200,  2.923),
		POINT(1.353,  7.465),
		POINT(3.444,  9.187),
		POINT(6.017,  9.933),
		POINT(1.464,  14.00),
		POINT(7.320,  16.26),
		POINT(9.478,  17.15),
		POINT(7.472,  22.14),
		POINT(13.48,  23.18)
	};

	int n = sizeof(testData) / sizeof(*testData);
	
	for (int i = 0; i < n; i++)

		std::cout << std::endl << testData[i];
	
	std::cout << std::endl;

	double e;
	SEGMENT seg;
	e = LeastSquares(testData, n, seg);

	std::cerr << "\nError: " << e << ", slope: " << seg.m << "b: " << seg.b << std::endl;

	std::cerr << "\nShould be: e: 0.68210106, line: y = 4.273150085299648 + 1.5109204502228364x\n";
}

/////////////////////////////////////////////////////////////////////////////////////////////
// ModelFit

double ModelFit::Fit(const POINTS& vertices, const SEGMENTS& segs)
{
	ASSERT(segs.GetSize() >= 1);
	ASSERT(vertices.GetSize() > 1);
	
	int i;		
	m_minLineLen.ReSize(segs.GetSize());
	m_minDataLen.ReSize(vertices.GetSize());
	
	// See of the data can possible be fit with the model given the constraints (m_minLenCoeff)
	m_minDataLen[0] = 0;
	
	for (i = 1; i < m_minDataLen.GetSize(); i++)
		m_minDataLen[i] = m_minDataLen[i - 1] + vertices[i].L2(vertices[i - 1]);
		
	m_minLineLen[0] = segs[0].GetLen() / m_minLenCoeff;
	
	for (i = 1; i < m_minLineLen.GetSize(); i++)
		m_minLineLen[i] = m_minLineLen[i - 1] + (segs[i].GetLen() / m_minLenCoeff);

	m_totalDataLen = m_minDataLen.GetTail();
	m_totalMinLineLen = m_minLineLen.GetTail();
	m_totalMaxLineLen = m_totalMinLineLen * m_minLenCoeff * m_minLenCoeff;
	
	if (m_totalDataLen < m_totalMinLineLen || m_totalDataLen > m_totalMaxLineLen)
	{
		m_points = vertices; // for display purposes. see plot.
		return INFINITY;
	}
		
	// There is enought room, so we have work to do
	int dim = segs.GetSize() + 1; //(int)ceil(segs.GetSize() / 2.0);
	m_points = vertices;
	m_segments = segs;
	m_minerrors.ReSize(dim, dim);	

	MEMDATA d = Min(0, segs.GetSize(), 0, m_points.GetSize() - 1);
	
	// Modify segments
	UpdateSegments(0, segs.GetSize(), 0, m_points.GetSize() - 1);
	
	return d.GetMinError();
}

void ModelFit::UpdateSegments(int ls, int le, int ps, int pe)
{
	if (le - ls == 1)
	{
		m_segments[ls].p0 = m_points[ps];
		m_segments[ls].p1 = m_points[pe];
	}
	else
	{
		const MEMDATA& d = m_minerrors[ls][le][ps][pe];
		
		/*if(d.IsEmpty())
		{
			DBG_VAL(ls)
			DBG_VAL(le)
			DBG_VAL(ps)
			DBG_VAL(pe)
		}*/
		
		ASSERT(!d.IsEmpty());
		
		//int li = (int)ceil(ls + ((le - ls) / 2.0));//(int)floor(le / 2.0);
		int li = int((le + ls) / 2.0);
		
		UpdateSegments(ls, li, ps, d.GetIdx());
		UpdateSegments(li, le, d.GetIdx(), pe);
	}
}

/*!
	Cumulated lengths, without including the last line
*/
MINMAX ModelFit::GetMinMaxLen(int ls, int li, int le, int ps, int pe) const
{
	double minSegLen, d;
	int i, j;
	
	/*if (!(li > ls && li < le))
	{
		DBG_MSG("\nThis should not happen\n")
		DBG_VAL(ls)
		DBG_VAL(li)
		DBG_VAL(le)
	}*/
	ASSERT(li > ls && li < le);
	
	d = (ls == 0) ? 0:m_minLineLen[ls - 1];
	minSegLen = m_minLineLen[li - 1] - d;
	
	for (i = ps + 1; i < pe && (m_minDataLen[i] - m_minDataLen[ps]) < minSegLen; i++);

	if(i == pe)
		i--;
	
	d = (li == 0) ? 0:m_minLineLen[li - 1];	
	minSegLen = m_minLineLen[le - 1] - d;
	
	for (j = pe - 1; j > ps && j > i && (m_minDataLen[pe] - m_minDataLen[j]) < minSegLen; j--);
		
	ASSERT(i <= j);
	/*if (i > j)
	{
		DBG_VAL(ps)
		DBG_VAL(pe)
		DBG_SHOW(m_totalDataLen)
		DBG_SHOW(m_totalMinLineLen)
		DBG_SHOW(m_totalMaxLineLen)
		DBG_VAL(i)
		DBG_VAL(j)
		DBG_VAL(ls)
		DBG_VAL(li)
		DBG_VAL(le)
	}*/
	
	return MINMAX(i, j);
}

MEMDATA ModelFit::Min(int ls, int le, int ps, int pe)
{
	if (m_minerrors[ls][le].GetSize() == 0)
	{
		int dim = m_points.GetSize(); //(int)ceil(m_points.GetSize() / 2.0);
		m_minerrors[ls][le].ReSize(dim, dim);
	}
	
	MEMDATA& d = m_minerrors[ls][le][ps][pe];
		
	if (d.IsEmpty())
	{
		if(le - ls == 1)
			d.Set(CompLSError(ls, ps, pe), pe);
		else
		{
			double m, minerror = INFINITY;
			int i = pe; //In case it never enters the for loop
			int li = int((le + ls) / 2.0); //(int)floor(le / 2.0);
			
			MINMAX mm = GetMinMaxLen(ls, li, le, ps, pe);
			
			for (int pi = mm.min; pi <= mm.max; pi++)
			{
				m = Min(ls, li, ps, pi) + Min(li, le, pi, pe);
				
				if (m < minerror)
				{
					minerror = m;
					i = pi;
				}
			}
			
			d.Set(minerror, i);
		}
	}
	
	return d;
}

double ModelFit::CompLSError(int ls, int ps, int pe) const
{
	const SEGMENT& s = m_segments[ls];
	double e = 0;
	
	for (int i = ps; i <= pe; i++)
		e += fabs(m_points[i].y - (s.m * m_points[i].x + s.b));
		
	return e;
}

void ModelFit::GetSegmentError(int s, double* error, double* scaleFactor) const
{
	/*const MEMDATA& d = m_minerrors[s][s + 1][m_segments[s].p0][m_segments[s].p1];
	ASSERT(!d.IsEmpty());
		
	*error = d.GetMinError();
	
	double oldLen = (s == 0) ? m_minLineLen[s]:m_minLineLen[s] - m_minLineLen[s - 1];
	*scaleFactor = m_segments[s].GetLen() / (oldLen * m_minLenCoeff);*/
}

void ModelFit::Plot(std::ostream& os, const POINTS& points2, const SEGMENTS& segments2) const
{	
	if (m_points.GetSize() > 0)
	{
		os << "\n% DATA 1...\na = ";
		m_points.Print(os, "\n");
		os << ";\nplot(a(:,1),a(:,2),'xb');\nhold on;\n";
	}

	if (points2.GetSize() > 0)
	{
		os << "\n% DATA 2...\nb = ";
		points2.Print(os, "\n");
		os << ";\nplot(b(:,1),b(:,2),'or');\n";
		
		if (m_points.GetSize() == 0)
			os << "hold on;\n";
	}
	
	int j;
	
	// Original model
	for (j = 0; j < segments2.GetSize(); j++)
	{
		const SEGMENT& seg = segments2[j];
		const char* c = "k";
			
		os << "x = " << seg.p0.x << ':' << seg.p1.x << ";\n";
		os << "y = " << seg.m << " * x + " << seg.b << ";\n";
		os << "plot(x,y,'" << c << "');\n";
	}
		
	// Model with new params
	for (j = 0; j < m_segments.GetSize(); j++)
	{
		const SEGMENT& seg = m_segments[j];
		const char* c = (j % 2) ? "b":"r";
			
		os << "x = " << seg.p0.x << ':' << seg.p1.x << ";\n";
		os << "y = " << seg.m << " * x + " << seg.b << ";\n";
		os << "plot(x,y,'" << c << "', 'linewidth', 2);\n";
	}

	double minerror = INFINITY;
	
	if (m_segments.GetSize() > 0 && m_points.GetSize() > 0)
		minerror = m_minerrors[0][m_segments.GetSize()][0][m_points.GetSize() - 1].GetMinError();
	
	os << "title('Num of segments: " << m_segments.GetSize() << ", min lines len: " << m_totalMinLineLen;
	os << ", data len: " << m_totalDataLen	<< ", error: " << minerror << ",  min len coeff: " << m_minLenCoeff << "');\n";
	os << "hold off;\npause;\n\n";
	os << "% End fitting...\n";
}


