using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;

using System.Threading;
using System.Threading.Tasks;

namespace VSI
{
	public enum MotionStatus
	{
		Pending,
		Executing,
		Complete,
		Aborted
	}

	public interface IMotion
	{
		void Execute();
		void Cancel();
		MotionStatus Status { get; }
	}

	public class LinearMotion : IMotion
	{
		int axisMask;
		double feedrate;
		double acceleration;
		double[] positions;
		HiCONDLL.MoveType moveType;

		bool readyToStop = false;

		public LinearMotion(int axisMask, double[] positions, double feedrate, double acceleration, HiCONDLL.MoveType moveType)
		{
			this.axisMask = axisMask;
			this.feedrate = feedrate;
			this.acceleration = acceleration;
			this.moveType = moveType;
			this.positions = positions;
		}

		public MotionStatus Status { get; private set; } = MotionStatus.Pending;

		public void Execute()
		{
			switch (Status)
			{
				case MotionStatus.Pending:
					{

						var result = HiCONDLL.vsiCmdExecuteLinearMove((HiCONDLL.AxisMask)axisMask, positions, feedrate, acceleration, moveType);

						if (result != 0)
						{
							Status = MotionStatus.Aborted;
						}
						else
						{
							Status = MotionStatus.Executing;
							Task.Factory.StartNew(() =>
							{
								Thread.Sleep(500);
								readyToStop = true;
							});
						}

					}
					break;

				case MotionStatus.Executing:
					{
						if (readyToStop && !HiCONDLL.vsiStatusIsMoving(-1))
						{
							Status = MotionStatus.Complete;
						}
					}
					break;

				case MotionStatus.Aborted:
				case MotionStatus.Complete:
					break;

			}
		}

		public void Cancel()
		{
			HiCONDLL.vsiCmdDisarm();
			Status = MotionStatus.Aborted;
		}
	}

	public class ArcMotionRadius : IMotion
	{
		int rotationAxis;
		double radius;
		double feedrate;
		double[] positions;
		bool clockwise;

		public ArcMotionRadius(int rotationAxis, double[] positions, double radius, double feedrate,  bool clockwise)
		{
			this.rotationAxis = rotationAxis;
			this.radius = radius;
			this.feedrate = feedrate;
			this.positions = positions;
			this.clockwise = clockwise;
		}

		public MotionStatus Status { get; private set; } = MotionStatus.Pending;
		bool readyToStop = false;

		public void Execute()
		{
			switch (Status)
			{
				case MotionStatus.Pending:
					{
						var result = HiCONDLL.vsiCmdExecuteArcMoveRadius(rotationAxis, positions, radius, feedrate, clockwise);

						if (result != 0)
						{
							Status = MotionStatus.Aborted;
						}
						else
						{
							Status = MotionStatus.Executing;
							Task.Factory.StartNew(() =>
							{
								Thread.Sleep(500);
								readyToStop = true;
							});
						}

					}
					break;

				case MotionStatus.Executing:
					{
						if (readyToStop && !HiCONDLL.vsiStatusIsMoving(-1))
						{
							Status = MotionStatus.Complete;
						}
					}
					break;

				case MotionStatus.Aborted:
				case MotionStatus.Complete:
					break;

			}
		}

		public void Cancel()
		{
			HiCONDLL.vsiCmdDisarm();
			Status = MotionStatus.Aborted;
		}
	}


	//Start + offsets must be equidistant from both start point and destination
	//IE    offset = (destination - start)/2 + X * inverse(destination - start)/2
	//where X is any number
	//Example: if Start = (0, 0) and Dest = (2, 1), then:
	//Offset = (1, .5) + X * (-.5, 1)
	public class ArcMotionCenter : IMotion
	{
		int rotationAxis;
		double startOffset1;
		double startOffset2;
		double feedrate;
		double[] positions;
		bool clockwise;

