using System;
using UnityEngine;
using System.Collections.Generic;

public class BezierHelper
{
	public static List<Vector2> GetInterpolatedPoints (Vector2[] points, float pointsPerUnit, bool ignoreFirstAndLast)
	{
		Vector2[] p1, p2;
		BezierHelper.GetCurveControlPoints (points, out p1, out p2);
		
		int start = 0, count = points.Length;
		if (ignoreFirstAndLast == true) {
			start = 1;
			count -= 2;
		}
		
		return BezierHelper.GetInterpolatedPointsWithControlPoints (points, p1, p2, pointsPerUnit, start, count);
	}

	public static List<Vector2> GetInterpolatedPointsWithControlPoints (Vector2[] points, float pointsPerUnit)
	{
		if (points.Length % 3 != 1) {
			throw new ArgumentException ("Invalid number of points!");
		}
		var count = (points.Length - 1) / 3;
		
		var knots = new Vector2[count + 1];
		var first = new Vector2[count];
		var second = new Vector2[count];
		for (int i = 0; i < count; i++) {
			knots [i] = points [i * 3];
			first [i] = points [i * 3 + 1];
			second [i] = points [i * 3 + 2];
		}
		knots [count] = points [points.Length - 1];
		
		return BezierHelper.GetInterpolatedPointsWithControlPoints (knots, first, second, pointsPerUnit, 0, count);
	}

	public static List<Vector2> GetInterpolatedPointsWithControlPoints (Vector2[] knots, Vector2[] firstControlPoints, Vector2[] secondControlPoints, float pointsPerUnit, int start, int count)
	{
		var result = new List<Vector2> ();
		result.Add (knots [0]);
		
		int max = start + count;
		for (int i = start; i < max; i++) {
			var len = (knots [i + 1] - knots [i]).magnitude * 1.5f; // This is not how approximation is done, but should work for us
			var pointsCount = (int)(len * pointsPerUnit);
			var delta = 1f / pointsCount;
			
			for (int j = 1; j < pointsCount; j++) {
				var p = BezierHelper.CalculatePoint (knots [i], firstControlPoints [i], secondControlPoints [i], knots [i + 1], j * delta);
				result.Add (p);
			}
		}
		
		return result;
	}

	/// <summary>
	/// Get open-ended Bezier Spline Control Points.
	/// </summary>
	/// <param name="knots">Input Knot Bezier spline points.</param>
	/// <param name="firstControlPoints">Output First Control points
	/// array of knots.Length - 1 length.</param>
	/// <param name="secondControlPoints">Output Second Control points
	/// array of knots.Length - 1 length.</param>
	/// <exception cref="ArgumentNullException"><paramref name="knots"/>
	/// parameter must be not null.</exception>
	/// <exception cref="ArgumentException"><paramref name="knots"/>
	/// array must contain at least two points.</exception>
	public static void GetCurveControlPoints (Vector2[] knots,
	                                          out Vector2[] firstControlPoints, out Vector2[] secondControlPoints)
	{
		if (knots == null)
			throw new ArgumentNullException ("knots");
		int n = knots.Length - 1;
		if (n < 1)
			throw new ArgumentException ("At least two knot points required", "knots");
					
		if (n == 1) { // Special case: Bezier curve should be a straight line.
			firstControlPoints = new Vector2[1];
			// 3P1 = 2P0 + P3
			firstControlPoints [0].x = (2f * knots [0].x + knots [1].y) / 3f;
			firstControlPoints [0].y = (2f * knots [0].y + knots [1].y) / 3f;
				
			secondControlPoints = new Vector2[1];
			// P2 = 2P1 – P0
			secondControlPoints [0].x = 2f * firstControlPoints [0].x - knots [0].x;
			secondControlPoints [0].y = 2f * firstControlPoints [0].y - knots [0].y;
			return;
		}
			
		// Calculate first Bezier control points
		// Right hand side vector
		float[] rhs = new float[n];
			
		// Set right hand side X values
		for (int i = 1; i < n - 1; ++i)
			rhs [i] = 4 * knots [i].x + 2 * knots [i + 1].x;
		rhs [0] = knots [0].x + 2 * knots [1].x;
		rhs [n - 1] = (8 * knots [n - 1].x + knots [n].x) / 2f;
		// Get first control points X-values
		float[] x = GetFirstControlPoints (rhs);
			
		// Set right hand side Y values
		for (int i = 1; i < n - 1; ++i)
			rhs [i] = 4 * knots [i].y + 2 * knots [i + 1].y;
		rhs [0] = knots [0].y + 2 * knots [1].y;
		rhs [n - 1] = (8 * knots [n - 1].y + knots [n].y) / 2f;
		// Get first control points Y-values
		float[] y = GetFirstControlPoints (rhs);
			
		// Fill output arrays.
		firstControlPoints = new Vector2[n];
		secondControlPoints = new Vector2[n];
		for (int i = 0; i < n; ++i) {
			// First control point
			firstControlPoints [i] = new Vector2 (x [i], y [i]);
			// Second control point
			if (i < n - 1)
				secondControlPoints [i] = new Vector2 (2 * knots [i + 1].x - x [i + 1], 2 * knots [i + 1].y - y [i + 1]);
			else
				secondControlPoints [i] = new Vector2 ((knots [n].x + x [n - 1]) / 2, (knots [n].y + y [n - 1]) / 2);
		}
	}

