//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// HiCON Motion API Sample C# Project
// Copyright Vital Systems Inc. 2014
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;





namespace VSI
{
	public partial class Form1 : Form
	{
		string xmlPath = @"..\..\hiconConfig.xml";

		PictureBox[,] digitalInputs = new PictureBox[2, 16];
		PictureBox[,] digitalOutputs = new PictureBox[2, 8];

		HiCONDLL.DeviceStatus newDeviceStatus = new HiCONDLL.DeviceStatus();

		bool controllerOnline = false;
		bool controllerArmed = false;

		double[] controllerDACVoltage = new double[HiCONDLL.MAX_DAC_CHANNELS];
		string controllerMessage = "";

		MotionSequence motionSequence = null;

		public Form1()
		{
			HiCONDLL.vsiAPIInitialize();
			HiCONDLL.vsiAPIOpenConsole();

			InitializeComponent();
			InitializeIO(digitalInputGroup, digitalInputs);
			InitializeIO(digitalOutputGroup, digitalOutputs);

			cbAxis.SelectedIndex = 0;

			for (int dac = 0; dac < HiCONDLL.MAX_DAC_CHANNELS; dac++)
				cbDACChannel.Items.Add(dac);

			cbDACChannel.SelectedIndex = 0;
			
			txtAccel.Text = "10.00";
			txtPosition.Text = "30.00";
			txtVelocity.Text = "100.00";

			XPos.Tag = 0;
			YPos.Tag = 1;
			ZPos.Tag = 2;
			APos.Tag = 3;
			BPos.Tag = 4;
			CPos.Tag = 5;

			btnARMPID.Enabled = false;
			string str = "12341234";
			HiCONDLL.vsiAPIGetVersion(ref str);
			Text += " - DLL: " + str;
		}


		protected override void OnClosing(CancelEventArgs e)
		{
			HiCONDLL.vsiCmdDisarm();
			HiCONDLL.vsiAPIDisconnect();

			base.OnClosing(e);
		}


		/// <summary>
		/// Organize the I/O LED pictureboxes into a 2D array for easy reference
		/// </summary>
		void InitializeIO(GroupBox groupBox, PictureBox[,] array)
		{
			bool loadOutputs = (array == digitalOutputs) ? true : false;
			int maxPins = (array == digitalOutputs) ? 8 : 16;

			for(ushort port = 0; port < 2; port++)
			{
				Label portLabel = new Label();
				portLabel.Text = "P" + (port + 11).ToString();
				portLabel.Width = 30;
				portLabel.Location = new Point(0, port * 25) + new Size(10, 42);

				portLabel.Parent = groupBox;

				for (ushort pin = 0; pin < maxPins; pin++)
				{
					PictureBox pictureBox = new PictureBox();
					pictureBox.BackColor = Color.LightGray;
					pictureBox.BorderStyle = BorderStyle.Fixed3D;
					pictureBox.Size = new Size(12, 20);
					pictureBox.Location = new Point(pin * 13, port * 25) + new Size(45, 40);
					pictureBox.Parent = groupBox;

					ushort tag = (ushort)(((port + 11) << 8) | pin);
					pictureBox.Tag = tag;
					array[port, pin] = pictureBox;

					if (loadOutputs)
					{
						pictureBox.Click += OutputLED_Clicked;
						pictureBox.Cursor = Cursors.Hand;
					}

					if(port == 0 && pin % 4 == 0)
					{
						Label pinLabel = new Label();
						pinLabel.Text = pin.ToString();
						pinLabel.Size = new Size(20, 18);
						pinLabel.Location = new Point(pin * 13, 0) + new Size(43, 22);
						pinLabel.Parent = groupBox;
					}
				}
			}

			groupBox.AutoSize = true;
			groupBox.Refresh();
		}