		public ArcMotionCenter(int rotationAxis, double[] positions, double startOffset1, double startOffset2, double feedrate, bool clockwise)
		{
			this.rotationAxis = rotationAxis;
			this.startOffset1 = startOffset1;
			this.startOffset2 = startOffset2;
			this.feedrate = feedrate;
			this.clockwise = clockwise;
			this.positions = positions;
		}

		public MotionStatus Status { get; private set; } = MotionStatus.Pending;
		bool readyToStop = false;

		public void Execute()
		{
			switch (Status)
			{
				case MotionStatus.Pending:
					{
						var result = HiCONDLL.vsiCmdExecuteArcMoveCenter(rotationAxis, positions, startOffset1, startOffset2, feedrate, clockwise);

						if (result != 0)
						{
							Status = MotionStatus.Aborted;
						}
						else
						{
							Status = MotionStatus.Executing;
							Task.Factory.StartNew(() =>
							{
								Thread.Sleep(500);
								readyToStop = true;
							});
						}

					}
					break;

				case MotionStatus.Executing:
					{
						if (readyToStop && !HiCONDLL.vsiStatusIsMoving(-1))
						{
							Status = MotionStatus.Complete;
						}
					}
					break;

				case MotionStatus.Aborted:
				case MotionStatus.Complete:
					break;

			}
		}

		public void Cancel()
		{
			HiCONDLL.vsiCmdDisarm();
			Status = MotionStatus.Aborted;
		}
	}



	public class ArcMotionAngle : IMotion
	{
		int rotationAxis;
		double angle;
		double feedrate;
		double[] positions;
		bool clockwise;

		public ArcMotionAngle(int rotationAxis, double[] positions, double angle, double feedrate, bool clockwise)
		{
			this.rotationAxis = rotationAxis;
			this.angle = angle;
			this.feedrate = feedrate;
			this.clockwise = clockwise;
			this.positions = positions;
		}

		public MotionStatus Status { get; private set; } = MotionStatus.Pending;
		bool readyToStop = false;

		public void Execute()
		{
			switch (Status)
			{
				case MotionStatus.Pending:
					{
						var result = HiCONDLL.vsiCmdExecuteArcMoveAngle(1, positions, angle, feedrate, clockwise);

						if (result != 0)
						{
							Status = MotionStatus.Aborted;
						}
						else
						{
							Status = MotionStatus.Executing;
							Task.Factory.StartNew(() =>
							{
								Thread.Sleep(500);
								readyToStop = true;
							});
						}

					}
					break;

				case MotionStatus.Executing:
					{
						if (readyToStop && !HiCONDLL.vsiStatusIsMoving(-1))
						{
							Status = MotionStatus.Complete;
						}
					}
					break;

				case MotionStatus.Aborted:
				case MotionStatus.Complete:
					break;

			}
		}

		public void Cancel()
		{
			HiCONDLL.vsiCmdDisarm();
			Status = MotionStatus.Aborted;
		}
	}



	public class MotionSequence : IEnumerable<IMotion>
	{
		List<IMotion> motions = new List<IMotion>();

		public IEnumerator<IMotion> GetEnumerator()
		{
			return motions.GetEnumerator();
		}

		IEnumerator IEnumerable.GetEnumerator()
		{
			return motions.GetEnumerator();
		}

		public void Add(IMotion motion) 
		{
			motions.Add(motion);
		}

		public MotionStatus Status { get; private set; } = MotionStatus.Pending;

		public bool Complete
		{
			get { return Current >= motions.Count; }
		}


		public int Current { get; private set; } = 0;

		public void Cancel()
		{
			Status = MotionStatus.Aborted;
			Current = motions.Count;
		}



		public void Execute()
		{
			if (!Complete)
			{
				Status = MotionStatus.Executing;
				motions[Current].Execute();

				if (motions[Current].Status == MotionStatus.Complete)
				{
					Current++;
					if (Complete)
						Status = MotionStatus.Complete;
				}
				else if (motions[Current].Status == MotionStatus.Aborted)
				{
					Status = MotionStatus.Aborted;
					Current = motions.Count;
				}

			}
		}

	}

}
