/*
 * Copyright (c) 2015 Tricoire Sebastien 3dsman@free.fr
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 *
 */

#include "OE_curve.h"
#include "OE_utils.h"
#include <iostream>

#include <GL/gl.h>
#include <cstdlib>
#include <math.h>
#include <cstdio>
#include <cstring>
#include <algorithm>

static inline float minf(float a, float b) { return a < b ? a : b; }
static inline float maxf(float a, float b) { return a > b ? a : b; }

unsigned char OE_curve::lineColor[] = {0,160,192,255};
unsigned char OE_curve::controlLineColor[] = {100,60,92,255};
unsigned char OE_curve::controlPointColor[] = {100,60,92,255};
 unsigned char OE_curve::controlEndPointColor[] = {100,60,92,255};

OE_curve::OE_curve()
{
    //next = 0;		// Pointer to next curve, or NULL if last element.
    npts = 0;					// Total number of bezier points.
    closed = false;				// Flag indicating if shapes should be treated as closed.
}

OE_curve::~OE_curve()
{
}

bool OE_curve::getPoint(uint16_t nb, float* x, float* y)
{
    if ((npts)&&(nb<=npts))
    {
        *x = pts[(nb-1)].x;
        *y = pts[(nb-1)].y;
        return true;
	}
	return false;
}

bool OE_curve::getPoint(uint16_t nb, vector_2d* pt)
{
    if ((npts)&&(nb<=npts))
    {
        pt->x = pts[(nb-1)].x;
        pt->y = pts[(nb-1)].y;
        return true;
	}
	return false;
}

bool OE_curve::setPoint(uint16_t nb, float x, float y)
{
    if (nb<=npts)
    {
        pts[(nb-1)] = vector_2d(x,y);
        return true;
	}
	return false;
}

bool OE_curve::addPoint( float x, float y)
{
	pts.push_back(vector_2d(x,y));
	npts++;
	return true;
}

bool OE_curve::addPoint( vector_2d pt)
{
	pts.push_back(pt);
	npts++;
	return true;
}

void OE_curve::lineTo(float x, float y)
{
	float px,py, dx,dy;
	if (npts > 0) {

        getPoint(npts, &px, &py);
		dx = x - px;
		dy = y - py;
		addPoint(px + dx/3.0f, py + dy/3.0f);
		addPoint(x - dx/3.0f, y - dy/3.0f);
		addPoint(x, y);
	}
}

void OE_curve::lineTo(vector_2d pt)
{
	vector_2d p,d;
	if (npts > 0) {

        getPoint(npts, &p);
		d = pt-p;
		
		addPoint(p + d/3.0f);
		addPoint(pt - d/3.0f);
		addPoint(pt);
	}
}

void OE_curve::cubicBezTo(float cpx1, float cpy1, float cpx2, float cpy2, float x, float y)
{
	addPoint(cpx1, cpy1);
	addPoint(cpx2, cpy2);
	addPoint(x, y);
}

#define EPSILON (1e-12)

bool OE_curve::ptInBounds( vector_2d pt, float* bounds)
{
	return pt.x >= bounds[0] && pt.x <= bounds[2] && pt.y >= bounds[1] && pt.y <= bounds[3];
}

double OE_curve::evalBezier(double t, double p0, double p1, double p2, double p3)
{
	double it = 1.0-t;
	return it*it*it*p0 + 3.0*it*it*t*p1 + 3.0*it*t*t*p2 + t*t*t*p3;
}

 std::vector<vector_2d> OE_curve::interPoint(vector_2d pt1, vector_2d pt2, vector_2d pt3, vector_2d pt4, float t)
 {
	std::vector<vector_2d> out;
	
	vector_2d pt12 = (pt2-pt1)*t+pt1;
	vector_2d pt23 = (pt3-pt2)*t+pt2;
	vector_2d pt34 = (pt4-pt3)*t+pt3;
	vector_2d pt123 = (pt23-pt12)*t+pt12;
	vector_2d pt234 = (pt34-pt23)*t+pt23;
	vector_2d pt1234 = (pt234-pt123)*t+pt123;
	
	out.push_back(pt123);
	out.push_back(pt1234);
	out.push_back(pt234);
	
	return out;
 }