		/// <summary>
		/// Toggle the output LED when clicked
		/// </summary>
		private void OutputLED_Clicked(object sender, EventArgs e)
		{
			PictureBox pictureBox = (PictureBox)sender;

			ushort tag = (ushort)pictureBox.Tag;
			int port = tag >> 8;
			int pinNumber = tag & 0xff;

			bool newPinState = pictureBox.BackColor == Color.Red;
			HiCONDLL.vsiCmdSetDigitalOutput(port, pinNumber, newPinState);
		}


		/// <summary>
		/// Toggle the output LED when clicked
		/// </summary>
		private void OutputECATLED_Clicked(object sender, EventArgs e)
		{
			PictureBox pictureBox = (PictureBox)sender;

			int tag = int.Parse(pictureBox.Tag as string) - 1;

			bool newState = pictureBox.BackColor == Color.Red;

			HiCONDLL.vsiCmdSetEtherCATOutput(1, tag, newState);
		}


		void OnSuccessfulConnect()
		{
			string serial = new string(new char[10]);
			HiCONDLL.vsiStatusGetSerial(ref serial);

			serialTxtBox.Text = serial.Trim().ToUpper();
			serialTxtBox.Enabled = false;

			foreach (PictureBox pictureBox in digitalInputs)
				pictureBox.BackColor = Color.Red;
			foreach (PictureBox pictureBox in digitalOutputs)
				pictureBox.BackColor = Color.Red;
			btnConnect.Text = "Disconnect";


			//Fill out ECAT Rx and Tx Data Grids

			HiCONDLL.vsiCmdGetRxPDOs(0, out var rxPDOs);

			rxPDOs = rxPDOs.Where(x => x.ObjectID != 0 && x.IoType == (int)HiCONDLL.ECAT_ObjOutputType.CustomOutput).ToArray();

			rxPDOGrid.AutoGenerateColumns = false;
			rxPDOGrid.DataSource = rxPDOs;


			HiCONDLL.vsiCmdGetTxPDOs(0, out var txPDOs);

			txPDOs = txPDOs.Where(x => x.ObjectID != 0).ToArray();

			txPDOGrid.AutoGenerateColumns = false;
			txPDOGrid.DataSource = txPDOs;

		}



		void EnableMotionControl(bool enable)
		{
			moveButton.Enabled = enable;
			homeButton.Enabled = enable;
			zeroAllButton.Enabled = enable;

			executeTestButton.Enabled = enable;

			cancelMoveButton.Enabled = !enable;
			cancelAllButton.Enabled = !enable;
			clearPositionButton.Enabled = !enable;
		}


		void DisableDeviceControl(bool enable)
		{
			FollowErrorLED.BackColor = Color.LightGray;
			MotionDoneLED.BackColor = Color.LightGray;
			
			picHomeStatus.BackColor = Color.LightGray;

			btnARMPID.BackColor = Color.LightGray;
			btnARMPID.Enabled = false;
			btnARMPID.Text = "ARM";
			
			serialTxtBox.Enabled = true;

			foreach (PictureBox pictureBox in digitalInputs)
				pictureBox.BackColor = Color.LightGray;
			foreach (PictureBox pictureBox in digitalOutputs)
				pictureBox.BackColor = Color.LightGray;
		}

		void DisplayLastError()
		{
			//HiCONDLL.vsiAPIGetLastNotification(ref controllerMessage);
			MsgDisp.Text = controllerMessage.Trim();
		}


		private void OnControlTimerTick(object sender, EventArgs e)
		{
			if (!controllerOnline)
				return;

			//required to exchange data and maintain connection with the device.
			HiCONDLL.vsiCmdDataExchange(ref newDeviceStatus);

			controllerOnline = newDeviceStatus.IsOnline;

		 /*   
			for (int axis = 0; axis < HiCONDLL.MAX_AXIS; axis++)
			{
				controllerAxisMoving[axis] = HiCONDLL.vsiStatusIsMoving(axis);
				controllerAxisHomed[axis] = HiCONDLL.vsiStatusIsHomeFound(axis);
			}

			HiCONDLL.vsiStatusGetFollowErrorBits(ref controllerFollowingErrorBits);
			HiCONDLL.vsiStatusGetAxisPositions(controllerAxisPositions);
		 */
		}


