#include <iostream>
#include <cstdlib>
#include <cassert>
#include <vector>

#include <wstp.h>
#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>

#define DEQUE_SIZE 7
#define MAX_ITERATION 64
#define EPSILON 1e-4

static void error(WSLINK lp)
{
	fprintf(stderr, "WSTP Error: %s.\n",
		WSErrorMessage(lp));
	exit(EXIT_FAILURE);
}

// discard resulting packages until RETURNPKT 
static void discard_pkt(WSLINK lp, bool keep_retpkt = false)
{
	int pkt;
	while ((pkt = WSNextPacket(lp), pkt) && pkt != RETURNPKT)
	{
		WSNewPacket(lp);
		if (WSError(lp))
			error(lp);
	}
	if (!keep_retpkt)
	{
		WSNewPacket(lp);
	}
}

// plot n order bezier curve with n+1 control points in gj
static void show_bezier_curve(WSLINK lp, glm::vec2 gj[], int n = 3)
{
	//printf("gj = {{0.2, -0.1}, {-0.2, 0.4}, {0.2, 0.6}, {0.5, -0.1}};\n");
	printf("gj = {");
	for (int idx = 0; idx < n; idx++)
	{
		printf("{%2.9f, %2.9f}, ", gj[idx].x, gj[idx].y);
	}
	printf("{%2.9f, %2.9f}};\n", gj[n].x, gj[n].y);

	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "Set", 2L);
		WSPutSymbol(lp, "gj");
		WSPutFunction(lp, "List", n + 1);
		for (int idx = 0; idx < n + 1; idx++)
		{
			WSPutReal32List(lp, glm::value_ptr(gj[idx]), 2L);
		}
	//WSPutString(lp, "{{0.2, -0.1}, {-0.2, 0.4}, {0.2, 0.6}, {0.5, -0.1}}");
	WSEndPacket(lp);
	discard_pkt(lp);

	printf("Show[Graphics[{Brown, BezierCurve[gj], Red, Point[gj], Green, Line[gj]}], Axes->True, AxesOrigin -> {0, 0}]\n");
	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "ToExpression", 1L);
		WSPutString(lp, "Show[Graphics[{Brown, BezierCurve[gj], Red, Point[gj], Green, Line[gj]}], Axes->True, AxesOrigin -> {0, 0}]");
	WSEndPacket(lp);
	discard_pkt(lp);
}

// coefficients of distance constraint function for a given cubic bezier
// it is explicit -- just a quintic Bernstein polynomial
static void bezier5_hk(const glm::vec2 gj[], glm::vec2 thk[])
{
	// numerator of coefficients of product of a cubic bezier function
	// and its 1st order derivative, which is a quadratic bezier function
	glm::vec2 g0g0 = gj[0] * gj[0];
	glm::vec2 g0g1 = gj[0] * gj[1];
	glm::vec2 g0g2 = gj[0] * gj[2];
	glm::vec2 g0g3 = gj[0] * gj[3];

	glm::vec2 g1g1 = gj[1] * gj[1];
	glm::vec2 g1g2 = gj[1] * gj[2];
	glm::vec2 g1g3 = gj[1] * gj[3];

	glm::vec2 g2g2 = gj[2] * gj[2];
	glm::vec2 g2g3 = gj[2] * gj[3];

	glm::vec2 g3g3 = gj[3] * gj[3];

	// only numerator p is left since denominator of hk is canceled 
	// by the binomial coefficient of Bernstein polynomial
	glm::vec2 hk_p[6];
	hk_p[0] = -3.f*g0g0 + 3.f*g0g1;
	hk_p[1] = -15.f*g0g1 + 6.f*g0g2 + 9.f*g1g1;
	hk_p[2] = -12.f*g0g2 + 3.f*g0g3 - 18.f*g1g1 + 27.f*g1g2;
	hk_p[3] = -3.f*g0g3 - 27.f*g1g2 + 12.f*g1g3 + 18.f*g2g2;
	hk_p[4] = -6.f*g1g3 - 9.f*g2g2 + 15.f*g2g3;
	hk_p[5] = -3.f*g2g3 + 3.f*g3g3;

	// apply the denominator of hk
	// 1 / {Binomial[5, 0], Binomial[5, 1], Binomial[5, 2], Binomial[5, 3], Binomial[5, 4], Binomial[5, 5]}
	float binomial_coeff_inv[] = { 1.f / 1.f, 1.f / 5.f, 1.f / 10.f, 1.f / 10.f, 1.f / 5.f, 1.f / 1.f };
	for (int idx = 0; idx < 6; idx++)
	{
		thk[idx].y = (hk_p[idx].x + hk_p[idx].y)*binomial_coeff_inv[idx];
	}
}

// t coordinates of control points of an n order explicit bezier function
static void bezier_tk(glm::vec2 thk[], int n)
{
	float offset = 1.f / n;
	thk[0].x = 0.f;
	for (int idx = 1; idx < n; idx++)
	{
		thk[idx].x = idx * offset;
	}
	thk[n].x = 1.f;
}