void OE_curve::segmentBounds(float* bounds, vector_2d* segment)
{
	int i, j, count;
	double roots[2], a, b, c, b2ac, t, v;
	vector_2d* v0 = &segment[0];
	vector_2d* v1 = &segment[1];
	vector_2d* v2 = &segment[2];
	vector_2d* v3 = &segment[3];

	// Start the bounding box by end points
	bounds[0] = minf(v0->x, v3->x);
	bounds[1] = minf(v0->y, v3->y);
	bounds[2] = maxf(v0->x, v3->x);
	bounds[3] = maxf(v0->y, v3->y);

	// Bezier segment fits inside the convex hull of it's control points.
	// If control points are inside the bounds, we're done.
	if (ptInBounds(*v1, bounds) && ptInBounds(*v2, bounds))
		return;

	// Add bezier segment inflection points in X and Y.
	for (i = 0; i < 2; i++) {
		if (i){
			a = -3.0 * v0->y+ 9.0 * v1->y - 9.0 * v2->y + 3.0 * v3->y;
			b = 6.0 * v0->y - 12.0 * v1->y + 6.0 * v2->y;
			c = 3.0 * v1->y - 3.0 * v0->y;
		}
		else{
			a = -3.0 * v0->x + 9.0 * v1->x - 9.0 * v2->x + 3.0 * v3->x;
			b = 6.0 * v0->x - 12.0 * v1->x + 6.0 * v2->x;
			c = 3.0 * v1->x - 3.0 * v0->x;
		}
		count = 0;
		if (fabs(a) < EPSILON) {
			if (fabs(b) > EPSILON) {
				t = -c / b;
				if (t > EPSILON && t < 1.0-EPSILON)
					roots[count++] = t;
			}
		} else {
			b2ac = b*b - 4.0*c*a;
			if (b2ac > EPSILON) {
				t = (-b + sqrt(b2ac)) / (2.0 * a);
				if (t > EPSILON && t < 1.0-EPSILON)
					roots[count++] = t;
				t = (-b - sqrt(b2ac)) / (2.0 * a);
				if (t > EPSILON && t < 1.0-EPSILON)
					roots[count++] = t;
			}
		}
		for (j = 0; j < count; j++) {
			if (i){
				v = evalBezier(roots[j], v0->y, v1->y, v2->y, v3->y);
			}else{
				v = evalBezier(roots[j], v0->x, v1->x, v2->x, v3->x);
			}
				bounds[0+i] = minf(bounds[0+i], (float)v);
				bounds[2+i] = maxf(bounds[2+i], (float)v);

		}
	}
}

void OE_curve::getBound(float* xMin, float* yMin, float* xMax, float* yMax)
{
	float tmpbounds[4];
	vector_2d* curve;
	// Find bounds
	for (int i = 0; i < npts-1; i += 3) {
		curve = &pts[i];

		segmentBounds(tmpbounds, curve);
		if (i == 0) {
			bounds[0] = tmpbounds[0];
			bounds[1] = tmpbounds[1];
			bounds[2] = tmpbounds[2];
			bounds[3] = tmpbounds[3];
		} else {
			bounds[0] = minf(bounds[0], tmpbounds[0]);
			bounds[1] = minf(bounds[1], tmpbounds[1]);
			bounds[2] = maxf(bounds[2], tmpbounds[2]);
			bounds[3] = maxf(bounds[3], tmpbounds[3]);
		}
	}
	*xMin = bounds[0];
	*yMin = bounds[1];
	*xMax = bounds[2];
	*yMax = bounds[3];
}

std::vector<vector_2d> OE_curve::subCurve( float start, float end, bool rev)
{
	std::vector<vector_2d> out;
	if (((start>end)&&(!closed))||(start==end)||(start<0)||(end<0)||(start>npts/3)||(end>npts/3))
		return out;
		
		
	std::vector<vector_2d> tangeant;
	bool loop =false;
	if (start>end)
	{
		if(start==npts/3) start = 0;
		if(end==0) end = (npts-1)/3;
	}
	if (start>end)
		loop = true;

	float scaleVect = 1;
	float nearPt;
	nearPt = floor(start);
	vector_2d* ps = &pts[nearPt*3];
	vector_2d* ps2;
	tangeant = interPoint(ps[0],ps[1],ps[2],ps[3], start-nearPt);
	out.push_back(tangeant[1]);
	out.push_back(tangeant[2]);
	scaleVect = 1-(start-nearPt);
	//middle of the curve
	while ((nearPt+1<end)||(loop))
	{
		nearPt++;
		ps = &pts[nearPt*3];
		ps2 = ps;
		
		if ((loop)&&(nearPt == npts/3)) 
		{
			ps2 = &pts[0];
			nearPt = 0;
			loop =false;
		}
		out.push_back((ps[-1]-ps2[0])*scaleVect + ps2[0]);
		out.push_back(ps2[0]);
		out.push_back(ps2[1]);
		
		scaleVect = 1;
	}
	out[out.size()-1] = (out[out.size()-1]-out[out.size()-2])*(end-nearPt)+out[out.size()-1];
	ps = &pts[nearPt*3];
	tangeant = interPoint(ps[0],ps[1],ps[2],ps[3], end-nearPt);
	out.push_back(tangeant[0]);
	out.push_back(tangeant[1]);
	
	if (rev) std::reverse(out.begin(),out.end());
	
	return out;
}

