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;
using System.IO.Ports;

namespace WheelchairControl
{
    /// <summary>
    /// Form with regulators controls.
    /// </summary>
    public partial class FormRegulators : Form
    {
        #region Delegates

        /// <summary>
        /// Delegate of UART send method of parent form.
        /// </summary>
        /// <param name="data">Data array to be sent.</param>
        public delegate void UARTSend_Delegate(uint[] data);

        /// <summary>
        /// Delegate of log button click event handler of parent form.
        /// </summary>
        /// <param name="sender">Sender.</param>
        /// <param name="e">Event arguments.</param>
        public delegate void toolStripLogClickDelegate(object sender, EventArgs e);

        #endregion

        #region Constants

        /// <summary>
        /// Stop command repeats.
        /// </summary>
        private const Byte STOP_REQUEST_REPEATS = 3;

        /// <summary>
        /// Command buffer data length.
        /// </summary>
        private const Byte RX_DATA_LENGTH = FormMain.RX_DATA_LENGTH;

        /// <summary>
        /// Smallest possible angle increment.
        /// </summary>
        private const Byte ANGLE_REGULATOR_INCREMENT = 1;

        /// <summary>
        /// Angle units regulator is capable to handle.
        /// </summary>
        private enum ComboBoxAngleUnit : byte
        {
            /// <summary>
            /// Select degrees.
            /// </summary>
            DEGREES,

            /// <summary>
            /// Select miliradians.
            /// </summary>
            MILIRADIANS,

            ITEMS_COUNT
        };

        #endregion

        #region Private variables

        /// <summary>
        /// Singleton instance.
        /// </summary>
        private static FormRegulators instance = null;

        /// <summary>
        /// Singleton instance access lock.
        /// </summary>
        private static Object singeltonLock = new Object();

        /// <summary>
        /// Parent form serial port.
        /// </summary>
        private SerialPort serialPort_formMain;

        /// <summary>
        /// Parent form log button.
        /// </summary>
        private ToolStripMenuItem formMainLogToolStrip;

        /// <summary>
        /// Parent form UART send method deldegate. 
        /// </summary>
        private UARTSend_Delegate UARTSendMethod;

        /// <summary>
        /// Parent form button log click event handler.
        /// </summary>
        private toolStripLogClickDelegate toolStripLogClickMethod;

        #endregion

        #region Private methods

        /// <summary>
        /// Sends stop sequence repeately to stop motors.
        /// <see cref="STOP_REQUEST_REPEATS"/>
        /// </summary>
        private void SendStopSequence()
        {
            UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];

            for (Byte i = 0; i < STOP_REQUEST_REPEATS; i++)    //Send mutiple times, just for sure
            {
                outputBuffer[0] = 0xC4;   //Stop request for both motors
                outputBuffer[1] = 128;
                outputBuffer[2] = 128;
                UARTSendMethod(outputBuffer);
            }
        }

        /// <summary>
        /// Subroutine to reset odometry data
        /// </summary>
        private void ResetOdometry()
        {
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            outputBuffer[0] = 0xA3;
            outputBuffer[1] = 0x00;
            outputBuffer[2] = 0x00;
            UARTSendMethod(outputBuffer);
        }

        #endregion

        /// <summary>
        /// Singleton constructor.
        /// </summary>
        /// <param name="formMainCOMPort">Com port reference.</param>
        /// <param name="SendMethod">UART send method reference.</param>
        /// <param name="LogClickDelegate">Log button click even handler delegate.</param>
        /// <param name="toolStripLog">Log button reference.</param>
        private FormRegulators(SerialPort formMainCOMPort, UARTSend_Delegate SendMethod, toolStripLogClickDelegate LogClickDelegate, ToolStripMenuItem toolStripLog)
        {
            serialPort_formMain = formMainCOMPort;
            UARTSendMethod = SendMethod;
            toolStripLogClickMethod = LogClickDelegate;
            formMainLogToolStrip = toolStripLog;

            //Register handler
            FormMain.COMStatusChanged += delegate()
            {
                if (this.InvokeRequired)
                {
                    this.Invoke((MethodInvoker)delegate
                    {
                        FormRegulators_Activated(this, null);
                    });
                }
                else
                {
                    FormRegulators_Activated(this, null);
                }
            };

            InitializeComponent();
        }

