// Copyright 2013 Howling Moon Software. All rights reserved.
// See http://chipmunk2d.net/legal.php for more information.

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include "chipmunk/chipmunk_private.h"
#include "chipmunk/cpPolyline.h"


static inline int Next(int i, int count){return (i+1)%count;}

//MARK: Polylines

#define DEFAULT_POLYLINE_CAPACITY 16

static int
cpPolylineSizeForCapacity(int capacity)
{
	return sizeof(cpPolyline) + capacity*sizeof(cpVect);
}

static cpPolyline *
cpPolylineMake(int capacity)
{
	capacity = (capacity > DEFAULT_POLYLINE_CAPACITY ? capacity : DEFAULT_POLYLINE_CAPACITY);

	cpPolyline *line = (cpPolyline *)cpcalloc(1, cpPolylineSizeForCapacity(capacity));
	line->count = 0;
	line->capacity = capacity;

	return line;
}

static cpPolyline *
cpPolylineMake2(int capacity, cpVect a, cpVect b)
{
	cpPolyline *line = cpPolylineMake(capacity);
	line->count = 2;
	line->verts[0] = a;
	line->verts[1] = b;

	return line;
}

static cpPolyline *
cpPolylineShrink(cpPolyline *line)
{
	line->capacity = line->count;
	return (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(line->count));
}

void
cpPolylineFree(cpPolyline *line)
{
	cpfree(line);
}

// Grow the allocated memory for a polyline.
static cpPolyline *
cpPolylineGrow(cpPolyline *line, int count)
{
  line->count += count;

  int capacity = line->capacity;
  while(line->count > capacity) capacity *= 2;

  if(line->capacity < capacity){
    line->capacity = capacity;
		line = (cpPolyline*) cprealloc(line, cpPolylineSizeForCapacity(capacity));
  }

	return line;
}

// Push v onto the end of line.
static cpPolyline *
cpPolylinePush(cpPolyline *line, cpVect v)
{
  int count = line->count;
  line = cpPolylineGrow(line, 1);
  line->verts[count] = v;

	return line;
}

// Push v onto the beginning of line.
static cpPolyline *
cpPolylineEnqueue(cpPolyline *line, cpVect v)
{
	// TODO could optimize this to grow in both directions.
	// Probably doesn't matter though.
  int count = line->count;
  line = cpPolylineGrow(line, 1);
  memmove(line->verts + 1, line->verts, count*sizeof(cpVect));
  line->verts[0] = v;

	return line;
}

// Returns true if the polyline starts and ends with the same vertex.
cpBool
cpPolylineIsClosed(cpPolyline *line)
{
	return (line->count > 1 && cpveql(line->verts[0], line->verts[line->count-1]));
}

// Check if a cpPolyline is longer than a certain length
// Takes a range which can wrap around if the polyline is looped.
static cpBool
cpPolylineIsShort(cpVect *points, int count, int start, int end, cpFloat min)
{
  cpFloat length = 0.0f;
	for(int i=start; i!=end; i=Next(i, count)){
		length += cpvdist(points[i], points[Next(i, count)]);
		if(length > min) return cpFalse;
	}

  return cpTrue;
}

//MARK: Polyline Simplification

static inline cpFloat
Sharpness(cpVect a, cpVect b, cpVect c)
{
	// TODO could speed this up by caching the normals instead of calculating each twice.
  return cpvdot(cpvnormalize(cpvsub(a, b)), cpvnormalize(cpvsub(c, b)));
}

// Join similar adjacent line segments together. Works well for hard edged shapes.
// 'tol' is the minimum anglular difference in radians of a vertex.
cpPolyline *
cpPolylineSimplifyVertexes(cpPolyline *line, cpFloat tol)
{
	cpPolyline *reduced = cpPolylineMake2(0, line->verts[0], line->verts[1]);

	cpFloat minSharp = -cpfcos(tol);

	for(int i=2; i<line->count; i++){
		cpVect vert = line->verts[i];
		cpFloat sharp = Sharpness(reduced->verts[reduced->count - 2], reduced->verts[reduced->count - 1], vert);

		if(sharp <= minSharp){
			reduced->verts[reduced->count - 1] = vert;
		} else {
			reduced = cpPolylinePush(reduced, vert);
		}
	}

	if(
		cpPolylineIsClosed(line) &&
		Sharpness(reduced->verts[reduced->count - 2], reduced->verts[0], reduced->verts[1]) < minSharp
	){
		reduced->verts[0] = reduced->verts[reduced->count - 2];
		reduced->count--;
	}

	// TODO shrink
	return reduced;
}