	/// <summary>
	/// Solves a tridiagonal system for one of coordinates (x or y)
	/// of first Bezier control points.
	/// </summary>
	/// <param name="rhs">Right hand side vector.</param>
	/// <returns>Solution vector.</returns>
	private static float[] GetFirstControlPoints (float[] rhs)
	{
		int n = rhs.Length;
		float[] x = new float[n]; // Solution vector.
		float[] tmp = new float[n]; // Temp workspace.
			
		float b = 2f;
		x [0] = rhs [0] / b;
		for (int i = 1; i < n; i++) { // Decomposition and forward substitution.
			tmp [i] = 1 / b;
			b = (i < n - 1 ? 4.0f : 3.5f) - tmp [i];
			x [i] = (rhs [i] - x [i - 1]) / b;
		}
		for (int i = 1; i < n; i++)
			x [n - i - 1] -= tmp [n - i] * x [n - i]; // Backsubstitution.
			
		return x;
	}

	/// <summary>
	/// Calculates the point for a quadratic Brzier curve at time t (param between 0 and 1).
	/// </summary>
	/// <returns>The point.</returns>
	/// <param name="start">The start point</param>
	/// <param name="control">The control point</param>
	/// <param name="end">The end point</param>
	/// <param name="t">Position on the curve (0 - start, 1 - end)</param>
	public static Vector2 CalculatePoint (Vector2 start, Vector2 control, Vector2 end, float t)
	{
		return (((1 - t) * (1 - t)) * start) + (2 * t * (1 - t) * control) + ((t * t) * end);
	}

	/// <summary>
	/// Calculates the point for a quadratic Brzier curve at time t (param between 0 and 1).
	/// </summary>
	/// <returns>The point.</returns>
	/// <param name="start">The start point</param>
	/// <param name="control">The control point</param>
	/// <param name="end">The end point</param>
	/// <param name="t">Position on the curve (0 - start, 1 - end)</param>
	public static Vector3 CalculatePoint (Vector3 start, Vector3 control, Vector3 end, float t)
	{
		return (((1 - t) * (1 - t)) * start) + (2 * t * (1 - t) * control) + ((t * t) * end);
	}

	/// <summary>
	/// Calculates the point for a cubic Brzier curve at time t (param between 0 and 1).
	/// </summary>
	/// <returns>The point.</returns>
	/// <param name="start">The start point</param>
	/// <param name="control1">The first control point</param>
	/// <param name="control2">The second control point</param>
	/// <param name="end">The end point</param>
	/// <param name="t">Position on the curve (0 - start, 1 - end)</param>
	public static Vector2 CalculatePoint (Vector2 start, Vector2 control1, Vector2 control2, Vector2 end, float t)
	{
		var x = (((-start.x + 3 * (control1.x - control2.x) + end.x) * t + (3 * (start.x + control2.x) - 6 * control1.x)) * t + 3 * (control1.x - start.x)) * t + start.x;
		var y = (((-start.y + 3 * (control1.y - control2.y) + end.y) * t + (3 * (start.y + control2.y) - 6 * control1.y)) * t + 3 * (control1.y - start.y)) * t + start.y;
		return new Vector2 (x, y);
	}