        /// <summary>
        /// Singleton instance getter.
        /// </summary>
        /// <param name="formMainCOMPort">Com port reference.</param>
        /// <param name="sendMethod">UART send method reference.</param>
        /// <param name="logClickDelegate">Log button click even handler delegate.</param>
        /// <param name="toolStripLog">Log button reference.</param>
        /// <returns>Instance of form.</returns>
        public static FormRegulators GetInstance(SerialPort formMainCOMPort, UARTSend_Delegate sendMethod, toolStripLogClickDelegate logClickDelegate, ToolStripMenuItem toolStripLog)
        {
            //Prevent possible thread race
            lock (singeltonLock)
            {
                if (instance == null || instance.IsDisposed)   //Create new singleton instance
                    instance = new FormRegulators(formMainCOMPort, sendMethod, logClickDelegate, toolStripLog);
            }

            return instance;
        }

        private void buttonSTOP_Click(object sender, EventArgs e)
        {
            SendStopSequence();

            tsslInfo.Text = "STOP signal has been sent.";

            //Stop log
            if (formMainLogToolStrip.Text == "Stop log")
                toolStripLogClickMethod(sender, null);
        }

        private void FormRegulators_Activated(object sender, EventArgs e)
        {
            UInt16 FORM_TO_BUTTONSTOP_HEIGH_RATIO = 61;

            //Com port status info & controls enable
            if (serialPort_formMain.IsOpen)
            {
                tssLabelComPortState.Text = "COM Port Status: OPEN";

                //Enable buttons
                foreach (Control controlItem in this.Controls)
                    controlItem.Enabled = true;
            }
            else
            {
                tssLabelComPortState.Text = "COM Port Status: CLOSED";
                tsslInfo.Text = "Port closed, controls are disabled!";

                //Disable buttons (leave status strip enabled)
                foreach (Control controlItem in this.Controls)
                    if (controlItem.GetType() != typeof(StatusStrip))
                        controlItem.Enabled = false;
            }

            //Resize form
            buttonSTOP.Height = (collapsibleGroupBoxSettings.Top + collapsibleGroupBoxSettings.Height) - buttonSTOP.Top;
            this.Height = buttonSTOP.Height + FORM_TO_BUTTONSTOP_HEIGH_RATIO;
        }