// Recursive function used by cpPolylineSimplifyCurves().
static cpPolyline *
DouglasPeucker(
	cpVect *verts, cpPolyline *reduced,
	int length, int start, int end,
	cpFloat min, cpFloat tol
){
	// Early exit if the points are adjacent
  if((end - start + length)%length < 2) return reduced;

	cpVect a = verts[start];
	cpVect b = verts[end];

	// Check if the length is below the threshold
	if(cpvnear(a, b, min) && cpPolylineIsShort(verts, length, start, end, min)) return reduced;

	// Find the maximal vertex to split and recurse on
	cpFloat max = 0.0;
	int maxi = start;

	cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
	cpFloat d = cpvdot(n, a);

	for(int i=Next(start, length); i!=end; i=Next(i, length)){
		cpFloat dist = fabs(cpvdot(n, verts[i]) - d);

		if(dist > max){
			max = dist;
			maxi = i;
		}
	}

	if(max > tol){
    reduced = DouglasPeucker(verts, reduced, length, start, maxi, min, tol);
		reduced = cpPolylinePush(reduced, verts[maxi]);
    reduced = DouglasPeucker(verts, reduced, length, maxi, end, min, tol);
	}

	return reduced;
}

// Recursively reduce the vertex count on a polyline. Works best for smooth shapes.
// 'tol' is the maximum error for the reduction.
// The reduced polyline will never be farther than this distance from the original polyline.
cpPolyline *
cpPolylineSimplifyCurves(cpPolyline *line, cpFloat tol)
{
	cpPolyline *reduced = cpPolylineMake(line->count);

	cpFloat min = tol/2.0f;

  if(cpPolylineIsClosed(line)){
		int start, end;
    cpLoopIndexes(line->verts, line->count - 1, &start, &end);

		reduced = cpPolylinePush(reduced, line->verts[start]);
		reduced = DouglasPeucker(line->verts, reduced, line->count - 1, start, end, min, tol);
		reduced = cpPolylinePush(reduced, line->verts[end]);
		reduced = DouglasPeucker(line->verts, reduced, line->count - 1, end, start, min, tol);
		reduced = cpPolylinePush(reduced, line->verts[start]);
  } else {
		reduced = cpPolylinePush(reduced, line->verts[0]);
		reduced = DouglasPeucker(line->verts, reduced, line->count, 0, line->count - 1, min, tol);
		reduced = cpPolylinePush(reduced, line->verts[line->count - 1]);
  }

	return cpPolylineShrink(reduced);
}

//MARK: Polyline Sets

cpPolylineSet *
cpPolylineSetAlloc(void)
{
	return (cpPolylineSet *)cpcalloc(1, sizeof(cpPolylineSet));
}

cpPolylineSet *
cpPolylineSetInit(cpPolylineSet *set)
{
	set->count = 0;
	set->capacity = 8;
	set->lines = (cpPolyline**) cpcalloc(set->capacity, sizeof(cpPolyline));

  return set;
}


cpPolylineSet *
cpPolylineSetNew(void)
{
	return cpPolylineSetInit(cpPolylineSetAlloc());
}

void
cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines)
{
	if(freePolylines){
		for(int i=0; i<set->count; i++){
			cpPolylineFree(set->lines[i]);
		}
	}

	cpfree(set->lines);
}


void
cpPolylineSetFree(cpPolylineSet *set, cpBool freePolylines)
{
	if(set){
		cpPolylineSetDestroy(set, freePolylines);
		cpfree(set);
	}
}

// Find the polyline that ends with v.
static int
cpPolylineSetFindEnds(cpPolylineSet *set, cpVect v){
	int count = set->count;
	cpPolyline **lines = set->lines;

  for(int i=0; i<count; i++){
		cpPolyline *line = lines[i];
    if(cpveql(line->verts[line->count - 1], v)) return i;
  }

  return -1;
}

// Find the polyline that starts with v.
static int
cpPolylineSetFindStarts(cpPolylineSet *set, cpVect v){
	int count = set->count;
	cpPolyline **lines = set->lines;

  for(int i=0; i<count; i++){
    if(cpveql(lines[i]->verts[0], v)) return i;
  }

  return -1;
}