// distance from the origin to a cubic bezier curve g[t]
static float bezier3_dist(WSLINK lp, const glm::vec2 gj[], float t)
{
	printf("BezierFunction[gj][%2.9f];\n", t);

	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "Set", 2L);
		WSPutSymbol(lp, "gj");
		WSPutFunction(lp, "List", 3 + 1);
		for (int idx = 0; idx < 3 + 1; idx++)
		{
			WSPutReal32List(lp, glm::value_ptr(gj[idx]), 2L);
		}
	WSEndPacket(lp);
	discard_pkt(lp);

	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "ToExpression", 1L);
		WSPutString(lp, "g = BezierFunction[gj]");
	WSEndPacket(lp);
	discard_pkt(lp);

	WSPutFunction(lp, "EvaluatePacket", 1L);
		WSPutFunction(lp, "g", 1L);
		WSPutReal32(lp, t);
	WSEndPacket(lp);
	discard_pkt(lp, true);

	int len;
	if (!WSTestHead(lp, "List", &len) || len != 2)
		error(lp);
	glm::vec2 gt;
	WSGetReal32(lp, &(glm::value_ptr(gt)[0]));
	WSGetReal32(lp, &(glm::value_ptr(gt)[1]));
	printf("{%2.9f, %2.9f}\n", gt.x, gt.y);

	return glm::length(gt);
}

// step forward: pop bottom, push top
static inline int deque_step1(int p) { return int(glm::mod(float(p + 1), float(DEQUE_SIZE))); }
// step backward: push bottom, pop top
static inline int deque_step0(int p) { return int(glm::mod(float(p - 1), float(DEQUE_SIZE))); }
// no. of elements
static inline int deque_size(int bm, int tp) { return int(glm::mod(float(tp - bm), float(DEQUE_SIZE))) + 1; }

// signed area of triangle given p2 wrt. p0p1
static inline float signed_area(const glm::vec2& p0, const glm::vec2& p1, const glm::vec2& p2)
{
	glm::vec2 e01 = p1 - p0;
	glm::vec2 e02 = p2 - p0;
	return e01.x*e02.y - e02.x*e01.y;
}

// plot polyline of n+1 points and its convex hull being recorded in deque
static void show_convex_hull(WSLINK lp, glm::vec2 thk[], int n, int Dk[], int bm, int tp)
{
	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "Set", 2L);
		WSPutSymbol(lp, "thk");
		WSPutFunction(lp, "List", n + 1);
		for (int idx = 0; idx <= n; idx++)
		{
			WSPutReal32List(lp, glm::value_ptr(thk[idx]), 2L);
		}
	WSEndPacket(lp);
	discard_pkt(lp);

	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "Set", 2L);
		WSPutSymbol(lp, "thDk");
		WSPutFunction(lp, "List", deque_size(bm, tp));
		for (; bm != tp; bm = deque_step1(bm))
		{
			WSPutReal32List(lp, glm::value_ptr(thk[Dk[bm]]), 2L);
		}
		WSPutReal32List(lp, glm::value_ptr(thk[Dk[bm]]), 2L);
	WSEndPacket(lp);
	discard_pkt(lp);

	printf("Show[Graphics[{Red, Point[thk], Green, Line[thk], Blue, Dotted, Line[thDk]}], Axes->True, AxesOrigin -> {0, 0}]\n");
	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "ToExpression", 1L);
	WSPutString(lp, "Show[Graphics[{Red, Point[thk], Green, Line[thk], Blue, Dotted, Line[thDk]}], Axes->True, AxesOrigin -> {0, 0}]");
	WSEndPacket(lp);
	discard_pkt(lp);
}