        private void buttonStartWallFollowing_Click(object sender, EventArgs e)
        {
            //Confirm start
            if (checkBoxStartConfirmation.Checked &&
                MessageBox.Show("Start the wall following?", "Confirmation required!", MessageBoxButtons.YesNo,
                MessageBoxIcon.Question, MessageBoxDefaultButton.Button2) != DialogResult.Yes)
            {
                return;
            }

            //Declare variables
            UInt16 tempY;
            Int32 tempV;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Reset odometry
            if (checkBoxResetOdometry.Checked)
                ResetOdometry();

            //Set focus on stop button to allow easier emergency stop
            buttonSTOP.Focus();

            //Convert parameters
            try
            {
                tempV = Convert.ToInt32(textBoxVdes.Text) + 128;
                tempY = Convert.ToUInt16(textBoxY0Des.Text);
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }

            if (tempY < 0 || tempY > 255 || tempV < 0 || tempV > 255) //Address out of range
            {
                MessageBox.Show("Only distances 0 - 255 & speeds -128 - 127 allowed!", "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
            }
            else
            {
                outputBuffer[0] = 0xC6;
                outputBuffer[1] = (Byte)(tempV);
                outputBuffer[2] = (Byte)tempY;
                UARTSendMethod(outputBuffer);
            }

            //Start log if no logging is active
            if (formMainLogToolStrip.Text != "Stop log" && checkBoxEnableLog.Checked)
                toolStripLogClickMethod(sender, null);

            tsslInfo.Text = "Wall following started!";
        }

        private void buttonPositionRegulator_Click(object sender, EventArgs e)
        {
            //Confirm start
            if (checkBoxStartConfirmation.Checked &&
                MessageBox.Show("Start the position regulator?", "Confirmation required!", MessageBoxButtons.YesNo,
                MessageBoxIcon.Question, MessageBoxDefaultButton.Button2) != DialogResult.Yes)
            {
                return;
            }

            //Declare variables
            Int16 tempX;
            Int16 tempY;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Reset odometry
            if (checkBoxResetOdometry.Checked)
                ResetOdometry();

            //Set focus on stop button to allow easier emergency stop
            buttonSTOP.Focus();

            //Convert parameters
            try
            {
                tempX = Convert.ToInt16(textBoxXdes.Text);
                tempY = Convert.ToInt16(textBoxYdes.Text);
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }

            //Send desired X position
            outputBuffer[0] = 0xD0;
            outputBuffer[1] = (Byte)(tempX >> 8);
            outputBuffer[2] = (Byte)(tempX & 0x00FF);
            UARTSendMethod(outputBuffer);

            //Send desired Y position
            outputBuffer[0] = 0xD1;
            outputBuffer[1] = (Byte)(tempY >> 8);
            outputBuffer[2] = (Byte)(tempY & 0x00FF);
            UARTSendMethod(outputBuffer);

            //Start position regulator
            outputBuffer[0] = 0xC7;
            outputBuffer[1] = 0x00;
            outputBuffer[2] = 0x00;
            UARTSendMethod(outputBuffer);

            //Start log if no logging is active
            if (formMainLogToolStrip.Text != "Stop log" && checkBoxEnableLog.Checked)
                toolStripLogClickMethod(sender, null);

            tsslInfo.Text = "Position regulation started!";
        }

        private void buttonRegulateAngle_Click(object sender, EventArgs e)
        {
            //Confirm start
            if (checkBoxStartConfirmation.Checked &&
                MessageBox.Show("Start the angle regulator?", "Confirmation required!", MessageBoxButtons.YesNo,
                MessageBoxIcon.Question, MessageBoxDefaultButton.Button2) != DialogResult.Yes)
            {
                return;
            }

            //Declare variables
            UInt16 angle;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Reset odometry
            if (checkBoxResetOdometry.Checked)
                ResetOdometry();

            //Set focus on stop button to allow easier emergency stop
            buttonSTOP.Focus();

            //Convert parameters
            try
            {
                switch (comboBoxAngleUnits.SelectedIndex)
                {
                    case (Int32)ComboBoxAngleUnit.MILIRADIANS:

                        //Normalize to 0 - 2PI (miliradians)
                        angle = (UInt16)(1000.0 * ((Convert.ToDouble(textBoxAngle.Text) / 1000.0) % (2 * Math.PI)));

                        break;

                    case (Int32)ComboBoxAngleUnit.DEGREES:

                        //Convert and normalize to 0 - 2PI (miliradians)
                        angle = (UInt16)(1000.0 * ((Convert.ToDouble(textBoxAngle.Text) * (Math.PI / 180.0)) % (2 * Math.PI)));

                        break;

                    default: 
                        return;    //Error
                }
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }
            
            //0 would disable angle regulator, send small value instead
            if (angle == 0)
                angle = ANGLE_REGULATOR_INCREMENT;

            //Start angle regulator
            outputBuffer[0] = 0xC7;
            outputBuffer[1] = (Byte)(angle >> 8);
            outputBuffer[2] = (Byte)(angle & 0x00FF);
            UARTSendMethod(outputBuffer);

            //Start log if no logging is active
            if (formMainLogToolStrip.Text != "Stop log" && checkBoxEnableLog.Checked)
                toolStripLogClickMethod(sender, null);

            tsslInfo.Text = "Angle regulation started! Angle increment: " + angle.ToString() + " mrad";
        }

        private void FormRegulators_FormClosing(object sender, FormClosingEventArgs e)
        {
            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_CheckboxLogToFileEnabled = checkBoxEnableLog.Checked;
            WheelchairControl.Properties.Settings.Default.Setting_CheckboxRequireStartConfirmation = checkBoxStartConfirmation.Checked;
            WheelchairControl.Properties.Settings.Default.Setting_CheckboxResetOdometry = checkBoxResetOdometry.Checked;

            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void FormRegulators_Load(object sender, EventArgs e)
        {
            //Restore settings
            checkBoxEnableLog.Checked =  WheelchairControl.Properties.Settings.Default.Setting_CheckboxLogToFileEnabled;
            checkBoxStartConfirmation.Checked = WheelchairControl.Properties.Settings.Default.Setting_CheckboxRequireStartConfirmation;
            checkBoxResetOdometry.Checked = WheelchairControl.Properties.Settings.Default.Setting_CheckboxResetOdometry;

            //Select degrees in combobox
            comboBoxAngleUnits.SelectedIndex = (Int32)ComboBoxAngleUnit.DEGREES;

            //Select last waypoint
            comboBoxWaypointSelection.SelectedIndex = comboBoxWaypointSelection.Items.Count - 1;
        }

        private void collapsibleGroupBoxSettings_CollapseBoxClickedEvent(object sender)
        {
            UInt16 FORM_TO_BUTTONSTOP_HEIGH_RATIO = 61;

            //Resize form
            buttonSTOP.Height = (collapsibleGroupBoxSettings.Top + collapsibleGroupBoxSettings.Height) - buttonSTOP.Top;
            this.Height = buttonSTOP.Height + FORM_TO_BUTTONSTOP_HEIGH_RATIO;
        }

        private void buttonOdometryResetAll_Click(object sender, EventArgs e)
        {
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1] { 0xA3, 0x00, 0x00 };

            //Stop motors -> position regulator could start the wheelchair if odometry is reset
            SendStopSequence();

            UARTSendMethod(outputBuffer);

            tsslInfo.Text = "All coordinates reset";
        }

        private void buttonOdometrySetPhi_Click(object sender, EventArgs e)
        {
            Int16 tempValue;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Stop motors -> position regulator could start the wheelchair if odometry is reset
            SendStopSequence();

            //Convert parameters
            try
            {
                tempValue = Convert.ToInt16(textBoxOdometryPhi.Text);
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }

            outputBuffer[0] = 0xA2;
            outputBuffer[1] = (Byte)(tempValue >> 8);
            outputBuffer[2] = (Byte)(tempValue & 0x00FF);
            UARTSendMethod(outputBuffer);

            tsslInfo.Text = "Odometry Phi set to " + tempValue.ToString() + ".";
        }

        private void buttonOdometrySetY_Click(object sender, EventArgs e)
        {
            Int16 tempValue;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Stop motors -> position regulator could start the wheelchair if odometry is reset
            SendStopSequence();

            //Convert parameters
            try
            {
                tempValue = Convert.ToInt16(textBoxOdometryY.Text);
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }

            outputBuffer[0] = 0xA1;
            outputBuffer[1] = (Byte)(tempValue >> 8);
            outputBuffer[2] = (Byte)(tempValue & 0x00FF);
            UARTSendMethod(outputBuffer);

            tsslInfo.Text = "Odometry Y set to " + tempValue.ToString() + ".";
        }

        private void buttonOdometrySetX_Click(object sender, EventArgs e)
        {
            Int16 tempValue;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Stop motors -> position regulator could start the wheelchair if odometry is reset
            SendStopSequence();

            //Convert parameters
            try
            {
                tempValue = Convert.ToInt16(textBoxOdometryX.Text);
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }

            outputBuffer[0] = 0xA0;
            outputBuffer[1] = (Byte)(tempValue >> 8);
            outputBuffer[2] = (Byte)(tempValue & 0x00FF);
            UARTSendMethod(outputBuffer);

            tsslInfo.Text = "Odometry X set to " + tempValue.ToString() + ".";
        }

        private void buttonTestFinalApproach_Click(object sender, EventArgs e)
        {
            //No more implemented on this form, test function on FormMain
            tsslInfo.Text = "Function unavailable on this form. Use FormMain instead.";
        }

        private void buttoFollowPathFromWaypoint_Click(object sender, EventArgs e)
        {
            //No more implemented on this form, test function on FormMain
            tsslInfo.Text = "Function unavailable on this form. Use FormMain instead.";
        }

        private void buttonFindMarks_Click(object sender, EventArgs e)
        {
            //No more implemented on this form, test function on FormMain
            tsslInfo.Text = "Function unavailable on this form. Use FormMain instead.";
        }

        private void buttonTestBackwardDrive_Click(object sender, EventArgs e)
        {
            //No more implemented on this form, test function on FormMain
            tsslInfo.Text = "Function unavailable on this form. Use FormMain instead.";
        }

        private void buttonSetXdes_Click(object sender, EventArgs e)
        {
            Int16 tempX = 0;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Convert parameters
            try
            {
                tempX = Convert.ToInt16(textBoxXdes.Text);
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }

            //Send desired X position
            outputBuffer[0] = 0xD0;
            outputBuffer[1] = (Byte)(tempX >> 8);
            outputBuffer[2] = (Byte)(tempX & 0x00FF);
            UARTSendMethod(outputBuffer);

            //Info
            tsslInfo.Text = "Desired X coordinate set to: " + tempX.ToString() + " cm.";
        }

        private void buttonSetYdes_Click(object sender, EventArgs e)
        {
            Int16 tempY = 0;
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1];

            //Convert parameters
            try
            {
                tempY = Convert.ToInt16(textBoxYdes.Text);
            }
            catch (Exception ex)
            {
                if (ex is OverflowException || ex is FormatException)
                {
                    //Process wrong parameter exceptions
                    tsslInfo.Text = "Wrong parameter specified!";

                    return;
                }
                else
                    //Other exceptions are thrown higher
                    throw;
            }

            //Send desired Y position
            outputBuffer[0] = 0xD1;
            outputBuffer[1] = (Byte)(tempY >> 8);
            outputBuffer[2] = (Byte)(tempY & 0x00FF);
            UARTSendMethod(outputBuffer);

            //Info
            tsslInfo.Text = "Desired Y coordinate set to: " + tempY.ToString() + " cm.";
        }
    }
}