		private void OnGUITimerTick(object sender, EventArgs e)
		{
			if (newDeviceStatus.DriveEnable != controllerArmed)
			{
				if (!newDeviceStatus.DriveEnable)
					HiCONDLL.vsiAPIGetLastNotification(out controllerMessage);
				else
					controllerMessage = "";

				MsgDisp.Text = controllerMessage.Trim();
				controllerArmed = newDeviceStatus.DriveEnable;
			}

			if (motionSequence != null)
			{
				if (motionSequence.Complete)
				{
					//MsgDisp.Text = motionSequence.ErrorMessage;
					motionSequence.Cancel();
					motionSequence = null;
				}
				else
				{
					motionSequence.Execute();
				}
			}


			if (!controllerOnline)
			{
				DisableDeviceControl(false);
				return;
			}

			btnARMPID.BackColor = (newDeviceStatus.DriveEnable) ? Color.Lime : Color.Red;
			btnARMPID.Text = (newDeviceStatus.DriveEnable) ? "DISARM" : "ARM";
			//btnConnect.Text = (newDeviceStatus.IsOnline) ? "Disconnect" : "Connect";

			FollowErrorLED.BackColor = (newDeviceStatus.FollowingErrorActive) ? Color.Red : Color.Lime;

			if (newDeviceStatus.motors != null)
			{
				MotionDoneLED.BackColor = newDeviceStatus.motors[cbAxis.SelectedIndex].IsMotionActive ? Color.Red : Color.Lime;
				picHomeStatus.BackColor = newDeviceStatus.motors[cbAxis.SelectedIndex].IsHomed ? Color.LightGreen : Color.Red;
			}

			bool newPinState = false;
			for (int port = 11; port <= 12; port++)
			{
				for (int pin = 0; pin < 16; pin++) //Read the digital input status
				{
					newPinState = HiCONDLL.vsiStatusGetDigitalInput(port, pin);
					digitalInputs[port - 11, pin].BackColor = newPinState ? Color.Lime : Color.Red;
				}

				for (int pin = 0; pin < 8; pin++) //Read the digital output status
				{
					newPinState = HiCONDLL.vsiStatusGetDigitalOutput(port, pin);
					digitalOutputs[port - 11, pin].BackColor = newPinState ? Color.Lime : Color.Red;
				}
			}
			if (newDeviceStatus.motors != null)
			{
				EnableMotionControl(!(motionSequence != null || HiCONDLL.vsiStatusIsMoving(-1)));

				XPos.Text = string.Format("{0:n4}", newDeviceStatus.motors[0].ActualPosition);
				YPos.Text = string.Format("{0:n4}", newDeviceStatus.motors[1].ActualPosition);
				ZPos.Text = string.Format("{0:n4}", newDeviceStatus.motors[2].ActualPosition);
				APos.Text = string.Format("{0:n4}", newDeviceStatus.motors[3].ActualPosition);
				BPos.Text = string.Format("{0:n4}", newDeviceStatus.motors[4].ActualPosition);
				CPos.Text = string.Format("{0:n4}", newDeviceStatus.motors[5].ActualPosition);
			}

			double[] systemVars = new double[10];
			HiCONDLL.vsiAPIGetSysVars(systemVars);

			textBox1.Text = systemVars[0].ToString(); //FifoVectorLevel milliseconds
			textBox2.Text = systemVars[1].ToString(); //CmdFIFOLevel 
			textBox3.Text = systemVars[2].ToString(); //CmdFIFOSize
			textBox4.Text = systemVars[3].ToString(); //Elapsed millisec between buffer download
			textBox5.Text = systemVars[4].ToString(); //vectors downloaded in one cycle  


			//Just some shorthand for readability
			Color GetColor(bool value) => value ? Color.Green : Color.Red; 
			bool Input(int id, int input) => HiCONDLL.vsiStatusGetEtherCATInput(id, input); 
			bool Output(int id, int output) => HiCONDLL.vsiStatusGetEtherCATOutput(id, output);

			//ECAT Inputs
			pictureBox1.BackColor = GetColor(Input(1, 0));
			pictureBox2.BackColor = GetColor(Input(1, 1));
			pictureBox3.BackColor = GetColor(Input(1, 2));
			pictureBox4.BackColor = GetColor(Input(1, 3));
			pictureBox5.BackColor = GetColor(Input(1, 4));
			pictureBox6.BackColor = GetColor(Input(1, 5));
			pictureBox7.BackColor = GetColor(Input(1, 6));
			pictureBox8.BackColor = GetColor(Input(1, 7));

			//ECAT Outputs
			pictureBox9.BackColor = GetColor(Output(1, 0));
			pictureBox10.BackColor = GetColor(Output(1, 1));
			pictureBox11.BackColor = GetColor(Output(1, 2));
			pictureBox12.BackColor = GetColor(Output(1, 3));
			pictureBox13.BackColor = GetColor(Output(1, 4));
			pictureBox14.BackColor = GetColor(Output(1, 5));
			pictureBox15.BackColor = GetColor(Output(1, 6));
			pictureBox16.BackColor = GetColor(Output(1, 7));


			//Populate PDO tables with data
			//(Note that Input DROs (Tx PDOs) are always available to the API, whereas output DROs (Rx PDOs) must be explicitly mapped as such in ECLink to be available) 
			for (int i = 0; i < rxPDOGrid.RowCount; i++)
			{
				if (rxPDOGrid.Rows[i].DataBoundItem is HiCONDLL.URxPdoObjModel model)
				{
					var value = HiCONDLL.vsiStatusGetEtherCATOutputDRO(model.SoemID, model.ObjectID, model.SubIndex);
					rxPDOGrid[RxValue.Index, i].Value = $"0x{value:X}";
				}
			}

			for (int i = 0; i < txPDOGrid.RowCount; i++)
			{
				if (txPDOGrid.Rows[i].DataBoundItem is HiCONDLL.UTxPdoObjModel model)
				{
					var value = HiCONDLL.vsiStatusGetEtherCATInputDRO(model.SoemID, model.ObjectID, model.SubIndex);
					txPDOGrid[TxValue.Index, i].Value = $"0x{value:X}";
				}
			}

			labelAnalogIn.Text = $"Analog Inputs\n" +
				$"0: {HiCONDLL.vsiCmdGetEtherCATAnalogInput(1, 0)}\n" +
				$"1: {HiCONDLL.vsiCmdGetEtherCATAnalogInput(1, 1)}\n" +
				$"2: {HiCONDLL.vsiCmdGetEtherCATAnalogInput(1, 2)}\n" +
				$"3: {HiCONDLL.vsiCmdGetEtherCATAnalogInput(1, 3)}";

		}
		int x = 0;