bool OE_curve::setNext(OE_curve* next)
{
    this->next = next;
    return true;
}

OE_curve* OE_curve::getNext()
{
    return next;
}


int OE_curve::getNpts(){return npts;}
bool OE_curve::getClosed(){return closed;}
void OE_curve::setClosed(bool closed){this->closed = closed;}

/** \brief draw the curve on screen
 *
 * \param dpi is the discretisation level of the spline used to display
 * \return true if all is ok
 *
 */

bool OE_curve::draw(float dpi)
{

	int i;
	std::vector<vector_2d> disc = discretizeFast(dpi);
	
	//draw curves
	glLineWidth(1.5);
	glBegin(GL_LINE_STRIP);
	glColor4ubv(lineColor);
	glVertex2f(pts[0].x, pts[0].y);
	int discsize = disc.size();
	for (i = 0; i < discsize-1; i++) {
            glVertex2f(disc[i].x, disc[i].y);
	}
	
	if (closed) {
		glVertex2f(pts[0].x, pts[0].y);
	}
	glEnd();
	
	//draw curve points
	if(0)
	{
		glPointSize(3.0f);
		glBegin(GL_POINTS);
		glVertex2f(pts[0].x, pts[0].y);
		for (i = 0; i < discsize-1; i++) {
				glVertex2f(disc[i].x, disc[i].y);
		}
		if (closed) {
			glVertex2f(pts[0].x, pts[0].y);
		}
		glEnd();
	}
	
	// Subcurve lines
	if(1)
	{
		std::vector<vector_2d> subcurve;
		subcurve = subCurve(3.7,1.2,false);
		//subcurve = subCurve(2,6,1,2,false);
		
		glLineWidth(3);
		glPointSize(3.0f);
		glColor4f(0.8,0.1,0.1,1);
		glBegin(GL_LINE_STRIP);
		//glBegin(GL_POINTS);
		int subsize = subcurve.size();
		for (i = 0; i <= subsize-1; i+=1) {
				glVertex2f(subcurve[i].x, subcurve[i].y);
		}
		glEnd();
		
		subcurve = subCurve(1.2,3.7,false);
		//subcurve = subCurve(1,2,2,6,false);
		
		glLineWidth(1.5);
		glPointSize(3.0f);
		glColor4f(0.1,0.8,0.1,1);
		glBegin(GL_LINE_STRIP);
		//glBegin(GL_POINTS);
		subsize = subcurve.size();
		for (i = 0; i <= subsize-1; i+=1) {
				glVertex2f(subcurve[i].x, subcurve[i].y);
		}
		glEnd();
	}


	// Tangeant interpo lines
	if(0)
		{
		glLineWidth(1.5);
		glColor4f(0.1,0.1,0.1,1);
		glBegin(GL_LINES);
		for (i = 0; i < npts-1; i += 3) {
			vector_2d* p = &pts[i];
			std::vector<vector_2d> tangeant;
			tangeant = interPoint(p[0],p[1],p[2],p[3], 0.2f);
			glVertex2f(tangeant[0].x,tangeant[0].y);
			glVertex2f(tangeant[1].x,tangeant[1].y);
			glVertex2f(tangeant[1].x,tangeant[1].y);
			glVertex2f(tangeant[2].x,tangeant[2].y);
		}
		glEnd();
		
		// Tangeant interpo point
		glColor4f(0.1,0.1,0.1,1);
		glPointSize(3.0f);
		glBegin(GL_POINTS);
		for (i = 0; i < npts-1; i += 3) {
			vector_2d* p = &pts[i];
			std::vector<vector_2d> tangeant;
			tangeant = interPoint(p[0],p[1],p[2],p[3], 0.2f);
			glVertex2f(tangeant[1].x,tangeant[1].y);
		}
		glEnd();
		}
		
    //draw controls
    if (controls)
   {

        // regular discretisation points
		if(0)
		{
			std::vector<vector_2d> discReg = discretizeRegular(1);

			glPointSize(3.0f);
			glBegin(GL_POINTS);
			glColor3f(0.8,0.1,0.1);
			discsize = discReg.size();
			for (i = 0; i < discsize-1; i++) {
				glVertex2f(discReg[i].x, discReg[i].y);
			}
			glEnd();
		}

        // Tangeant lines
        glLineWidth(1.5);
        glColor4ubv(controlLineColor);
        glBegin(GL_LINES);
        for (i = 0; i < npts-1; i += 3) {
            vector_2d* p = &pts[i];
            glVertex2f(p[0].x,p[0].y);
            glVertex2f(p[1].x,p[1].y);
            glVertex2f(p[2].x,p[2].y);
            glVertex2f(p[3].x,p[3].y);
        }
        glEnd();
		
        // tangeant end points
        glPointSize(5.0f);
        glColor4ubv(controlEndPointColor);

        glBegin(GL_POINTS);
        glVertex2f(pts[0].x,pts[0].y);

        glColor4ubv(controlPointColor);

        for (i = 0; i < npts-2; i += 1) {
            vector_2d* p = &pts[i];
            glVertex2f(p[3].x,p[3].y);
        }

        glColor4ubv(controlEndPointColor);
        glVertex2f(pts[(npts-1)].x,pts[(npts-1)].y);

        glEnd();

        // Points
        glPointSize(3.0f);
        glColor4ubv(controlPointColor);
        glBegin(GL_POINTS);

        for (i = 0; i < npts-1; i += 1) {
            vector_2d* p = &pts[i];
            glVertex2f(p[1].x,p[1].y);
            glVertex2f(p[2].x,p[2].y);
        }
        glEnd();
		
		// start point & direction
		if(1)
		{
			glColor4f(1.0,0.0,0.0,1.0);
			glPointSize(2.0f);
			glBegin(GL_POINTS);
			glVertex2f(pts[0].x,pts[0].y);
			glEnd();
			
			glLineWidth(2);
			glBegin(GL_LINES);
			vector_2d p  = pts[1] - pts[0];
			p.normalize();
			p = p+pts[0];
			glVertex2f(pts[0].x,pts[0].y);
			glVertex2f(p.x,p.y);
			glEnd();
		}
    }
	
	return true;
}

