using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Media;

namespace OcadMapLibrary {

	public class MapBezierSegment : MapPathSegment {

		public MapBezierSegment() {
		}

		public MapBezierSegment(Point control1, Point control2, Point endPoint) {
			Control1 = control1;
			Control2 = control2;
			EndPoint = endPoint;
		}

		Point control1;
		public Point Control1 {
			get {
				return control1;
			}
			set {
				if (control1 != value) {
					DataChanged();
				}
				control1 = value;
			}
		}

		Point control2;
		public Point Control2 {
			get {
				return control2;
			}
			set {
				if (control2 != value) {
					DataChanged();
				}
				control2 = value;
			}
		}

		protected override void DataChanged() {
			cachedLength = null;
		}

		double? cachedLength;

		Point cachedStartPoint;

		public override double GetLength(Point startPoint) {
			if (cachedLength != null && startPoint == cachedStartPoint) {
				return cachedLength.Value;
			}
			cachedLength = GetPartialLength(startPoint, 1);
			cachedStartPoint = startPoint;
			return cachedLength.Value;
		}

		public override bool HasLength(Point startPoint) {
			double dist = Point.Subtract(EndPoint, startPoint).Length;
			if (dist > 0) {
				return true;
			}
			return false;
		}

		public override double GetAngleAtLength(Point startPoint, double length) {
			double t = GetTAtDistance(startPoint, length);
			return GetAngleAtT(startPoint, t);
		}

		public override double GetAngleAtStart(Point startPoint) {
			Vector dir = Point.Subtract(Control1, startPoint);
			if (dir.Length == 0) {
				return 0;
			}
			return Math.Atan2(dir.Y, dir.X);
		}

		public override double GetAngleAtEnd(Point startPoint) {
			Vector dir = Point.Subtract(EndPoint, Control2);
			if (dir.Length == 0) {
				return 0;
			}
			return Math.Atan2(dir.Y, dir.X);
		}

		public override PathSegment GetPathSegment() {
			BezierSegment segment = new BezierSegment(Control1, Control2, EndPoint, true);
			return segment;
		}

		public override List<MapPathSegment> SplitAtLength(Point startPoint, double length) {
			List<MapPathSegment> result = new List<MapPathSegment>();
			if (length <= 0) {
				return result;
			}
			double totalLength = GetLength(startPoint);
			if (length >= totalLength) {
				result.Add(Clone());
				return result;
			}
			double t = GetTAtDistance(startPoint, length);
			MapBezierSegment segment1;
			MapBezierSegment segment2;
			Vector a0, a1, a2, a3;
			a0 = this.a0(startPoint);
			a1 = this.a1(startPoint);
			a2 = this.a2(startPoint);
			a3 = this.a3(startPoint);
			Vector newA0;
			Vector newA1;
			Vector newA2;
			Vector newA3;
			Vector p0, p1, p2, p3;
			newA0 = Vector.Multiply(a0, 1);
			newA1 = Vector.Multiply(a1, t);
			newA2 = Vector.Multiply(a2, t * t);
			newA3 = Vector.Multiply(a3, t * t * t);
			p0 = GetP0(newA0, newA1, newA2, newA3);
			p1 = GetP1(newA0, newA1, newA2, newA3);
			p2 = GetP2(newA0, newA1, newA2, newA3);
			p3 = GetP3(newA0, newA1, newA2, newA3);
			segment1 = (MapBezierSegment)Clone();
			segment1.Control1 = new Point(p1.X, p1.Y);
			segment1.Control2 = new Point(p2.X, p2.Y);
			segment1.EndPoint = new Point(p3.X, p3.Y);
			newA0 = Vector.Add(Vector.Add(Vector.Add(a0, Vector.Multiply(t, a1)), Vector.Multiply(t * t, a2)), Vector.Multiply(t * t * t, a3));
			newA1 = Vector.Multiply(1 - t, Vector.Add(Vector.Add(a1, Vector.Multiply(2 * t, a2)), Vector.Multiply(3 * t * t, a3)));
			newA2 = Vector.Multiply((1 - t) * (1 - t), Vector.Add(a2, Vector.Multiply(3 * t, a3)));
			newA3 = Vector.Multiply(a3, (1 - t) * (1 - t) * (1 - t));
			p0 = GetP0(newA0, newA1, newA2, newA3);
			p1 = GetP1(newA0, newA1, newA2, newA3);
			p2 = GetP2(newA0, newA1, newA2, newA3);
			p3 = GetP3(newA0, newA1, newA2, newA3);
			segment2 = (MapBezierSegment)Clone();
			segment2.Control1 = new Point(p1.X, p1.Y);
			segment2.Control2 = new Point(p2.X, p2.Y);
			segment2.EndPoint = new Point(p3.X, p3.Y);
			result.Add(segment1);
			result.Add(segment2);
			return result;
		}