// Add a new polyline to a polyline set.
static void
cpPolylineSetPush(cpPolylineSet *set, cpPolyline *line)
{
  // grow set
  set->count++;
  if(set->count > set->capacity){
    set->capacity *= 2;
    set->lines = (cpPolyline**) cprealloc(set->lines, set->capacity*sizeof(cpPolyline));
  }

	set->lines[set->count - 1] = line;
}

// Add a new polyline to a polyline set.
static void
cpPolylineSetAdd(cpPolylineSet *set, cpVect v0, cpVect v1)
{
	cpPolylineSetPush(set, cpPolylineMake2(DEFAULT_POLYLINE_CAPACITY, v0, v1));
}

// Join two cpPolylines in a polyline set together.
static void
cpPolylineSetJoin(cpPolylineSet *set, int before, int after)
{
  cpPolyline *lbefore = set->lines[before];
  cpPolyline *lafter = set->lines[after];

  // append
  int count = lbefore->count;
  lbefore = cpPolylineGrow(lbefore, lafter->count);
  memmove(lbefore->verts + count, lafter->verts, lafter->count*sizeof(cpVect));
	set->lines[before] = lbefore;

  // delete lafter
  set->count--;
	cpPolylineFree(set->lines[after]);
  set->lines[after] = set->lines[set->count];
}

// Add a segment to a polyline set.
// A segment will either start a new polyline, join two others, or add to or loop an existing polyline.
void
cpPolylineSetCollectSegment(cpVect v0, cpVect v1, cpPolylineSet *lines)
{
  int before = cpPolylineSetFindEnds(lines, v0);
  int after = cpPolylineSetFindStarts(lines, v1);

  if(before >= 0 && after >= 0){
    if(before == after){
      // loop by pushing v1 onto before
      lines->lines[before] = cpPolylinePush(lines->lines[before], v1);
    } else {
      // join before and after
      cpPolylineSetJoin(lines, before, after);
    }
  } else if(before >= 0){
    // push v1 onto before
    lines->lines[before] = cpPolylinePush(lines->lines[before], v1);
  } else if(after >= 0){
    // enqueue v0 onto after
    lines->lines[after] = cpPolylineEnqueue(lines->lines[after], v0);
  } else {
    // create new line from v0 and v1
    cpPolylineSetAdd(lines, v0, v1);
  }
}

//MARK: Convex Hull Functions

cpPolyline *
cpPolylineToConvexHull(cpPolyline *line, cpFloat tol)
{
	cpPolyline *hull = cpPolylineMake(line->count + 1);
	hull->count = cpConvexHull(line->count, line->verts, hull->verts, NULL, tol);
	hull = cpPolylinePush(hull, hull->verts[0]);

	return cpPolylineShrink(hull);
}

//MARK: Approximate Concave Decompostition

struct Notch {
	int i;
	cpFloat d;
	cpVect v;
	cpVect n;
};

static cpFloat
FindSteiner(int count, cpVect *verts, struct Notch notch)
{
	cpFloat min = INFINITY;
	cpFloat feature = -1.0;

	for(int i=1; i<count-1; i++){
		int index = (notch.i + i)%count;

		cpVect seg_a = verts[index];
		cpVect seg_b = verts[Next(index, count)];

		cpFloat thing_a = cpvcross(notch.n, cpvsub(seg_a, notch.v));
		cpFloat thing_b = cpvcross(notch.n, cpvsub(seg_b, notch.v));
		if(thing_a*thing_b <= 0.0){
			cpFloat t = thing_a/(thing_a - thing_b);
			cpFloat dist = cpvdot(notch.n, cpvsub(cpvlerp(seg_a, seg_b, t), notch.v));

			if(dist >= 0.0 && dist <= min){
				min = dist;
				feature = index + t;
			}
		}
	}

	return feature;
}