// distance.exe -linklaunch
// http://support.wolfram.com/kb/12487
// http://reference.wolfram.com/language/JLink/tutorial/WritingJavaProgramsThatUseTheWolframLanguage.html
int main(int argc, char **argv)
{
	WSENV ep = (WSENV)0;
	WSLINK lp = (WSLINK)0;

	// initializes the WSTP environment object and passes parameters in p
	ep = WSInitialize((WSParametersPointer)0);
	if (ep == (WSENV)0)
		exit(EXIT_FAILURE);

	int err;
	// opens a WSTP connection, taking parameters from command-line arguments
	lp = WSOpenArgcArgv(ep, argc, argv, &err);
	if (lp == (WSLINK)0)
		exit(EXIT_FAILURE);

	printf("<<JavaGraphics`\n");
	WSPutFunction(lp, "EvaluatePacket", 1L);
	WSPutFunction(lp, "ToExpression", 1L);
	WSPutString(lp, "<<JavaGraphics`");
	WSEndPacket(lp);
	discard_pkt(lp);

	//////////////////////////////////////////////////////////////////////////
	// a cubic bezier curve
	glm::vec2 gj[] = { { 0.2f, -0.1f }, { -0.2f, 0.4f }, { 0.2f, 0.6f }, { 0.5f, -0.1f } };
	show_bezier_curve(lp, gj);
	glm::vec2 thk[6];
	bezier_tk(thk, 5);
	bezier5_hk(gj, thk);

	show_bezier_curve(lp, thk, 5);

	int Dk[DEQUE_SIZE] = { 0L };// deque as circular buffer recording current convex hull 
	// bottom ^^^^ top
	int order = 6L - 1L;// order of current bezier function
	float tl = 0.f, tr = 1.f;
	float dist;// = std::numeric_limits<float>::infinity();

	dist = bezier3_dist(lp, gj, 0.f);
	if (float dist1 = bezier3_dist(lp, gj, 1.f) < dist)
	{
		tl = 1.f;
		dist = dist1;
	}

	for (int n_iter = 0; n_iter < MAX_ITERATION; n_iter++)
	{		
		// find an edge which is not collinear with the first point
		float area_0kk1 = 0.f;
		int k = 1;
		for (; k < order && area_0kk1 == 0.f; k++)
			area_0kk1 = signed_area(thk[0], thk[k], thk[k + 1]);

		if (area_0kk1 == 0.f)
		{
			printf("all control points are collinear\n");
			// there is an intersection with the t-axis
			if (thk[0].y * thk[order].y < 0.f)
			{
				tl = (thk[0].x*thk[order].y - thk[0].y*thk[order].x) / (thk[order].y - thk[0].y);
				dist = bezier3_dist(lp, gj, tl);
			}
			break;
		}

		// initial deque by a triangle as the convex hull
		Dk[0] = k;// bottom
		if (area_0kk1 < 0.f)
		{
			Dk[1] = 0;
			Dk[2] = k - 1;
		} 
		else
		{
			Dk[1] = k - 1;
			Dk[2] = 0;
		}
		Dk[3] = k;// top
		int bm = 0, tp = 3;
		int bm1 = deque_step1(bm), tp1 = deque_step0(tp);
		//show_convex_hull(lp, thk, order, Dk, bm, tp);

		// Melkman convex hull algorithm for simple polyline (e.g. control polygon)
		// 0,...,k,(k+1)th points have been used for the initial hull of triangle
		for (k = k + 1; k <= order; k++)
		{
			// kth point is to the left of both Db+1,D_b and Dt,Dt-1
			if (signed_area(thk[Dk[bm1]], thk[Dk[bm]], thk[k]) >= 0.f && 
				signed_area(thk[Dk[tp]], thk[Dk[tp1]], thk[k]) >= 0.f)
			{
				printf("INFO point %d is interior.\n", k);
				continue;
			} 
			// tangent to the bottom of hull 
			while (signed_area(thk[Dk[bm1]], thk[Dk[bm]], thk[k]) <= 0.f)
			{
				bm = bm1;
				bm1 = deque_step1(bm);
			}
			bm1 = bm;
			bm = deque_step0(bm);
			Dk[bm] = k;
			// tangent to the top of hull 
			while (signed_area(thk[Dk[tp]], thk[Dk[tp1]], thk[k]) <= 0.f)
			{
				tp = tp1;
				tp1 = deque_step0(tp);
			}
			tp1 = tp;
			tp = deque_step1(tp);
			Dk[tp] = k;
		}
		show_convex_hull(lp, thk, order, Dk, bm, tp);
		
		// intersect the resulting hull with the t-axis and find the leftmost intersection
		float t = 1.f;
		for (int idx = bm, idx1; idx != tp; idx = idx1)
		{
			idx1 = deque_step1(idx);
			glm::vec2 thD_idx = thk[Dk[idx]];
			glm::vec2 thD_idx1 = thk[Dk[idx1]];
			if (thD_idx.y * thD_idx1.y <= 0.f)
			{
				t = glm::min(t, 
					(thD_idx.y * thD_idx1.x - thD_idx.x * thD_idx1.y) / (thD_idx.y - thD_idx1.y));
			}
		}

		if (t < 1.f)
		{
			printf("convex hull leftmost intersection t = %2.9f\n", t);
			// subdividing by de Casteljau algorithm at t
			tr = tr * (1.f - t);
			for (int o = order; o >= 1; o--)
			{
				for (int idx = 0; idx < o; idx++)
				{
					thk[idx].y = (1.f - t)*thk[idx].y + t*thk[idx + 1].y;
				}
			}
			show_bezier_curve(lp, thk, 5);
		}
		else
		{
			printf("convex hull has no more intersection\n", t);
			break;
		}

		if (t < EPSILON)
		{
			float tlt = 1.f - tr;
			float distt = bezier3_dist(lp, gj, tlt);
			printf("root launch detected t = %2.9f, d = %2.9f\n", tlt, distt);

			// a nearer root
			if (distt < dist)
			{
				tl = tlt;
				dist = distt;
			}

			// polynomial deflation
			for (int idx = 1; idx <= order; idx++)
			{
				thk[idx - 1].y = float(order) / float(idx) * thk[idx].y;
			}
			order -= 1;
			bezier_tk(thk, order);
			printf("polynomial deflation o = %d\n", order);
			show_bezier_curve(lp, thk, order);
		}

		assert(n_iter < MAX_ITERATION);
	}

	printf("projection of origin onto curve t = %2.9f, d = %2.9f\n", tl, dist);
	
	system("pause");
	WSPutFunction(lp, "Exit", 0);
	WSClose(lp);
	WSDeinitialize(ep);
	
	return 0;
}