		private void btnConnect_Click(object sender, EventArgs e)
		{
			if(controllerOnline)
			{
				HiCONDLL.vsiAPIDisconnect();
				btnConnect.Text = "Connect";
				controllerOnline = false;
				btnARMPID.Enabled = false;
				return;
			}

			HiCONDLL.vsiAPILoadXMLConfig(xmlPath);
			string serial = serialTxtBox.Text.Trim();
			
			if (HiCONDLL.vsiAPIConnect(serial, controlTimer.Interval, 2000, false) != HiCONDLL.ERROR.NONE)
			{
				HiCONDLL.vsiAPIGetLastNotification(out controllerMessage);
				MessageBox.Show("Connection Failed: " + controllerMessage, "HiCON CSharp Demo");
				btnARMPID.Enabled = false;
				return;
			}
			else
				OnSuccessfulConnect();

			controllerOnline = true;
			btnARMPID.Enabled = true;
		}


		private void btnARMPID_Click(object sender, EventArgs e)
		{
			if (newDeviceStatus.DriveEnable)
				HiCONDLL.vsiCmdDisarm();
			else
				HiCONDLL.vsiCmdArm(HiCONDLL.AxisMask.XYZ | HiCONDLL.AxisMask.ABC);
		}


		private void btnMove_Click(object sender, EventArgs e)
		{
			/*double[] positions = new double[HiCONDLL.MAX_AXIS];
			Array.Copy(axisPositions, positions, HiCONDLL.MAX_AXIS);

			positions[0] = 1;
			positions[1] = 5;
			positions[3] = 10;

			HiCONDLL.vsiCmdExecuteLinearMove(11, positions, 100, 10, HiCONDLL.VSI_MOVE_ABSOLUTE);
			*/

			HiCONDLL.MoveType moveFlags = 0;

			if (relativePositionBtn.Checked)
				moveFlags = HiCONDLL.MoveType.RELATIVE;
			else if (absolutePositionBtn.Checked)
				moveFlags = HiCONDLL.MoveType.ABSOLUTE;
			else if (velocityMotionBtn.Checked)
				moveFlags = HiCONDLL.MoveType.VELOCITY;

			uint motionSeqID =  newDeviceStatus.motors[cbAxis.SelectedIndex].MotionSequenceID;

			if (HiCONDLL.vsiCmdExecuteMove(cbAxis.SelectedIndex,
				Convert.ToDouble(txtPosition.Text),
				Convert.ToDouble(txtVelocity.Text),
				Convert.ToDouble(txtAccel.Text),
				moveFlags,
			   ++motionSeqID) != HiCONDLL.ERROR.NONE)
			{
				DisplayLastError();
			}
		}

