//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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[4, 16];
        PictureBox[,] digitalOutputs = new PictureBox[4, 8];

        /*bool controllerOnline = false;
        bool controllerArmed = false;

        uint controllerFollowingErrorBits = 0;
        string controllerMessage = "";

        double[] controllerAxisPositions = new double[HiCONDLL.MAX_AXIS];
        uint[] controllerMoveSeqIDs = new uint[HiCONDLL.MAX_AXIS];
        bool[] controllerAxisHomed = new bool[HiCONDLL.MAX_AXIS];
        bool[] controllerAxisMoving = new bool[HiCONDLL.MAX_AXIS];

        double[] controllerDACVoltage = new double[HiCONDLL.MAX_DAC_CHANNELS];*/

        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;
        }


        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 < 4; 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);
        }


        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";
        }



        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 && motionSequence.Complete)
            {
                MsgDisp.Text = motionSequence.ErrorMessage;
                motionSequence.Cancel();
                motionSequence = null;
            }


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


            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 <= 14; 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(!newDeviceStatus.motors[cbAxis.SelectedIndex].IsMotionActive);

                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  
        }


        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.X | HiCONDLL.AxisMask.Y | HiCONDLL.AxisMask.Z | HiCONDLL.AxisMask.A | HiCONDLL.AxisMask.B | HiCONDLL.AxisMask.C);
        }


        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 < HiCONDLL.MAX_AXIS; axis++)
            {
                foreach (Control control in axisGroup.Controls)
                {
                    if (control.Tag != null && (int)control.Tag == axis)
                    {
                        Label label = (Label)control;
                        if (Math.Abs(Convert.ToSingle(label.Text)) < 0.0001f)
                            goto SKIP;
                    }
                }

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

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

            SKIP:
                continue;
            }
        }


        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();

            motionSequence = new ArcMotionSequence(Convert.ToDouble(txtVelocity.Text), Convert.ToDouble(txtAccel.Text));
            //motionSequence = new LinearMotionSequence(Convert.ToDouble(txtVelocity.Text), Convert.ToDouble(txtAccel.Text));

            motionSequence.ExecuteAsync();
        }


        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();

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



       




    }
}


