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

namespace OPlanner {

	public class CustomPath {

		List<CustomPathSegment> segments;
		bool isClosed;

		public CustomPath() {
			segments = new List<CustomPathSegment>();
			isClosed = false;
		}

		public List<CustomPathSegment> Segments {
			get {
				return segments;
			}
		}

		public bool IsClosed {
			get {
				return isClosed;
			}
			set {
				isClosed = value;
			}
		}

		public Point Start {
			get {
				if (segments.Count == 0) {
					return new Point(0, 0);
				}
				return segments[0].Start;
			}
		}

		public Point End {
			get {
				if (segments.Count == 0) {
					return new Point(0, 0);
				}
				return segments[segments.Count-1].End;
			}
		}

		public double Length {
			get {
				double length = 0;
				for (int i = 0; i < segments.Count; i++) {
					length += segments[i].Length;
				}
				return length;
			}
		}

		public PathGeometry GetGeometry() {
			if (segments.Count == 0) {
				return null;
			}
			PathSegmentCollection pathSegments = new PathSegmentCollection();
			for (int i = 0; i < segments.Count; i++) {
				pathSegments.Add(segments[i].PathSegment);
			}
			PathFigure pathFigure = new PathFigure(Start,pathSegments,IsClosed);
			PathGeometry geometry = new PathGeometry(new PathFigure[]{pathFigure});
			return geometry;
		}

		public bool Load(PathGeometry geometry) {
			Clear();
			if (geometry.Figures.Count == 0) {
				return false;
			}
			PathFigure pathFigure = geometry.Figures[0];
			if (pathFigure.Segments == null) {
				return false;
			}
			if (pathFigure.Segments.Count == 0) {
				return false;
			}
			IsClosed = pathFigure.IsClosed;
			Point lastPoint = pathFigure.StartPoint;
			for (int i = 0; i < pathFigure.Segments.Count; i++) {
				if (pathFigure.Segments[i] is LineSegment) {
					LineSegment segment = (LineSegment)pathFigure.Segments[i];
					CustomLineSegment lineSegment = new CustomLineSegment(lastPoint,segment.Point);
					segments.Add(lineSegment);
					lastPoint = segment.Point;
				} else if (pathFigure.Segments[i] is PolyLineSegment) {
					PolyLineSegment segment = (PolyLineSegment)pathFigure.Segments[i];
					for (int j = 0; j < segment.Points.Count; j++) {
						CustomLineSegment lineSegment = new CustomLineSegment(lastPoint, segment.Points[j]);
						segments.Add(lineSegment);
						lastPoint = segment.Points[j];
					}
				} else if (pathFigure.Segments[i] is QuadraticBezierSegment) {
					QuadraticBezierSegment segment = (QuadraticBezierSegment)pathFigure.Segments[i];
					CustomQuadraticBezierSegment quadraticBezierSegment = new CustomQuadraticBezierSegment(lastPoint,segment.Point1,segment.Point2);
					segments.Add(quadraticBezierSegment);
					lastPoint = segment.Point2;
				}
			}
			return true;
		}

		public void Clear() {
			segments.Clear();
			isClosed = false;
		}

		public void AddLineSegment(Point point) {
			segments.Add(new CustomLineSegment(End,point));
		}

		public void AddQuadraticBezierSegment(Point control, Point end) {
			segments.Add(new CustomQuadraticBezierSegment(End,control,end));
		}

		public bool RemovePoint(CustomPathSegment segment, PointType pointType, Action beforeChangeCallback) {
			if (!Segments.Contains(segment)) {
				return false;
			}
			if (pointType != PointType.Control && segments.Count < 2) {
				return false;
			}
			beforeChangeCallback();
			if (pointType == PointType.Start) {
				int pos = Segments.IndexOf(segment);
				if (pos > 0) {
					Segments[pos - 1].End = segment.End;
				}
				Segments.Remove(segment);
			} else if (pointType == PointType.End) {
				int pos = Segments.IndexOf(segment);
				if (pos < Segments.Count - 1) {
					Segments[pos + 1].Start = segment.Start;
				}
				Segments.Remove(segment);
			} else if (pointType == PointType.Control) {
				int pos = Segments.IndexOf(segment);
				Segments.Remove(segment);
				CustomLineSegment lineSegment = new CustomLineSegment(segment.Start, segment.End);
				Segments.Insert(pos, lineSegment);
			}
			return true;
		}