		private void zeroMoveBtn_Click(object sender, EventArgs e)
		{
			for (int axis = 0; axis < 6; axis++)
			{

				uint motionSeqID = newDeviceStatus.motors[axis].MotionSequenceID;

				if (HiCONDLL.vsiCmdExecuteMove(axis,
					0,
					Convert.ToDouble(txtVelocity.Text),
					Convert.ToDouble(txtAccel.Text),
					HiCONDLL.MoveType.ABSOLUTE,
					++motionSeqID) != HiCONDLL.ERROR.NONE)
				{
					DisplayLastError();
				}

			}
		}


		private void btnCancelMove_Click(object sender, EventArgs e)
		{
			if(motionSequence != null)
				motionSequence.Cancel();

			if (HiCONDLL.vsiCmdCancelMove(cbAxis.SelectedIndex, false) != HiCONDLL.ERROR.NONE)
				DisplayLastError();
		}


		private void cancelAllBtn_Click(object sender, EventArgs e)
		{
			if (motionSequence != null)
				motionSequence.Cancel();

			if (HiCONDLL.vsiCmdCancelMove(-1, false) != HiCONDLL.ERROR.NONE)
				DisplayLastError();
		}

		private void btnClearAxisPosition_Click(object sender, EventArgs e)
		{
			if (HiCONDLL.vsiCmdClearAxisPosition(cbAxis.SelectedIndex) != HiCONDLL.ERROR.NONE)
				DisplayLastError();

			int pos = 0;
			double[,] buffer = new double[5000, HiCONDLL.MAX_AXIS];

			for (int vector = 0; vector < 5000; vector++)
			{
				for (int axis = 0; axis < HiCONDLL.MAX_AXIS; axis++)
				{
					buffer[vector, axis] = pos;
				}

				pos++;
			}

			HiCONDLL.vsiCmdLoadBufferedMotion(buffer, 5000);
		}


		private void btnHome_Click(object sender, EventArgs e)
		{
			HiCONDLL.ERROR error = HiCONDLL.vsiCmdExecuteHoming(cbAxis.SelectedIndex, 0, 30.0, 7.5);
			if (error != HiCONDLL.ERROR.NONE)
				DisplayLastError();
		}


		private void velocityChkBox_CheckedChanged(object sender, EventArgs e)
		{

		}


		private void jogLeftButton_MouseDown(object sender, MouseEventArgs e)
		{
			HiCONDLL.vsiCmdExecuteMove(cbAxis.SelectedIndex,
				-9999,
				Convert.ToDouble(txtVelocity.Text),
				Convert.ToDouble(txtAccel.Text),
				HiCONDLL.MoveType.ABSOLUTE,
				9999);
		}