	/// <summary>
	/// Calculates the point for a cubic Brzier curve at time t (param between 0 and 1).
	/// </summary>
	/// <returns>The point.</returns>
	/// <param name="start">The start point</param>
	/// <param name="control1">The first control point</param>
	/// <param name="control2">The second control point</param>
	/// <param name="end">The end point</param>
	/// <param name="t">Position on the curve (0 - start, 1 - end)</param>
	public static Vector3 CalculatePoint (Vector3 start, Vector3 control1, Vector3 control2, Vector3 end, float t)
	{
		var x = (((-start.x + 3 * (control1.x - control2.x) + end.x) * t + (3 * (start.x + control2.x) - 6 * control1.x)) * t + 3 * (control1.x - start.x)) * t + start.x;
		var y = (((-start.y + 3 * (control1.y - control2.y) + end.y) * t + (3 * (start.y + control2.y) - 6 * control1.y)) * t + 3 * (control1.y - start.y)) * t + start.y;
		var z = (((-start.z + 3 * (control1.z - control2.z) + end.z) * t + (3 * (start.z + control2.z) - 6 * control1.z)) * t + 3 * (control1.z - start.z)) * t + start.z;
		return new Vector3 (x, y, z);
	}
}


















/// <summary>
/// Closed Bezier Spline Control Points calculation.
/// </summary>
public static class ClosedBezierSpline
{
	/// <summary>
	/// Get Closed Bezier Spline Control Points.
	/// </summary>
	/// <param name="knots">Input Knot Bezier spline points.</param>
	/// <param name="firstControlPoints">
	/// Output First Control points array of the same 
	/// length as the <paramref name="knots"> array.</param>
	/// <param name="secondControlPoints">
	/// Output Second Control points array of the same
	/// length as the <paramref name="knots"> array.</param>
	public static void GetCurveControlPoints (Vector2[] knots, 
	                                          out Vector2[] firstControlPoints, out Vector2[] secondControlPoints)
	{
		int n = knots.Length;
		if (n <= 2) { // There should be at least 3 knots to draw closed curve.
			firstControlPoints = new Vector2[0];
			secondControlPoints = new Vector2[0];
			return;
		}
		
		// Calculate first Bezier control points
		
		// The matrix.
		float[] a = new float[n], b = new float[n], c = new float[n];
		for (int i = 0; i < n; ++i) {
			a [i] = 1;
			b [i] = 4;
			c [i] = 1;
		}
		
		// Right hand side vector for points X coordinates.
		float[] rhs = new float[n];
		for (int i = 0; i < n; ++i) {
			int j = (i == n - 1) ? 0 : i + 1;
			rhs [i] = 4 * knots [i].x + 2 * knots [j].x;
		}
		// Solve the system for X.
		float[] x = Cyclic.Solve (a, b, c, 1, 1, rhs);
		
		// Right hand side vector for points Y coordinates.
		for (int i = 0; i < n; ++i) {
			int j = (i == n - 1) ? 0 : i + 1;
			rhs [i] = 4 * knots [i].y + 2 * knots [j].y;
		}
		// Solve the system for Y.
		float[] y = Cyclic.Solve (a, b, c, 1, 1, rhs);
		
		// Fill output arrays.
		firstControlPoints = new Vector2[n];
		secondControlPoints = new Vector2[n];
		for (int i = 0; i < n; ++i) {
			// First control point.
			firstControlPoints [i] = new Vector2 (x [i], y [i]);
			// Second control point.
			secondControlPoints [i] = new Vector2 (2 * knots [i].x - x [i], 2 * knots [i].y - y [i]);
		}
	}
}