//static cpFloat
//FindSteiner2(cpVect *verts, int count, struct Notch notch)
//{
//	cpVect a = verts[(notch.i + count - 1)%count];
//	cpVect b = verts[(notch.i + 1)%count];
//	cpVect n = cpvnormalize(cpvadd(cpvnormalize(cpvsub(notch.v, a)), cpvnormalize(cpvsub(notch.v, b))));
//
//	cpFloat min = INFINITY;
//	cpFloat feature = -1.0;
//
//	for(int i=1; i<count-1; i++){
//		int index = (notch.i + i)%count;
//
//		cpVect seg_a = verts[index];
//		cpVect seg_b = verts[Next(index, count)];
//
//		cpFloat thing_a = cpvcross(n, cpvsub(seg_a, notch.v));
//		cpFloat thing_b = cpvcross(n, cpvsub(seg_b, notch.v));
//		if(thing_a*thing_b <= 0.0){
//			cpFloat t = thing_a/(thing_a - thing_b);
//			cpFloat dist = cpvdot(n, cpvsub(cpvlerp(seg_a, seg_b, t), notch.v));
//
//			if(dist >= 0.0 && dist <= min){
//				min = dist;
//				feature = index + t;
//			}
//		}
//	}
//
//	cpAssertSoft(feature >= 0.0, "No closest features detected. This is likely due to a self intersecting polygon.");
//	return feature;
//}

//struct Range {cpFloat min, max;};
//static inline struct Range
//clip_range(cpVect delta_a, cpVect delta_b, cpVect clip)
//{
//	cpFloat da = cpvcross(delta_a, clip);
//	cpFloat db = cpvcross(delta_b, clip);
//	cpFloat clamp = da/(da - db);
//	if(da > db){
//		return (struct Range){-INFINITY, clamp};
//	} else if(da < db){
//		return (struct Range){clamp, INFINITY};
//	} else {
//		return (struct Range){-INFINITY, INFINITY};
//	}
//}
//
//static cpFloat
//FindSteiner3(cpVect *verts, int count, struct Notch notch)
//{
//	cpFloat min = INFINITY;
//	cpFloat feature = -1.0;
//
//	cpVect support_a = verts[(notch.i - 1 + count)%count];
//	cpVect support_b = verts[(notch.i + 1)%count];
//
//	cpVect clip_a = cpvlerp(support_a, support_b, 0.1);
//	cpVect clip_b = cpvlerp(support_b, support_b, 0.9);
//
//	for(int i=1; i<count - 1; i++){
//		int index = (notch.i + i)%count;
//		cpVect seg_a = verts[index];
//		cpVect seg_b = verts[Next(index, count)];
//
//		cpVect delta_a = cpvsub(seg_a, notch.v);
//		cpVect delta_b = cpvsub(seg_b, notch.v);
//
//		// Ignore if the segment faces away from the point.
//		if(cpvcross(delta_b, delta_a) > 0.0){
//			struct Range range1 = clip_range(delta_a, delta_b, cpvsub(notch.v, clip_a));
//			struct Range range2 = clip_range(delta_a, delta_b, cpvsub(clip_b, notch.v));
//
//			cpFloat min_t = cpfmax(0.0, cpfmax(range1.min, range2.min));
//			cpFloat max_t = cpfmin(1.0, cpfmin(range1.max, range2.max));
//
//			// Ignore if the segment has been completely clipped away.
//			if(min_t < max_t){
//				cpVect seg_delta = cpvsub(seg_b, seg_a);
//				cpFloat closest_t = cpfclamp(cpvdot(seg_delta, cpvsub(notch.v, seg_a))/cpvlengthsq(seg_delta), min_t, max_t);
//				cpVect closest = cpvlerp(seg_a, seg_b, closest_t);
//
//				cpFloat dist = cpvdistsq(notch.v, closest);
//				if(dist < min){
//					min = dist;
//					feature = index + closest_t;
//				}
//			}
//		}
//	}
//
//	cpAssertWarn(feature >= 0.0, "Internal Error: No closest features detected.");
//	return feature;
//}