		private void jogRightButton_MouseDown(object sender, MouseEventArgs e)
		{
			HiCONDLL.vsiCmdExecuteMove(cbAxis.SelectedIndex,
				9999,
				Convert.ToDouble(txtVelocity.Text),
				Convert.ToDouble(txtAccel.Text),
				HiCONDLL.MoveType.ABSOLUTE,
				9999);
		}
		

		private void JogButtonRelease(object sender, MouseEventArgs e)
		{
			uint sequenceID = 0;
			if (HiCONDLL.vsiStatusGetMotionSequenceID(cbAxis.SelectedIndex, ref sequenceID) != HiCONDLL.ERROR.NONE)
				return;

			if (sequenceID != 9999)
				return;

			if (HiCONDLL.vsiCmdCancelMove(cbAxis.SelectedIndex, false) != HiCONDLL.ERROR.NONE)
				DisplayLastError();
		}




		private void OnExecuteMotionSequenceButton_Click(object sender, EventArgs e)
		{
			if (motionSequence != null)
				motionSequence.Cancel();

			var feedrate = Convert.ToDouble(txtVelocity.Text);
			var acceleration = Convert.ToDouble(txtAccel.Text);

			motionSequence = new MotionSequence()
			{
				new LinearMotion(0x07, new double[] { 2, 2, 0}, feedrate, acceleration, HiCONDLL.MoveType.ABSOLUTE),
				new LinearMotion(0x07, new double[] { -2, -2, 0}, feedrate, acceleration, HiCONDLL.MoveType.RELATIVE),
				new ArcMotionRadius(2, new double[] { -2, 2, 0}, 2, feedrate, HiCONDLL.CLOCKWISE),
				new ArcMotionRadius(2, new double[] { 2, 2, 0}, 2, feedrate, HiCONDLL.CLOCKWISE),
				new ArcMotionAngle(1, new double[] { -2, 2, 0}, 180, feedrate, HiCONDLL.CLOCKWISE),
				new ArcMotionAngle(1, new double[] { 2, 2, 0}, 180, feedrate, HiCONDLL.COUNTER_CLOCKWISE),
				new ArcMotionCenter(2, new double[] { 0, 4, 0}, -2, 0, feedrate, HiCONDLL.CLOCKWISE),
				new ArcMotionCenter(2, new double[] { 2, 2, 0}, 0, -2, feedrate, HiCONDLL.COUNTER_CLOCKWISE),
			};

		}


		private void OnDacChannelChanged(object sender, EventArgs e)
		{
			sliderDACVoltage.Value = (int)(controllerDACVoltage[cbDACChannel.SelectedIndex] * 1000);
		}


		private void OnDACVoltageSliderValueChanged(object sender, EventArgs e)
		{
			controllerDACVoltage[cbDACChannel.SelectedIndex] = (double)sliderDACVoltage.Value / 1000.0;
			if (HiCONDLL.vsiCmdSetDACOutput(cbDACChannel.SelectedIndex, controllerDACVoltage[cbDACChannel.SelectedIndex]) != HiCONDLL.ERROR.NONE)
				DisplayLastError();

			//This commented line will allow addressing the analog out channel by SlaveID + local channel. The local channel numbers for each slave start at 0. You cannot set the EC01's onboard DAC using this function.
			//if (HiCONDLL.vsiCmdSetEtherCATDACOutput(1, cbDACChannel.SelectedIndex - 1, controllerDACVoltage[cbDACChannel.SelectedIndex]) != HiCONDLL.ERROR.NONE)
				//DisplayLastError();

			textboxDACVolts.Text = string.Format("{0:n3}V", controllerDACVoltage[cbDACChannel.SelectedIndex]); 
		}

		private void rxPDOGrid_CellFormatting(object sender, DataGridViewCellFormattingEventArgs e)
		{
			if (e.ColumnIndex == RxIndex.Index)
			if(e.Value is int val)
			{
				e.Value = $"0x{val:X}";
			}
		}
	}
}