/// <summary>
/// Solves the cyclic set of linear equations.
/// </summary>
/// <remarks>
/// The cyclic set of equations have the form
/// ---------------------------
/// b0 c0  0 · · · · · · ß
///	a1 b1 c1 · · · · · · ·
///  · · · · · · · · · · · 
///  · · · a[n-2] b[n-2] c[n-2]
/// a  · · · · 0  a[n-1] b[n-1]
/// ---------------------------
/// This is a tridiagonal system, except for the matrix elements 
/// a and ß in the corners.
/// </remarks>
public static class Cyclic
{
	/// <summary>
	/// Solves the cyclic set of linear equations. 
	/// </summary>
	/// <remarks>
	/// All vectors have size of n although some elements are not used.
	/// The input is not modified.
	/// </remarks>
	/// <param name="a">Lower diagonal vector of size n; a[0] not used.</param>
	/// <param name="b">Main diagonal vector of size n.</param>
	/// <param name="c">Upper diagonal vector of size n; c[n-1] not used.</param>
	/// <param name="alpha">Bottom-left corner value.</param>
	/// <param name="beta">Top-right corner value.</param>
	/// <param name="rhs">Right hand side vector.</param>
	/// <returns>The solution vector of size n.</returns>
	public static float[] Solve (float[] a, float[] b, 
	                             float[] c, float alpha, float beta, float[] rhs)
	{
		// a, b, c and rhs vectors must have the same size.
		if (a.Length != b.Length || c.Length != b.Length ||
		    rhs.Length != b.Length)
			throw new ArgumentException ("Diagonal and rhs vectors must have the same size.");
		int n = b.Length;
		if (n <= 2)
			throw new ArgumentException ("n too small in Cyclic; must be greater than 2.");
		
		float gamma = -b [0]; // Avoid subtraction error in forming bb[0].
		// Set up the diagonal of the modified tridiagonal system.
		float[] bb = new float[n];
		bb [0] = b [0] - gamma;
		bb [n - 1] = b [n - 1] - alpha * beta / gamma;
		for (int i = 1; i < n - 1; ++i)
			bb [i] = b [i];
		// Solve A · x = rhs.
		float[] solution = Tridiagonal.Solve (a, bb, c, rhs);
		float[] x = new float[n];
		for (int k = 0; k < n; ++k)
			x [k] = solution [k];
		
		// Set up the vector u.
		float[] u = new float[n];
		u [0] = gamma;
		u [n - 1] = alpha;
		for (int i = 1; i < n - 1; ++i)
			u [i] = 0.0f;
		// Solve A · z = u.
		solution = Tridiagonal.Solve (a, bb, c, u);
		float[] z = new float[n];
		for (int k = 0; k < n; ++k)
			z [k] = solution [k];
		
		// Form v · x/(1 + v · z).
		float fact = (x [0] + beta * x [n - 1] / gamma)
		             / (1.0f + z [0] + beta * z [n - 1] / gamma);
		
		// Now get the solution vector x.
		for (int i = 0; i < n; ++i)
			x [i] -= fact * z [i];
		return x;
	}
}

/// <summary>
/// Tridiagonal system solution.
/// </summary>
public static class Tridiagonal
{
	/// <summary>
	/// Solves a tridiagonal system.
	/// </summary>
	/// <remarks>
	/// All vectors have size of n although some elements are not used.
	/// </remarks>
	/// <param name="a">Lower diagonal vector; a[0] not used.</param>
	/// <param name="b">Main diagonal vector.</param>
	/// <param name="c">Upper diagonal vector; c[n-1] not used.</param>
	/// <param name="rhs">Right hand side vector</param>
	/// <returns>system solution vector</returns>
	public static float[] Solve (float[] a, float[] b, float[] c, float[] rhs)
	{
		// a, b, c and rhs vectors must have the same size.
		if (a.Length != b.Length || c.Length != b.Length ||
		    rhs.Length != b.Length)
			throw new ArgumentException ("Diagonal and rhs vectors must have the same size.");
		if (b [0] == 0.0)
			throw new InvalidOperationException ("Singular matrix.");
		// If this happens then you should rewrite your equations as a set of 
		// order N - 1, with u2 trivially eliminated.
		
		ulong n = Convert.ToUInt64 (rhs.Length);
		float[] u = new float[n];
		float[] gam = new float[n]; 	// One vector of workspace, 
		// gam is needed.
		
		float bet = b [0];
		u [0] = rhs [0] / bet;
		for (ulong j = 1; j < n; j++) { // Decomposition and forward substitution.
			gam [j] = c [j - 1] / bet;
			bet = b [j] - a [j] * gam [j];
			if (bet == 0.0)  
				// Algorithm fails.
				throw new InvalidOperationException ("Singular matrix.");
			u [j] = (rhs [j] - a [j] * u [j - 1]) / bet;
		}
		for (ulong j = 1; j < n; j++)
			u [n - j - 1] -= gam [n - j] * u [n - j]; // Backsubstitution.
		
		return u;
	}
}