using UnityEngine;
using System;
using System.Collections;
using System.Text;
using System.Threading;
using System.Net.Sockets;
using System.Diagnostics;
using System.Collections.Generic;
using VSI;


namespace KINESIQ 
{
	public class LiveMotion {

		LogManager _log;

		LiveMotion singleton;

		// UI TCP
		public TcpClient m_Client;
		public Socket m_Socket;
		public NetworkStream theStream;
		bool socketConnected = false;

		// VitalSystem controller
		//UInt32[] MoveSeqID;
		uint followingErrorBits = 0;
		bool isArmed = false;


		//public Stopwatch _timer,_timer2;
		private System.Threading.Thread _t1;
		private bool _t1Paused = false;
		private Mutex _mutex = new Mutex();
		volatile bool ThreadRunning=false;
		public bool IsMoving = false;

		private bool isControllerOnline = false;

		public float deltatime=0f;
		public float totalTime=0f;

		List<float[]> _motorPositions;
		public float[] _currentMotorPositions = null;
		Int32 _currentListIndex;
		int _allAxis = 7;
		int _maxSpeed = 3000;
		int _acceleration = 1000;

		MicroLibrary.MicroTimer _microTimer;
		int _motionInterval = 100;  // Motion interval in milliseconds
		MicroLibrary.MicroTimer _controllerTicker;

		public LiveMotion() {

			// Microtimer for when motion is playing
			_microTimer = new MicroLibrary.MicroTimer ();
			_microTimer.MicroTimerElapsed +=
				new MicroLibrary.MicroTimer.MicroTimerElapsedEventHandler(SendMotion);
			_microTimer.Interval = _motionInterval * 1000; // Call micro timer every 10000µs (10ms)

			// Microtimer for when motion is idle
			_controllerTicker = new MicroLibrary.MicroTimer ();
			_controllerTicker.MicroTimerElapsed +=
				new MicroLibrary.MicroTimer.MicroTimerElapsedEventHandler(ControllerTick);
			_controllerTicker.Interval = 50000; // Call micro timer every 50000µs (50ms)

			singleton = this;

			try{
				HiCON.vsiAPIOpenConsole();
				ControllerConnect();
			}
			catch(Exception ex)	{
				//print ("***********************");
				do{
					_log.error (ex.Message);
					ex = ex.InnerException;
				}while(ex != null);
			}
		}

		void Update(){


		}

		void OnApplicationQuit() {
			_microTimer.Enabled = false; 
			_controllerTicker.Enabled = false;
		}

		public void StartLive(float startTime, bool startVideo){

			// Create log file
			_log = new LogManager ("liveMotion");
			_log.fine ("Starting motion");
			_motorPositions = DataManager.loadPlayerCSV (Globals.loadedVideoName);
			_currentListIndex = (int)(startTime * 100);

			// Start thread
			IsMoving = true;
			ThreadRunning = true;
			_microTimer.Enabled = true;
			_controllerTicker.Enabled = false;
		}
		
		private void SendMotion(object sender,
		                          MicroLibrary.MicroTimerEventArgs timerEventArgs)
		{
			string positionData = "";

			if (_motorPositions[_currentListIndex] != null) {

				//ControllerTick();

				// Skip this frame if we are too late
				if (timerEventArgs.TimerLateBy > 2000) {
					_currentListIndex += (_motionInterval / 10);
					_log.warning(string.Format(
						"Count = {0:#,0} - Dropping frame because of a lateBy of {1:#,0} µs", timerEventArgs.TimerCount, timerEventArgs.TimerLateBy));
					return;
				}

				_currentMotorPositions = _motorPositions[_currentListIndex];

				double[] axisPositions = {
					Utils.convertAndFormatVelocityPosition(-_currentMotorPositions[1]),
					Utils.convertAndFormatVelocityPosition(-_currentMotorPositions[2]),
					Utils.convertAndFormatVelocityPosition(-_currentMotorPositions[3]),
					Utils.convertAndFormatVelocityPosition(-_currentMotorPositions[4]),
					Utils.convertAndFormatVelocityPosition(-_currentMotorPositions[5]),
					Utils.convertAndFormatVelocityPosition(-_currentMotorPositions[6])
				};

				bool isMoving = false;
				HiCON.vsiStatusIsMoving(-1, ref isMoving);
				if (isMoving) {
					HiCON.vsiCmdCancelMove(-1, true);
				}

				HiCON.ERROR result = HiCON.vsiCmdExecuteLinearMove(HiCON.Axis.ALL,
				                                                   axisPositions, _maxSpeed, _acceleration, HiCON.MoveType.ABSOLUTE);
				if (result != HiCON.ERROR.NONE)
				{
					_log.error ("Sending data to controller: " + result.ToString());
				}

				// Log timed result
				_log.fine(string.Format(
					"Count = {0:#,0}  Timer = {1:#,0} µs, " + 
					"LateBy = {2:#,0} µs, ExecutionTime = {3:#,0} µs",
					timerEventArgs.TimerCount, timerEventArgs.ElapsedMicroseconds,
					timerEventArgs.TimerLateBy, timerEventArgs.CallbackFunctionExecutionTime));

				//_currentListIndex++;
				_currentListIndex += (_motionInterval / 10);
			}
		}
		