		public bool InsertPoint(Point point, PointType pointType, double tolerance, Action beforeChangeCallback) {
			if (Segments.Count == 0) {
				return false;
			}
			Point closest = Start;
			double distance = -1;
			int closestNum = 0;
			CustomPathSegment closestSegment = Segments[0];
			for (int i = 0; i < Segments.Count; i++) {
				Point newPoint = Segments[i].GetClosestPoint(point);
				double temp = Point.Subtract(newPoint, point).Length;
				if (distance < 0 || temp < distance) {
					closest = newPoint;
					distance = temp;
					closestNum = i;
					closestSegment = Segments[i];
				}
			}
			if (distance > tolerance) {
				return false;
			}
			if (pointType != PointType.Control) {
				beforeChangeCallback();
				if (closestSegment is CustomQuadraticBezierSegment) {
					double t = ((CustomQuadraticBezierSegment)closestSegment).GetTAtClosestPoint(point);
					CustomPathSegment[] newSegments = ((CustomQuadraticBezierSegment)closestSegment).SplitAtT(t);
					Segments.Remove(closestSegment);
					for (int i = 0; i < newSegments.Length; i++) {
						Segments.Insert(closestNum + i, newSegments[i]);
					}
				} else {
					CustomLineSegment lineSegment = new CustomLineSegment(closest, closestSegment.End);
					Segments.Insert(closestNum + 1, lineSegment);
					closestSegment.End = closest;
				}
			} else if (pointType == PointType.Control) {
				if (closestSegment is CustomQuadraticBezierSegment) {
					return false;
				}
				beforeChangeCallback();
				CustomQuadraticBezierSegment bezierSegment = new CustomQuadraticBezierSegment(closestSegment.Start, closest, closestSegment.End);
				Segments.Remove(closestSegment);
				Segments.Insert(closestNum, bezierSegment);
				closestSegment.End = closest;
			}
			return true;
		}

		public void MovePoint(int segmentNum, PointType pointType, Point newPoint) {
			CustomPathSegment segment = Segments[segmentNum];
			if (pointType == PointType.End) {
				segment.End = newPoint;
				if (segmentNum < Segments.Count - 1) {
					Segments[segmentNum + 1].Start = newPoint;
				}
			} else if (pointType == PointType.Start) {
				segment.Start = newPoint;
				if (segmentNum > 0) {
					Segments[segmentNum - 1].Start = newPoint;
				}
			} else if (pointType == PointType.Control) {
				CustomQuadraticBezierSegment quadratic = segment as CustomQuadraticBezierSegment;
				if (quadratic == null) {
					return;
				}
				quadratic.Control = newPoint;
			}
		}

		public void Offset(Vector vector) {
			for (int i = 0; i < Segments.Count; i++) {
				Segments[i].Offset(vector);
			}
		}

	}

	public abstract class CustomPathSegment {

		Point start;
		Point end;

		public Point Start {
			get {
				return start;
			}
			set {
				start = value;
			}
		}

		public Point End {
			get {
				return end;
			}
			set {
				end = value;
			}
		}

		public abstract double Length {
			get;
		}

		public abstract PathSegment PathSegment {
			get;
		}

		public abstract Point GetClosestPoint(Point point);

		public abstract void Offset(Vector vector);

	}

	public class CustomLineSegment : CustomPathSegment {

		public CustomLineSegment() {
		}

		public CustomLineSegment(Point start, Point end) {
			Start = start;
			End = end;
		}

		public override double Length {
			get { return Point.Subtract(End, Start).Length; }
		}

		public override PathSegment PathSegment {
			get {
				LineSegment segment = new LineSegment(End, true);
				return segment;
			}
		}

		public override Point GetClosestPoint(Point point) {
			Vector lineVector = Point.Subtract(End, Start);
			if(lineVector.Length==0){
				return Start;
			}
			Vector perp = new Vector(-lineVector.Y, lineVector.X);
			perp.Normalize();
			Vector vector = Point.Subtract(point, Start);
			double dist = Vector.Multiply(vector, perp);
			Vector scaledPerp = Vector.Multiply(-dist, perp);
			Point newPoint = new Point(point.X + scaledPerp.X, point.Y + scaledPerp.Y);
			Vector newVector = new Vector(newPoint.X-Start.X, newPoint.Y-Start.Y);
			lineVector.Normalize();
			double lengthDistance = Vector.Multiply(newVector, lineVector);
			if (lengthDistance > Length) {
				return End;
			}
			if (lengthDistance < 0) {
				return Start;
			}
			return newPoint;
		}

		public override void Offset(Vector vector) {
			Start = Point.Add(Start, vector);
			End = Point.Add(End, vector);
		}

	}

	public class CustomQuadraticBezierSegment : CustomPathSegment {

		Point control;

		public CustomQuadraticBezierSegment() {
		}

		public CustomQuadraticBezierSegment(Point start, Point control, Point end) {
			Start = start;
			End = end;
			Control = control;
		}

		public Point Control {
			get {
				return control;
			}
			set {
				control = value;
			}
		}