		public override List<MapLineSegment> Flatten(Point startPoint) {
			double totalLength = GetLength(startPoint);
			int numIntervals;
			numIntervals = (int)Math.Min(100, Math.Max(20, totalLength / 4));
			List<MapLineSegment> segments = new List<MapLineSegment>();
			double interval = 1.0 / numIntervals;
			Point lastPoint = startPoint;
			Point point;
			for (int i = 1; i < numIntervals; i++) {
				point = GetPointAtT(startPoint, interval * i);
				MapLineSegment segment = new MapLineSegment(point);
				segment.IsLeftStroked = IsLeftStroked;
				segment.IsRightStroked = IsRightStroked;
				segments.Add(segment);
			}
			MapLineSegment endSegment = new MapLineSegment(EndPoint);
			endSegment.IsLeftStroked = IsLeftStroked;
			endSegment.IsRightStroked = IsRightStroked;
			endSegment.IsCornerPoint = IsCornerPoint;
			endSegment.IsDashPoint = IsDashPoint;
			segments.Add(endSegment);
			return segments;
		}

		public override MapPathSegment Clone() {
			MapBezierSegment clone = new MapBezierSegment();
			clone.Control1 = Control1;
			clone.Control2 = Control2;
			PopulateCloneBase(clone);
			return clone;
		}

		public override void Reverse(Point oldStartPoint) {
			EndPoint = oldStartPoint;
			Point temp = Control1;
			Control1 = Control2;
			Control2 = temp;
		}


		//Bezier maths

		Vector PointToVector(Point point) {
			return new Vector(point.X, point.Y);
		}

		private Point GetPointAtT(Point startPoint, double t) {
			if (t <= 0) {
				return startPoint;
			}
			if (t >= 1) {
				return EndPoint;
			}
			Vector vector = Vector.Multiply(Math.Pow(1 - t, 3), PointToVector(startPoint)) + Vector.Multiply(3 * Math.Pow(1 - t, 2) * t, PointToVector(Control1)) + Vector.Multiply(3 * Math.Pow(t, 2) * (1 - t), PointToVector(Control2)) + Vector.Multiply(Math.Pow(t, 3), PointToVector(EndPoint));
			return new Point(vector.X, vector.Y);
		}

		private double GetPartialLength(Point startPoint, double t) {
			Point endPoint = GetPointAtT(startPoint, t);
			int numIntervals = (int)Math.Min(100, Math.Max(10, Point.Subtract(startPoint, endPoint).Length / 10));
			double length = 0;
			double width = t / ((double)numIntervals);
			Point point = startPoint;
			for (int i = 1; i <= numIntervals; i++) {
				Point newPoint = GetPointAtT(startPoint, ((double)i) * width);
				length += Point.Subtract(point, newPoint).Length;
				point = newPoint;
			}
			return length;
		}