float OE_curve::distPtSeg(vector_2d pt, vector_2d seg1, vector_2d seg2)
{
	float  d2, t;
	vector_2d pq, d;
	pq = seg2-seg1;
	
	d = pt-seg1;
	
	d2 = pq.x*pq.x + pq.y*pq.y;
	t = pq.x*d.x + pq.y*d.y;
	
	if (d2 > 0) t /= d2;
	if (t < 0) t = 0;
	else if (t > 1) t = 1;
	d = seg1 + t*pq - pt;

	return d.x*d.x + d.y*d.y;
}

std::vector<vector_2d> OE_curve::discretizeCubicBez(vector_2d pt1, vector_2d pt2, vector_2d pt3, vector_2d pt4, float tol, int level)
{
    std::vector<vector_2d> out;
	float d;

	if (level > 12) return out;
	
	vector_2d pt12 = (pt1+pt2)*0.5f;
	vector_2d pt23 = (pt2+pt3)*0.5f;
	vector_2d pt34 = (pt3+pt4)*0.5f;
	vector_2d pt123 = (pt12+pt23)*0.5f;
	vector_2d pt234 = (pt23+pt34)*0.5f;
	vector_2d pt1234 = (pt123+pt234)*0.5f;

	d = distPtSeg(pt1234, pt1, pt4);
	if (d > tol*tol) {
        out = discretizeCubicBez(pt1, pt12, pt123, pt1234, tol, level+1);

        std::vector<vector_2d> tmpdisc = discretizeCubicBez(pt1234, pt234, pt34, pt4, tol, level+1);
        out.insert(out.end(), tmpdisc.begin(), tmpdisc.end());
	} else {
	    out.push_back(pt4);
	}
	return out;
}

std::vector<vector_2d> OE_curve::discretizeFast(float maxDist)
{
	std::vector<vector_2d> out;
	for (int i = 0; i < npts-3; i += 3) {
		std::vector<vector_2d> tmpdisk = discretizeCubicBez(pts[i], pts[i+1], pts[i+2], pts[i+3], maxDist, 0);
		out.insert(out.end(), tmpdisk.begin(), tmpdisk.end());
	}
	return out;
}

std::vector<vector_2d> OE_curve::discretizeRegular(float dist)
{
	float vectlen, tmplen = 0;
	vector_2d vect;
    std::vector<vector_2d> out;
	std::vector<vector_2d> disc, tmpdisc ;

	tmpdisc.push_back(pts[0]);
	for (int i = 0; i < npts-3; i += 3) {
		std::vector<vector_2d> tmpdisk = discretizeCubicBez(pts[i], pts[i+1], pts[i+2], pts[i+3], dist/10.0, 0);

		tmpdisc.insert(tmpdisc.end(), tmpdisk.begin(), tmpdisk.end());
	}

	if (closed)
	{
		tmpdisc.push_back(pts[0]);
	}

	tmplen =0;

	int discsize;
	discsize = tmpdisc.size();
	for (int i = 0; i < discsize-2; i += 1) {
		vect = tmpdisc[i]-tmpdisc[i+1];

		vectlen=vect.len();
		tmplen = tmplen + vectlen;

		while (tmplen >= dist)
			{
			tmplen -= dist;

			out.push_back(vect*(tmplen/vectlen)+tmpdisc[i+1]);
		}
	}
    return out;

}