//static cpBool
//VertexUnobscured(int count, cpVect *verts, int index, int notch_i)
//{
//	cpVect v = verts[notch_i];
//	cpVect n = cpvnormalize(cpvsub(verts[index], v));
//
//	for(int i=0; i<count; i++){
//		if(i == index || i == Next(i, count) || i == notch_i || i == Next(notch_i, count)) continue;
//
//		cpVect seg_a = verts[i];
//		cpVect seg_b = verts[Next(i, count)];
//
//		cpFloat thing_a = cpvcross(n, cpvsub(seg_a, v));
//		cpFloat thing_b = cpvcross(n, cpvsub(seg_b, v));
//		if(thing_a*thing_b <= 0.0) return cpTrue;
//	}
//
//	return cpFalse;
//}
//
//static cpFloat
//FindSteiner4(int count, cpVect *verts, struct Notch notch, cpFloat *convexity)
//{
//	cpFloat min = INFINITY;
//	cpFloat feature = -1.0;
//
//	for(int i=Next(notch.b, count); i!=notch.a; i=Next(i, count)){
//		cpVect v = verts[i];
//		cpFloat weight = (1.0 + 0.1*convexity[i])/(1.0*cpvdist(notch.v, v));
//
//		if(weight <= min && VertexUnobscured(count, verts, i, notch.i)){
//			min = weight;
//			feature = i;
//		}
//	}
//
//	cpAssertSoft(feature >= 0.0, "No closest features detected. This is likely due to a self intersecting polygon.");
//	return feature;
//}

static struct Notch
DeepestNotch(int count, cpVect *verts, int hullCount, cpVect *hullVerts, int first, cpFloat tol)
{
	struct Notch notch = {};
	int j = Next(first, count);

	for(int i=0; i<hullCount; i++){
		cpVect a = hullVerts[i];
		cpVect b = hullVerts[Next(i, hullCount)];

		// TODO use a cross check instead?
		cpVect n = cpvnormalize(cpvrperp(cpvsub(a, b)));
		cpFloat d = cpvdot(n, a);

		cpVect v = verts[j];
		while(!cpveql(v, b)){
			cpFloat depth = cpvdot(n, v) - d;

			if(depth > notch.d){
				notch.d = depth;
				notch.i = j;
				notch.v = v;
				notch.n = n;
			}

			j = Next(j, count);
			v = verts[j];
		}

		j = Next(j, count);
	}

	return notch;
}

static inline int IMAX(int a, int b){return (a > b ? a : b);}

#define MAX_RECURSION_DEPTH 100

static void
ApproximateConcaveDecomposition(int depth, cpVect *verts, int count, cpFloat tol, cpPolylineSet *set)
{
	int first;
	cpVect *hullVerts = (cpVect*) alloca(count*sizeof(cpVect));
	int hullCount = cpConvexHull(count, verts, hullVerts, &first, 0.0);


	if(hullCount != count){
		struct Notch notch = DeepestNotch(count, verts, hullCount, hullVerts, first, tol);

		if(notch.d > tol && depth < MAX_RECURSION_DEPTH){
			cpFloat steiner_it = FindSteiner(count, verts, notch);

			if(steiner_it >= 0.0){
				int steiner_i = (int)steiner_it;
				cpVect steiner = cpvlerp(verts[steiner_i], verts[Next(steiner_i, count)], steiner_it - steiner_i);

				// Vertex counts NOT including the steiner point.
				int sub1_count = (steiner_i - notch.i + count)%count + 1;
				int sub2_count = count - (steiner_i - notch.i + count)%count;
				cpVect *scratch = (cpVect*) alloca((IMAX(sub1_count, sub2_count) + 1)*sizeof(cpVect));

				for(int i=0; i<sub1_count; i++) scratch[i] = verts[(notch.i + i)%count];
				scratch[sub1_count] = steiner;
				ApproximateConcaveDecomposition(depth+1,scratch, sub1_count + 1, tol, set);

				for(int i=0; i<sub2_count; i++) scratch[i] = verts[(steiner_i + 1 + i)%count];
				scratch[sub2_count] = steiner;
				ApproximateConcaveDecomposition(depth+1,scratch, sub2_count + 1, tol, set);

				return;
			}
		}
	}

	cpPolyline *hull = cpPolylineMake(hullCount + 1);

	memcpy(hull->verts, hullVerts, hullCount*sizeof(cpVect));
	hull->verts[hullCount] = hullVerts[0];
	hull->count = hullCount + 1;

	cpPolylineSetPush(set, hull);
}

cpPolylineSet *
cpPolylineConvexDecomposition_BETA(cpPolyline *line, cpFloat tol)
{
	cpAssertSoft(cpPolylineIsClosed(line), "Cannot decompose an open polygon.");
	cpAssertSoft(cpAreaForPoly(line->count, line->verts, 0.0) >= 0.0, "Winding is backwards. (Are you passing a hole?)");

	cpPolylineSet *set = cpPolylineSetNew();
	ApproximateConcaveDecomposition(0, line->verts, line->count - 1, tol, set);

	return set;
}