		private double GetTAtDistance(Point startPoint, double distance) {
			if (distance <= 0) {
				return 0;
			}
			double totalLength = GetLength(startPoint);
			if (distance >= totalLength) {
				return 1;
			}
			if (totalLength == 0) {
				return 0;
			}
			double t;
			double approxDist;
			t = distance / totalLength;
			approxDist = GetPartialLength(startPoint, t);
			if (approxDist < distance) {
				return GetTAtDistanceRecursive(startPoint, distance, t, 1.0, 15);
			}
			if (approxDist > distance) {
				return GetTAtDistanceRecursive(startPoint, distance, 0, t, 15);
			}
			return t;
		}

		private double GetTAtDistanceRecursive(Point startPoint, double distance, double lowerT, double upperT, int maxRecursions) {
			double midT = (upperT + lowerT) / 2;
			double t = midT;
			if (maxRecursions <= 0) {
				return t;
			}
			double approxDist;
			approxDist = GetPartialLength(startPoint, t);
			if (approxDist < distance) {
				return GetTAtDistanceRecursive(startPoint, distance, midT, upperT, maxRecursions - 1);
			}
			if (approxDist > distance) {
				return GetTAtDistanceRecursive(startPoint, distance, lowerT, midT, maxRecursions - 1);
			}
			return t;
		}

		public override Point? GetPointAtLength(Point startPoint, double length) {
			if (length < 0) {
				return null;
			}
			double totalLength = GetLength(startPoint);
			if (length > totalLength) {
				return null;
			}
			if (length == 0) {
				return startPoint;
			}
			if (length == totalLength) {
				return EndPoint;
			}
			if (totalLength == 0) {
				return startPoint;
			}
			double t = GetTAtDistance(startPoint, length);
			return GetPointAtT(startPoint, t);
		}

		private double GetAngleAtT(Point startPoint, double t) {
			Vector derivative = new Vector(0, 0);
			derivative = Vector.Multiply(-3 * Math.Pow(1 - t, 2), PointToVector(startPoint)) + Vector.Multiply(-6 * (1 - t) * t + 3 * Math.Pow(1 - t, 2), PointToVector(Control1)) + Vector.Multiply(6 * (1 - t) * t - 3 * Math.Pow(t, 2), PointToVector(Control2)) + Vector.Multiply(3 * Math.Pow(t, 2), PointToVector(EndPoint));
			if (derivative.Length == 0) {
				return 0;
			}
			return Math.Atan2(derivative.Y, derivative.X);
		}

		private Vector a0(Point startPoint) {
			return PointToVector(startPoint);
		}
		private Vector a1(Point startPoint) {
			return Vector.Add(Vector.Multiply(-3, PointToVector(startPoint)), Vector.Multiply(3, PointToVector(Control1)));
		}
		private Vector a2(Point startPoint) {
				return Vector.Add(Vector.Add(Vector.Multiply(3, PointToVector(startPoint)), Vector.Multiply(-6, PointToVector(Control1))), Vector.Multiply(3, PointToVector(Control2)));
		}
		private Vector a3(Point startPoint) {
				return Vector.Add(Vector.Add(Vector.Add(Vector.Multiply(-1, PointToVector(startPoint)), Vector.Multiply(3, PointToVector(Control1))), Vector.Multiply(-3, PointToVector(Control2))), PointToVector(EndPoint));
		}

		private Vector GetP3(Vector a0, Vector a1, Vector a2, Vector a3) {
			return Vector.Multiply(1, Vector.Add(Vector.Add(Vector.Add(a0, a1), a2), a3));
		}

		private Vector GetP2(Vector a0, Vector a1, Vector a2, Vector a3) {
			return Vector.Multiply(1.0 / 3.0, Vector.Add(Vector.Add(Vector.Multiply(3, a0), Vector.Multiply(2, a1)), a2));
		}

		private Vector GetP1(Vector a0, Vector a1, Vector a2, Vector a3) {
			return Vector.Multiply(1.0 / 3.0, Vector.Add(Vector.Multiply(3, a0), Vector.Multiply(1, a1)));
		}

		private Vector GetP0(Vector a0, Vector a1, Vector a2, Vector a3) {
			return a0;
		}

	}

}