		public void EndLive(){

			if (!IsMoving) {
				return;
			}

			HiCON.ERROR result = HiCON.vsiCmdExecuteLinearMove(_allAxis,
			                                                   new double[6] {0,0,0,0,0,0}, _maxSpeed, _acceleration, HiCON.MoveType.ABSOLUTE);
			_microTimer.Enabled = false; 
			_controllerTicker.Enabled = true;

			// End log file
			_log.fine("Stopping motion");
			_log.close ();
			ThreadRunning = false;
			IsMoving = false;
	}

		/*
		 * Connect to VitalSystem controller
		 */
		private void ControllerConnect(){
			

			HiCON.vsiAPIDisconnect();


			try{
				StringBuilder xmlPath = new StringBuilder(@"Assets/VitalSystem/hiconConfig.xml");
				HiCON.vsiAPILoadXMLConfig(xmlPath);
			}
			catch (Exception e)	{
				//_log.error (e.ToString);
			}
			
			StringBuilder serial = new StringBuilder("");//serialTxtBox.Text.Trim();
			
			if (HiCON.vsiAPIConnect("", 10, 2000) != HiCON.ERROR.NONE){}
			else
				OnSuccessfulConnect();
			
			HiCON.vsiCmdArm(HiCON.Axis.X | HiCON.Axis.Y | HiCON.Axis.Z | HiCON.Axis.A| HiCON.Axis.B| HiCON.Axis.C);
			_controllerTicker.Enabled = true;
		}


		void OnSuccessfulConnect()	{
			StringBuilder serial = new StringBuilder(10);
			HiCON.vsiCmdClearAxisPosition (1);
			HiCON.vsiCmdClearAxisPosition (2);
			HiCON.vsiCmdClearAxisPosition (3);
			HiCON.vsiCmdClearAxisPosition (4);
			HiCON.vsiCmdClearAxisPosition (5);
			HiCON.vsiCmdClearAxisPosition (6);
			HiCON.vsiStatusGetSerial(serial);
		}

		/**
		 * Tick happening when motion is idle
		 */
		private void ControllerTick(object sender,
		                            MicroLibrary.MicroTimerEventArgs timerEventArgs){

			HiCON.vsiStatusIsOnline(ref isControllerOnline);
			if (isControllerOnline){
				bool armedState = false;
				HiCON.vsiStatusIsArmed(ref armedState);
				HiCON.vsiStatusGetFollowErrorBits(ref followingErrorBits);
				HiCON.vsiCmdDataExchange();
				
				if (armedState != isArmed){
					if (!armedState){
						StringBuilder error = new StringBuilder(256);
						int length = 0;
						HiCON.ERROR cError = HiCON.vsiAPIGetLastNotification(error, ref length);
						_log.error ("Controller has been un-armed when it should not: " + error.ToString() + " - " + cError.ToString());
						_log.error ("Re-arming controller");
						HiCON.vsiCmdArm(HiCON.Axis.X | HiCON.Axis.Y | HiCON.Axis.Z | HiCON.Axis.A| HiCON.Axis.B| HiCON.Axis.C);
					}
					HiCON.vsiCmdSetDigitalOutput(13, 4, armedState);
				}
				isArmed = armedState;
			}
		}
	}
}