		public override double Length {
			get {
				return GetPartialLength(1);
			}
		}

		public override PathSegment PathSegment {
			get {
				QuadraticBezierSegment segment = new QuadraticBezierSegment(Control, End, true);
				return segment;
			}
		}

		Vector startVector {
			get {
				return new Vector(Start.X, Start.Y);
			}
		}

		Vector controlVector {
			get {
				return new Vector(Control.X, Control.Y);
			}
		}

		Vector endVector {
			get {
				return new Vector(End.X, End.Y);
			}
		}

		public Point GetPointAtT(double t) {
			if (t <= 0) {
				return Start;
			}
			if (t >= 1) {
				return End;
			}
			Vector vector = Vector.Add(Vector.Add(Vector.Multiply((1.0 - t) * (1.0 - t), startVector), Vector.Multiply(2.0 * (1.0 - t) * t, controlVector)), Vector.Multiply(t * t, endVector));
			return new Point(vector.X, vector.Y);
		}

		public double GetTAtClosestPoint(Point point) {
			int numIntervals = (int)Math.Min(200, Math.Max(20, Point.Subtract(Start, End).Length / 10));
			double width = 1.0 / ((double)numIntervals);
			Point closest = Start;
			double dist = -1;
			double t = 0;
			for (int i = 0; i <= numIntervals; i++) {
				Point newPoint = GetPointAtT(i * width);
				double temp = Point.Subtract(point, newPoint).Length;
				if (dist < 0 || temp < dist) {
					closest = newPoint;
					dist = temp;
					t = i * width;
				}
			}
			return t;
		}

		public override Point GetClosestPoint(Point point) {
			int numIntervals = (int)Math.Min(200, Math.Max(20, Point.Subtract(Start, End).Length / 10));
			double width = 1.0 / ((double)numIntervals);
			Point closest = Start;
			double dist = -1;
			for (int i = 0; i <= numIntervals; i++) {
				Point newPoint = GetPointAtT(i * width);
				double temp = Point.Subtract(point, newPoint).Length;
				if (dist < 0 || temp < dist) {
					closest = newPoint;
					dist = temp;
				}
			}
			return closest;
		}

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

		public CustomPathSegment[] SplitAtT(double t) {
			if (t <= 0 || t >= 1) {
				return new CustomPathSegment[] { new CustomQuadraticBezierSegment(Start, Control, End ) };
			}
			CustomPathSegment[] pathSegments = new CustomPathSegment[2];
			Vector a0, a1, a2;
			a0 = startVector;
			a1 = Vector.Add(Vector.Multiply(-2, startVector), Vector.Multiply(2, controlVector));
			a2 = Vector.Add(Vector.Add(startVector, Vector.Multiply(-2, controlVector)), endVector);
			Vector newA0, newA1, newA2;
			newA0 = a0;
			newA1 = Vector.Multiply(t, a1);
			newA2 = Vector.Multiply(t * t, a2);
			Point p0, p1, p2;
			p0 = GetP0(newA0, newA1, newA2);
			p1 = GetP1(newA0, newA1, newA2);
			p2 = GetP2(newA0, newA1, newA2);
			CustomQuadraticBezierSegment segment;
			segment = new CustomQuadraticBezierSegment(p0, p1, p2);
			pathSegments[0] = segment;
			newA0 = Vector.Add(Vector.Add(a0, Vector.Multiply(t, a1)), Vector.Multiply(a2, t * t));
			newA1 = Vector.Add(Vector.Multiply(1-t,a1), Vector.Multiply(2*t*(1-t), a2));
			newA2 = Vector.Multiply((1-t) * (1-t), a2);
			p0 = GetP0(newA0, newA1, newA2);
			p1 = GetP1(newA0, newA1, newA2);
			p2 = GetP2(newA0, newA1, newA2);
			segment = new CustomQuadraticBezierSegment(p0, p1, p2);
			pathSegments[1] = segment;
			return pathSegments;
		}

		public Point GetP0(Vector a0, Vector a1, Vector a2) {
			Vector vector = a0;
			return new Point(vector.X, vector.Y);
		}

		public Point GetP1(Vector a0, Vector a1, Vector a2) {
			Vector vector = Vector.Add(Vector.Multiply(0.5, a1), a0);
			return new Point(vector.X, vector.Y);
		}

		public Point GetP2(Vector a0, Vector a1, Vector a2) {
			Vector vector = Vector.Add(Vector.Add(a2, a1), a0);
			return new Point(vector.X, vector.Y);
		}

		public override void Offset(Vector vector) {
			Start = Point.Add(Start, vector);
			End = Point.Add(End, vector);
			Control = Point.Add(Control, vector);
		}

	}

	public enum PointType {
		Start, End, Control
	}

}
