using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Imaging;
using System.IO;
using System.IO.Ports;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Windows.Forms;

//Emgu usings
using Emgu.CV;
using Emgu.CV.Structure;

//My namespaces
using CsharpUSBJoystick;
using World;
using MyImageProcessingClasses;

namespace WheelchairControl
{
    /// <summary>
    /// Main GUI form.
    /// </summary>
    public partial class FormMain : Form
    {
        #region Constants

        /// <summary>
        /// Com port status (open / closed) sampling period [ms].
        /// </summary>
        public const Int32 COM_PORT_STATUS_SAMPLING_PERIOD_MS = 500;

        /// <summary>
        /// Command packet data length.
        /// </summary>
        public const Byte RX_DATA_LENGTH = 4;

        /// <summary>
        /// Indexes of particular items in ComboboxMeansofControl.
        /// </summary>
        private enum ComboboxControlIndexes : byte
        {
            /// <summary>
            /// Control moment using feedforward control.
            /// </summary>
            MOMENT,

            /// <summary>
            /// Use speed regulator.
            /// </summary>
            SPEED_REGULATOR
        };

        /// <summary>
        /// Indexes of particular items in ComboboxUserInput
        /// </summary>
        private enum ComboboxUserInputIndexes : byte
        {
            /// <summary>
            /// Joystick input index.
            /// </summary>
            JOYSTICK,

            /// <summary>
            /// Wifi input index.
            /// </summary>
            WIFI
        };

        /// <summary>
        /// Enum to specify mark detection method.
        /// </summary>
        private enum ComboboxDetectionMethodIndexes :  byte
        {
            /// <summary>
            /// Use reference HSV method to detect marks.
            /// </summary>
            REFERENCE_HSV,

            /// <summary>
            /// Use Surface detector to detect marks.
            /// </summary>
            SURF_DETECTOR
        }

        /// <summary>
        /// Rendering modes of the world map.
        /// </summary>
        private enum WorldMapRenderingModes : byte
        { 
            /// <summary>
            /// High speed mode.
            /// </summary>
            HIGH_SPEED,

            /// <summary>
            /// High quality mode.
            /// </summary>
            HIGH_QUALITY,

            /// <summary>
            /// Antialiasing mode.
            /// </summary>
            ANTIALIAS,

            /// <summary>
            /// Default mode.
            /// </summary>
            NONE
        }

        /// <summary>
        /// Indexes of tabs in tabcontrol Joystick / Auto Parking
        /// </summary>
        private enum JoyAutoParkingSelesctionIndex : byte
        { 
            /// <summary>
            /// Joystick control tab.
            /// </summary>
            JOYSTICK = 0,

            /// <summary>
            /// Autoparking tab.
            /// </summary>
            AUTOPARKING        
        }

        /// <summary>
        /// Indexes of tabs in TabControlMain.
        /// </summary>
        private enum MainPageTabsIndexes : byte
        { 
            /// <summary>
            /// Camera tab index.
            /// </summary>
            CAMERA,

            /// <summary>
            /// Settings tab index.
            /// </summary>
            SETTINGS,

            /// <summary>
            /// Log tab index.
            /// </summary>
            LOG
        }

        /// <summary>
        /// Enum to determine whether detected marks should be transmitted with the frame.
        /// </summary>
        private enum DetectedMarksTransmit : byte
        {
            /// <summary>
            /// Don't transmit marks flag.
            /// </summary>
            DONT_TRANSMIT,

            /// <summary>
            /// Transmit marks flag.
            /// </summary>
            TRANSMIT
        }

        /// <summary>
        /// Enum to determine UDP packet message header type.
        /// </summary>
        private enum PacketHeader : byte
        {
            /// <summary>
            /// Empty packet to reset watchdog.
            /// </summary>
            WATCHDOG_NOP = 0xC0,

            /// <summary>
            /// Artificial joystick data.
            /// </summary>
            JOYSTICK_COMMAND = 0xC1,

            /// <summary>
            /// Press remote stop button.
            /// </summary>
            STOP_BUTTON = 0xC2,

            /// <summary>
            /// Press remote start button.
            /// </summary>
            START_BUTTON = 0xC3,

            /// <summary>
            /// Press auto parking start button remotely.
            /// </summary>
            AUTO_PARKING_START_BUTTON = 0xC4,

            /// <summary>
            /// Request to send camera frame.
            /// </summary>
            CAMERA_FRAME_REQUEST = 0xC5,

            /// <summary>
            /// User's confirmation of mark detection correctness.
            /// </summary>
            USER_RESPONSE = 0xC6,

            /// <summary>
            /// Transmitt current auto parking phase.
            /// </summary>
            AUTOPARKING_PHASE = 0xC7,

            /// <summary>
            /// Camera frame data packet.
            /// </summary>
            CAMERA_FRAME_RESPONSE = 0xD0,

            /// <summary>
            /// Camera frame with detected marks to be confirmed by user.
            /// </summary>
            CAMERA_FRAME_TO_BE_CONFIRMED = 0xD1
        }
        
        /// <summary>
        /// Bytes' indexes in odometry status packet.
        /// </summary>
        private enum OdometryPacketByte : byte
        { 
            /// <summary>
            /// X coordinate base index (X length = 2 bytes).
            /// </summary>
            X_BASE = 0,
            
            /// <summary>
            /// Y coordinate base index (Y length = 2 bytes).
            /// </summary>
            Y_BASE = 2,

            /// <summary>
            /// Phi coordinate base index (Phi length = 2 bytes).
            /// </summary>
            PHI_BASE = 4,

            /// <summary>
            /// SRF08 front distance.
            /// </summary>
            SRF08_FRONT_DIST = 6,

            /// <summary>
            /// SRF08 rear distance.
            /// </summary>
            SRF08_REAR_DIST = 7,

            /// <summary>
            /// Position regulator enable byte.
            /// </summary>
            ENABLE_POSITION_REGULATOR = 8,

            /// <summary>
            /// Wall-following regulatro enable byte.
            /// </summary>
            ENABLE_WALL_FOLLOWING = 9
        }

        /// <summary>
        /// Enum to determine index in comboboxReferenceCoordinates.
        /// </summary>
        private enum MapReferenceCoordinates : byte
        { 
            /// <summary>
            /// Set center of marks as reference point.
            /// </summary>
            MARKS_CENTER,

            /// <summary>
            /// Set wheelchair center as reference point.
            /// </summary>
            WHEELCHAIR_CENTER
        }

        /// <summary>
        /// Enum to specify return codes of UARTreceive().
        /// </summary>
        private enum RxReturnCodes : byte
        { 
            /// <summary>
            /// Transmission was successful.
            /// </summary>
            TRANSMISSION_OK,

            /// <summary>
            /// Error - port is closed.
            /// </summary>
            PORT_CLOSED,

            /// <summary>
            /// Error - packet not complete.
            /// </summary>
            PACKET_INCOMPLETE,

            /// <summary>
            /// Error - packet checksum was wrong.
            /// </summary>
            WRONG_CHECKSUM,
        }

        /// <summary>
        /// Joystick maximal value.
        /// </summary>
        private const Byte JOYSTICK_RANGE = 255;

        /// <summary>
        /// Uart timeout [ms]
        /// </summary>
        private const Int32 UART_RX_TIMEOUT_MS = 50;

        /// <summary>
        /// Wifi watchdog timeout [ms].
        /// </summary>
        private const Int32 WIFI_WATCHDOG_TIMEOUT_MS = 1000;

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

        /// <summary>
        /// Random (not well-known) UDP port.
        /// </summary>
        private const UInt16 UDP_PORT = 50005;

        #endregion

        #region Events and delegates

        /// <summary>
        /// Delegate of method to be called when COM port status changes.
        /// </summary>
        public delegate void COMPortStatusChangedDelegate();

        /// <summary>
        /// Event invoked when COM port status changes.
        /// </summary>
        public static event COMPortStatusChangedDelegate COMStatusChanged;

        #endregion

        #region Private variables

        /// <summary>
        /// Thread to be run separately and receive UDP communication asynchronously.
        /// </summary>
        private Thread UDPreceiveThread;

        /// <summary>
        /// Thread to run auto parking algorithm.
        /// </summary>
        private Thread AutoParkingThread;

        /// <summary>
        /// Shared IP Endpoint to receive from all IP addresses.
        /// </summary>
        private IPEndPoint ipEp = new IPEndPoint(IPAddress.Any, UDP_PORT);

        /// <summary>
        /// UDP client to receive data.
        /// </summary>
        private UdpClient incomingUDPClient;

        /// <summary>
        /// UDP client to send data.
        /// </summary>
        private UdpClient outgoingUDPClient;

        /// <summary>
        /// Auto reset event to wait for user response.
        /// </summary>
        private AutoResetEvent arUserResponseTimout;

        /// <summary>
        /// Log start time.
        /// </summary>
        private DateTime swLogStartTime;

        /// <summary>
        /// Joystick instance.
        /// </summary>
        private USB_Joystick joystick;

        /// <summary>
        /// Thread to monitor COM port status and invoke ComStatusChanged event.
        /// </summary>
        private Thread threadCOMPortStatusGuard;

        /// <summary>
        /// State of COM port (open/closed).
        /// </summary>
        private Boolean portState = true;

        /// <summary>
        /// UART send routine lock.
        /// </summary>
        private static Object lockUart = new Object();
                
        /// <summary>
        /// Flag to allow PC to control wheelchair's motors.
        /// </summary>
        private Boolean motorControlAllowed = false;
        
        /// <summary>
        /// Log memory stream writer.
        /// </summary>
        private StreamWriter swLogWriter;

        /// <summary>
        /// Fake joystick to simulate XYZ axes during WIFI drive.
        /// </summary>
        private Fake_USBJoystick wifiJoy = new Fake_USBJoystick(JOYSTICK_RANGE / 2, JOYSTICK_RANGE / 2, 0);

        #endregion

        #region Miscelaneous methods

        /// <summary>
        /// Loads coordinates of calibration points from the storage.
        /// </summary>
        private void LoadCalibrationCoordinates()
        {
            foreach (CameraCalibrator.PointLocation location in CameraCalibrator.CalibrationPoints.Keys)
            {
                //Create string
                String coordsString = "(" + CameraCalibrator.CalibrationPoints[(Byte)location].X.ToString("F1")
                                        + ";" + CameraCalibrator.CalibrationPoints[(Byte)location].Y.ToString("F1") + ")";

                //Assign string to proper radio button
                switch (location)
                {
                    case CameraCalibrator.PointLocation.UPPER_LEFT:

                        rbUL.Text = "Upper left " + coordsString;

                        break;

                    case CameraCalibrator.PointLocation.UPPER_RIGHT:

                        rbUR.Text = "Upper right " + coordsString;

                        break;

                    case CameraCalibrator.PointLocation.LOWER_RIGHT:

                        rbLR.Text = "Lower right " + coordsString;

                        break;

                    case CameraCalibrator.PointLocation.LOWER_LEFT:

                        rbLL.Text = "Lower left " + coordsString;

                        break;

                    default: break;
                }
            }
        }

        /// <summary>
        /// Function to be run in separate thread and receive data via UDP.
        /// </summary>
        private void UDPDataReceiver()
        {
            try
            {
                Byte[] inBuffer; //Incoming data buffer
                String logMessage = null; //Message to be logged

                //Set socket timeout
                incomingUDPClient.Client.ReceiveTimeout = WIFI_WATCHDOG_TIMEOUT_MS;

                while (true)
                {
                    try
                    {
                        //Log message from previous loop (logged immediatelly, since thread waits on Receive() method)
                        //No need to check InvokeRequired property, since this thread is run separately
                        this.Invoke((MethodInvoker)delegate()
                        {
                            if (logMessage != null)
                            {
                                Timestamp();
                                textBoxData.AppendText(logMessage + Environment.NewLine);

                                logMessage = null;
                            }
                        });

                        //Receive
                        inBuffer = incomingUDPClient.Receive(ref ipEp);

                        //Blink indicator
                        //No need to check InvokeRequired property, since this thread is run separately
                        this.Invoke((MethodInvoker)delegate()
                        {
                            if (ssLabelWiFiWatchdog.BackColor == ssMain.BackColor)
                                ssLabelWiFiWatchdog.BackColor = Color.LimeGreen;
                            else
                                ssLabelWiFiWatchdog.BackColor = ssMain.BackColor;
                        });

                        //Parse header
                        switch (inBuffer[0])
                        {
                            case (Byte)PacketHeader.JOYSTICK_COMMAND:

                                //Set fake joy's coordinates
                                wifiJoy.SetCoordinates(inBuffer[1], inBuffer[2], inBuffer[3]);

                                //Create message to log
                                logMessage = "Joy coordinates received: X: " + inBuffer[1].ToString() + ", Y: " 
                                    + inBuffer[2].ToString() + ", Z: " + inBuffer[3].ToString();

                                break;

                            case (Byte)PacketHeader.STOP_BUTTON:

                                //Click stop button remotely
                                //No need to check InvokeRequired property, since this thread is run separately
                                this.Invoke((MethodInvoker)delegate()
                                {
                                    buttonSTOP_Click(this, null);
                                });

                                //Create message to log
                                logMessage = "Remote STOP command received";

                                break;

                            case (Byte)PacketHeader.START_BUTTON:

                                //Click start button remotely
                                //No need to check InvokeRequired property, since this thread is run separately
                                this.Invoke((MethodInvoker)delegate()
                                {
                                    //Only when wifi is used as input
                                    if (comboBoxUserInput.SelectedIndex == (Byte)ComboboxUserInputIndexes.WIFI)
                                        buttonSTART_Click(this, null);
                                });

                                //Create message to log
                                logMessage = "Remote START command received";

                                break;

                            case (Byte)PacketHeader.AUTO_PARKING_START_BUTTON:

                                //Click auto parking start button remotely
                                this.Invoke((MethodInvoker)delegate()
                                {
                                    //Only when wifi is used as input
                                    if (comboBoxUserInput.SelectedIndex == (Byte)ComboboxUserInputIndexes.WIFI)
                                        buttonSTARTAutoParking_Click(this, null);
                                });

                                //Create message to log
                                logMessage = "Remote autopraking START command received";

                                break;

                            case (Byte)PacketHeader.USER_RESPONSE:

                                //Analyze user response
                                wheelchairModel.UserResponse = 
                                    (WheelchairModel.UserResponseCode)Enum.Parse(typeof(WheelchairModel.UserResponseCode),
                                        inBuffer[1].ToString());

                                //Signal operation success
                                arUserResponseTimout.Set();

                                break;

                            case (Byte)PacketHeader.WATCHDOG_NOP:
                                {
                                    //Perform watchdog loopback if enabled, else no action
                                    if (checkBoxWatchdogResponseEnabled.Checked)
                                    {
                                        IPAddress targetIp = null;

                                        try
                                        {
                                            this.Invoke((MethodInvoker)delegate()
                                            {
                                                try
                                                {
                                                    targetIp = IPAddress.Parse(textBoxTargetIP.Text);
                                                }
                                                catch (Exception)
                                                { /*Failed to load address*/}
                                            });
                                        }
                                        catch (Exception)
                                        {/*Failed to load address*/}

                                        //Send response back
                                        outgoingUDPClient.Send(new Byte[] { (Byte)PacketHeader.WATCHDOG_NOP }, 1, new IPEndPoint(targetIp, UDP_PORT));
                                    }
                                }
                                break;

                            case (Byte)PacketHeader.CAMERA_FRAME_REQUEST:
                                {
                                    //Send actual camera frame
                                    List<Byte> packetTemp = new List<Byte>() { (Byte)PacketHeader.CAMERA_FRAME_RESPONSE };
                                    MemoryStream outStream = new MemoryStream();
                                    IPAddress targetIp = null;

                                    try
                                    {
                                        this.Invoke((MethodInvoker)delegate()
                                        {
                                            try
                                            {
                                                targetIp = IPAddress.Parse(textBoxTargetIP.Text);
                                            }
                                            catch (Exception)
                                            {  /*Failed to load addres*/}
                                        });
                                    }
                                    catch (Exception)
                                    {  /*Failed to load addres*/}

                                    //Switch clean image / image with detected marks and parse it into memorystream
                                    if (inBuffer[1] == (Byte)DetectedMarksTransmit.DONT_TRANSMIT)
                                    {
                                        //Set encoder parameters
                                        ImageProcessingTools.cameraFrameEncoderParameters.Param[ImageProcessingTools.ENCODER_PARAMETER_FIRST] 
                                            = ImageProcessingTools.cameraFrameEncoderParameter[(Byte)numericUpDownJpegCompression.Value];

                                        lastCaptured.ToBitmap().Save(outStream, ImageProcessingTools.jpegCodec, ImageProcessingTools.cameraFrameEncoderParameters);
                                    }
                                    else
                                    {
                                        //Set encoder parameters
                                        ImageProcessingTools.cameraFrameEncoderParameters.Param[ImageProcessingTools.ENCODER_PARAMETER_FIRST]
                                            = ImageProcessingTools.cameraFrameEncoderParameter[(Byte)numericUpDownJpegCompression.Value];

                                        using (Image tempImg = new Bitmap(pbFrame.Image))    //Buffer image first
                                            tempImg.Save(outStream, ImageProcessingTools.jpegCodec, ImageProcessingTools.cameraFrameEncoderParameters);
                                    }

                                    //Append serialized image data
                                    foreach (Byte dataByte in outStream.ToArray())
                                        packetTemp.Add(dataByte);

                                    //Send
                                    outgoingUDPClient.Send(packetTemp.ToArray(), packetTemp.Count, new IPEndPoint(targetIp, UDP_PORT));

                                    //Create message to log
                                    logMessage = (inBuffer[1] == (Byte)DetectedMarksTransmit.DONT_TRANSMIT ? "Camera frame query received. Marks positions not sent" : "Camera frame query received. Marks positions sent");
                                }
                                break;

                            default:
                                //Next loop, don't reset watchdog timer
                                continue;
                        }
                    }
                    catch (SocketException socketEx)
                    {
                        //Process exception only if it is caused by receive() timeout
                        if (socketEx.SocketErrorCode == SocketError.TimedOut)
                        {
                            //Click stop button
                            //No need to check InvokeRequired property, since this thread is run separately
                            this.Invoke((MethodInvoker)delegate()
                            {
                                if (ssLabelWiFiWatchdog.BackColor != Color.Red) //First occurence of watchdog timeout
                                {
                                    ssLabelWiFiWatchdog.BackColor = Color.Red;

                                    Timestamp();
                                    textBoxData.AppendText("WiFi watchdog timer elapsed!" + Environment.NewLine);

                                    buttonSTOP_Click(this, null);
                                }
                            });
                        }
                        else
                        {
                            //Pass exception further
                            throw;
                        }
                    }
                    catch (Exception ex)
                    {
                        //Log error and blink indicator
                        //No need to check InvokeRequired property, since this thread is run separately
                        this.Invoke((MethodInvoker)delegate()
                        {
                            Timestamp();
                            textBoxData.AppendText(ex.Message.ToString() + Environment.NewLine);
                        });
                    }
                }
            }
            catch (ThreadInterruptedException)
            {/*Handle something here???*/}
            catch (ThreadAbortException)
            {/*Handle something here???*/}
            catch (Exception)
            {/*Handle something here???*/}
        }

        /// <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;
                UARTsend(outputBuffer);
            }
        }

        /// <summary>
        /// Reset odometry coordinates.
        /// </summary>
        private void ResetOdometry()
        {
            uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1] { 0xA3, 0x00, 0x00 };

            UARTsend(outputBuffer);
        }

        /// <summary>
        /// Visualize current phase.
        /// </summary>
        /// <param name="phase">Current phase.</param>
        /// <param name="isError">True if error occured.</param>
        private void VisualizePhase(WheelchairModel.AutoParkingPhase phase, Boolean isError)
        {
            //Invoke always
            this.Invoke((MethodInvoker)delegate()
            {
                Color lblColor = isError ? Color.Red : Color.Lime;

                //Erase all labels
                foreach (Control ctrl in tabPageAutoParking.Controls)
                    if (ctrl is Label)
                        ((Label)ctrl).BackColor = tabPageAutoParking.BackColor;

                //Light up correct label
                switch (phase)
                {
                    default:
                    case WheelchairModel.AutoParkingPhase.WAITING_FOR_START:

                        labelPhaseWaitToStart.BackColor = lblColor;

                        break;

                    case WheelchairModel.AutoParkingPhase.WALL_FOLLOWING:

                        labelPhaseWallFollowing.BackColor = lblColor;

                        break;

                    case WheelchairModel.AutoParkingPhase.GOING_BACKWARDS:

                        labelPhaseBackwardDrive.BackColor = lblColor;

                        break;

                    case WheelchairModel.AutoParkingPhase.MARKS_TRACKING:

                        labelPhaseDetectingMarks.BackColor = lblColor;

                        break;

                    case WheelchairModel.AutoParkingPhase.PATH_FOLLOWING:

                        labelPhaseNextWaypoint.BackColor = lblColor;

                        break;

                    case WheelchairModel.AutoParkingPhase.END:

                        labelPhaseEnd.BackColor = lblColor;

                        break;
                }

                //Send via Wifi, if WiFi is enabled
                if (comboBoxUserInput.SelectedIndex == (Byte)ComboboxUserInputIndexes.WIFI)
                {
                    IPAddress targetIp = null;

                    try
                    {
                        targetIp = IPAddress.Parse(textBoxTargetIP.Text);
                    }
                    catch (Exception)
                    { /*Failed to load address*/}

                    //Send response back
                    outgoingUDPClient.Send(new Byte[] { (Byte)PacketHeader.AUTOPARKING_PHASE, (Byte)phase, 
                    isError ? (Byte)WheelchairModel.PhaseError.ERROR : (Byte)WheelchairModel.PhaseError.ERROR },
                        3 /*Packet length*/, new IPEndPoint(targetIp, UDP_PORT));
                }
            });
        }

        /// <summary>
        /// Sends data via UART.
        /// </summary>
        /// <param name="data">Data buffer to be sent.</param>
        private void UARTsend(UInt32[] data)
        {
            lock (lockUart)
            {
                if (data.Length != RX_DATA_LENGTH - 1)
                {
                    Timestamp();
                    textBoxData.AppendText("Wrong packet length: " + data.Length.ToString() + ", expected: " + (RX_DATA_LENGTH - 1).ToString() + Environment.NewLine);
                    return;
                }

                if (!serialPortComPort.IsOpen)
                {
                    Timestamp();
                    textBoxData.AppendText("Data not sent, port is closed" + Environment.NewLine);
                    return;
                }

                if (data[0] > 255 || data[1] > 255 || data[2] > 255)
                {
                    Timestamp();
                    textBoxData.AppendText("Data not sent, input data not valid" + Environment.NewLine);
                    return;
                }

                serialPortComPort.DiscardOutBuffer();

                Byte[] tempData = new Byte[RX_DATA_LENGTH];
                tempData[0] = (Byte)data[0];
                tempData[1] = (Byte)data[1];
                tempData[2] = (Byte)data[2];
                tempData[3] = (Byte)(data[0] + data[1] + data[2]);

                if (tempData[0] != 0)    //Do not log watchdog NOP packet
                {
                    Timestamp();
                    textBoxData.AppendText("SENT: " + "0x" + tempData[0].ToString("X") + " " + tempData[1].ToString() + " " + tempData[2].ToString() + " " + tempData[3].ToString() + Environment.NewLine);
                }

                serialPortComPort.Write(tempData, 0, tempData.Length);
            }
        }

        /// <summary>
        /// Log actual time.
        /// </summary>
        private void Timestamp()
        {
            textBoxData.AppendText(DateTime.Now.Hour.ToString("D2") + ":" + DateTime.Now.Minute.ToString("D2") + ":" + DateTime.Now.Second.ToString("D2") + " ");
        }

        /// <summary>
        /// Reads controller configuration via UART.
        /// </summary>
        /// <param name="instruction">Instruction code.</param>
        /// <param name="outBuffer">Reference to data buffer.</param>
        /// <returns>Status code (0 if succeedes).</returns>
        private Int32 UARTreceive(Byte instruction, ref Byte[] outBuffer)
        {
            Byte[] tempArray = new Byte[outBuffer.Length];
            Byte sum = 0;
            UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];

            //Return port not open error code if port is closed
            if (!serialPortComPort.IsOpen)
            {
                Timestamp();
                textBoxData.AppendText("Data not read, port closed" + Environment.NewLine);
                
                return (Int32)RxReturnCodes.PORT_CLOSED;
            }

            serialPortComPort.DiscardInBuffer();

            //Send request
            outputBuffer[0] = instruction;
            outputBuffer[1] = 0x00;
            outputBuffer[2] = 0x00;
            UARTsend(outputBuffer);

            DateTime startTime = DateTime.Now;

            while (serialPortComPort.BytesToRead < outBuffer.Length)   //Wait for all bytes  with specified timeout
            {
                if (DateTime.Now - startTime > TimeSpan.FromMilliseconds(UART_RX_TIMEOUT_MS))
                {
                    //If timeout passes, return error code
                    Timestamp();
                    textBoxData.AppendText("Packet incomplete, received bytes count: " + serialPortComPort.BytesToRead + "B" + Environment.NewLine);
                    
                    return (Int32)RxReturnCodes.PACKET_INCOMPLETE;
                }
            }

            serialPortComPort.Read(outBuffer, 0, outBuffer.Length);

            Timestamp();
            textBoxData.AppendText("RECEIVED: " + outBuffer[0].ToString() + "  " + outBuffer[1].ToString() + "  " + outBuffer[2].ToString() + "  " + outBuffer[3].ToString() + "  " + outBuffer[4].ToString() + "  " + outBuffer[5].ToString() + "  " + outBuffer[6].ToString() + "  " + outBuffer[7].ToString() + "  " + outBuffer[8].ToString() + "  " + outBuffer[9].ToString() + "  " + outBuffer[10].ToString() + "  " + outBuffer[11].ToString() + Environment.NewLine);

            //Calculate checksum
            for (Int32 i = 0; i < outBuffer.Length - 1; i++)
                sum += (Byte)outBuffer[i];

            //Return error code if checksum is not correct
            if (sum != outBuffer[outBuffer.Length - 1])
            {
                Timestamp();
                textBoxData.AppendText("Wrong checksum: " + sum.ToString() + " != " + outBuffer[outBuffer.Length - 1].ToString() + Environment.NewLine);
                
                return (Int32)RxReturnCodes.WRONG_CHECKSUM;
            }

            return (Int32)RxReturnCodes.TRANSMISSION_OK;
        }

        /// <summary>
        /// Determine whether X axis' value lies within borders.
        /// </summary>
        /// <param name="X">X axis value.</param>
        /// <returns>True if value lies within borders.</returns>
        private bool JoystickXinThreshold(Int32 X)
        {
            Int32 threshold = (Int32)((JOYSTICK_RANGE / 2) * ((float)numericUpDownThresholdPerc.Value / 100.0));

            if ((JOYSTICK_RANGE / 2 - threshold) > X || (JOYSTICK_RANGE / 2 + threshold) < X)
                return false;

            return true;
        }

        /// <summary>
        /// Determine whether Y axis' value lies within borders.
        /// </summary>
        /// <param name="Y">Y axis value.</param>
        /// <returns>True if value lies within borders.</returns>
        private bool JoystickYinThreshold(Int32 Y)
        {
            Int32 threshold = (Int32)((JOYSTICK_RANGE / 2) * ((float)numericUpDownThresholdPerc.Value / 100.0));

            if ((JOYSTICK_RANGE / 2 - threshold) > Y || (JOYSTICK_RANGE / 2 + threshold) < Y)
                return false;

            return true;
        }

        /// <summary>
        /// Calculates values to be sent to motors using X,Y,Z coordinates.
        /// </summary>
        /// <param name="X">X joystick coordinate.</param>
        /// <param name="Y">Y joystick coordinate.</param>
        /// <param name="Z">Z joystick coordinate.</param>
        private void SendCommandsToMotors(Int32 X, Int32 Y, Int32 Z)
        {
            Int32 offset = JOYSTICK_RANGE / 2, tempX, tempY;
            UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];
            float tempA, tempB, gain;

            //Calculate values
            gain = (Z / (float)JOYSTICK_RANGE);
            
            tempX = X - offset;
            tempY = Y - offset;

            tempA = ((tempY - tempX) * gain) + offset;
            tempB = ((tempY + tempX) * gain) + offset;

            //Truncate values
            if (tempA > JOYSTICK_RANGE) tempA = JOYSTICK_RANGE;
            if (tempB > JOYSTICK_RANGE) tempB = JOYSTICK_RANGE;
            if (tempA < 0) tempA = 0;
            if (tempB < 0) tempB = 0;
            
            //Visualize using progress bars
            progressBarRight.Value = (Int32)tempA;
            progressBarLeft.Value = (Int32)tempB;
            labelRychlostA.Text = ((Int32)tempA).ToString();
            labelRychlostB.Text = ((Int32)tempB).ToString();

            //Switch control methods
            if (comboBoxMeanOfControl.SelectedIndex == 0)
            {
                //Use feedforward moment control
                outputBuffer[0] = 0xC0;
                outputBuffer[1] = 0x00;
                outputBuffer[2] = (UInt32)tempA;
                UARTsend(outputBuffer);

                outputBuffer[0] = 0xC1;
                outputBuffer[1] = 0x00;
                outputBuffer[2] = (UInt32)tempB;
                UARTsend(outputBuffer);
            }
            else
            {
                //Use speed regulators
                Int16 temp = (Int16)(Convert.ToInt16(labelRychlostA.Text) - offset + 500);

                outputBuffer[0] = 0xC2;
                outputBuffer[1] = (UInt32)temp >> 8;
                outputBuffer[2] = (UInt32)temp % 256;
                UARTsend(outputBuffer);

                temp = (Int16)(Convert.ToInt16(labelRychlostB.Text) - offset + 500);

                outputBuffer[0] = 0xC3;
                outputBuffer[1] = (UInt32)temp >> 8;
                outputBuffer[2] = (UInt32)temp % 256;
                UARTsend(outputBuffer);
            }
        }

        /// <summary>
        /// Method to be invoked by COMPortStatusGuard thread.
        /// Supposed to invoke events when Port.IsOpen sttribute changes.
        /// </summary>
        private static void COMPortStatusGuard_Method(FormMain form)
        {
            while (true)
            {
                if (form.serialPortComPort.IsOpen != form.portState)
                {
                    //Invoke event
                    if (COMStatusChanged != null)
                        COMStatusChanged();
                }

                form.portState = form.serialPortComPort.IsOpen;

                Thread.Sleep(COM_PORT_STATUS_SAMPLING_PERIOD_MS);
            }
        }

        /// <summary>
        /// Method o be run in a separated thread and perform auto parking algorithm.
        /// </summary>
        private void AutoParkingThreadMethod()
        {
            try
            {
                uint[] outputBuffer = new uint[RX_DATA_LENGTH - 1]; //UART data buffer
                Boolean error;  //Error flag

                while (true)
                {
                    //Reset error flag
                    error = false;

                    //Reset regulation flag
                    wheelchairModel.IsRegulationActive = false;

                    //Run state machine
                    switch (wheelchairModel.CurrentPhase)
                    {
                        //Init algorithm
                        case WheelchairModel.AutoParkingPhase.WAITING_FOR_START:

                            //Start algorithm
                            wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.WALL_FOLLOWING;

                            break;

                        //Wall-following phase
                        case WheelchairModel.AutoParkingPhase.WALL_FOLLOWING:

                            //Invoke
                            this.Invoke((MethodInvoker)delegate()
                            {
                                //Reset odometry
                                ResetOdometry();

                                //Set desired X (wall-following track limit)
                                outputBuffer[0] = 0xD0;
                                outputBuffer[1] = (Byte)(Car.DRIVER_TO_REAR_DISTANCE_CM >> 8);
                                outputBuffer[2] = (Byte)(Car.DRIVER_TO_REAR_DISTANCE_CM & 0x00FF);
                                UARTsend(outputBuffer);

                                //Start wall-following
                                outputBuffer[0] = 0xC6;
                                outputBuffer[1] = (Byte)(WheelchairModel.VDES + 128);
                                outputBuffer[2] = (Byte)(WheelchairModel.Y0DES);
                                UARTsend(outputBuffer);

                                wheelchairModel.IsRegulationActive = true;
                            });

                            wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.GOING_BACKWARDS;

                            break;

                        //Driving backwards to get marks into the field of view
                        case WheelchairModel.AutoParkingPhase.GOING_BACKWARDS:
                            //Invoke
                            this.Invoke((MethodInvoker)delegate()
                            {
                                Int16 temp = (Int16)(WheelchairModel.VDES + 500);

                                //Reset odometry
                                ResetOdometry();

                                //Send commands to both motors to go back(do not use point-to-point regulator, wheelchair would rotate 180°)
                                outputBuffer[0] = 0xC2;
                                outputBuffer[1] = (uint)temp >> 8;
                                outputBuffer[2] = (uint)temp % 256;
                                UARTsend(outputBuffer);

                                outputBuffer[0] = 0xC3;
                                UARTsend(outputBuffer);
                            });

                            while (Math.Abs(wheelchairModel.X) < (100 * Car.PLATFORM_LENGTH))
                            {
                                //Wait for regulation to end
                                Thread.Sleep(timerLog.Interval);
                            }

                            wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.MARKS_TRACKING;

                            break;

                        //Look for marks
                        case WheelchairModel.AutoParkingPhase.MARKS_TRACKING:
                            {
                                Point leftMark, rightMark;
                                Boolean nextLoop = false;

                                //Invoke
                                this.Invoke((MethodInvoker)delegate()
                                {
                                    //Reset detectors
                                    leftMarkDetector.Reset();
                                    rightMarkDetector.Reset();
                                });

                                while (!leftMarkDetector.IsBufferFull || !rightMarkDetector.IsBufferFull)
                                { /*Wait for buffers to fill up*/ }

                                if (leftMarkDetector.DetectMark(out leftMark) && rightMarkDetector.DetectMark(out rightMark))
                                {
                                    //Marks detected, actualize map and position
                                    Double tmpX = 0, tmpY = 0, tmpPhi;

                                    //Invoke
                                    this.Invoke((MethodInvoker)delegate()
                                    {
                                        //Check if user confirmation is required
                                        if (checkBoxUserResponseDemanded.Checked)
                                        {
                                            #region Send

                                            //Send image and wait for response
                                            List<Byte> packetTemp = new List<Byte>() { (Byte)PacketHeader.CAMERA_FRAME_TO_BE_CONFIRMED };
                                            MemoryStream outStream = new MemoryStream();
                                            IPAddress targetIp = null;

                                            try
                                            {
                                                targetIp = IPAddress.Parse(textBoxTargetIP.Text);
                                            }
                                            catch (Exception)
                                            {  /*Failed to load addres*/}

                                            //Set encoder parameters
                                            ImageProcessingTools.cameraFrameEncoderParameters.Param[ImageProcessingTools.ENCODER_PARAMETER_FIRST]
                                                = ImageProcessingTools.cameraFrameEncoderParameter[(Byte)numericUpDownJpegCompression.Value];

                                            //Serialize image into the stream
                                            using (Image tempImg = new Bitmap(pbFrame.Image))
                                                tempImg.Save(outStream, ImageProcessingTools.jpegCodec, ImageProcessingTools.cameraFrameEncoderParameters);

                                            //Append serialized image data
                                            foreach (Byte dataByte in outStream.ToArray())
                                                packetTemp.Add(dataByte);

                                            //Reset the autoResetEvent
                                            arUserResponseTimout.Reset();

                                            //Send
                                            outgoingUDPClient.Send(packetTemp.ToArray(), packetTemp.Count, new IPEndPoint(targetIp, UDP_PORT));

                                            #endregion

                                            #region Response

                                            //Wait for response
                                            if (!arUserResponseTimout.WaitOne(WheelchairModel.USER_RESPONSE_TIMEOUT_MS))
                                            {
                                                //Response not received, end
                                                wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.END;
                                                error = true;
                                            }

                                            //Select proper action
                                            switch (wheelchairModel.UserResponse)
                                            {
                                                //End now
                                                default:
                                                case WheelchairModel.UserResponseCode.END:

                                                    //End script
                                                    wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.END;
                                                    error = true;

                                                    break;

                                                //Rotate and try to find marks
                                                case WheelchairModel.UserResponseCode.LOOK_FURTHER:

                                                    ResetOdometry();

                                                    wheelchairModel.IsRegulationActive = true;

                                                    //Rotate
                                                    outputBuffer[0] = 0xC7;
                                                    outputBuffer[1] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD >> 8);
                                                    outputBuffer[2] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD & 0x00FF);
                                                    UARTsend(outputBuffer);

                                                    //Run next loop
                                                    nextLoop = true;

                                                    break;

                                                //Ok, proceed
                                                case WheelchairModel.UserResponseCode.MARKS_FOUND:
                                                    break;
                                            }

                                            #endregion
                                        }

                                        if (!nextLoop) //Exit loop (break / continue cannot be used from within the Invoke() block)
                                        {
                                            //Actualize marks' positions
                                            if (leftMark.X > 0 && leftMark.Y > 0)
                                                worldMap.ModifyMark(WorldMap.MarkPosition.LEFT, leftMark, Color.GreenYellow);

                                            if (rightMark.X > 0 && rightMark.Y > 0)
                                                worldMap.ModifyMark(WorldMap.MarkPosition.RIGHT, rightMark, Color.Red);

                                            //Racalculate coordinates
                                            if (!RecalculateRefPointToMarksCenter(ref tmpX, ref tmpY, out tmpPhi))
                                                error = true;

                                            //Proceed only if no error occured
                                            if (!error)
                                            {
                                                //Actualize wheelchair odometry
                                                try
                                                {
                                                    //Convert to Int16
                                                    Int16 iX = Convert.ToInt16(100 * tmpX);    //Convert to cm
                                                    Int16 iY = Convert.ToInt16(100 * tmpY);    //Convert to cm
                                                    Int16 iPhi = Convert.ToInt16(tmpPhi);

                                                    //Actualize odometry
                                                    outputBuffer[0] = 0xA0;
                                                    outputBuffer[1] = (Byte)(iX >> 8);
                                                    outputBuffer[2] = (Byte)(iX & 0x00FF);
                                                    UARTsend(outputBuffer);

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

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

                                                    //Switch state
                                                    wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.PATH_FOLLOWING;
                                                }
                                                catch (Exception)
                                                {
                                                    //Error occured
                                                    error = true;
                                                }
                                            }
                                        }
                                    });
                                }
                                else
                                {
                                    //Invoke
                                    this.Invoke((MethodInvoker)delegate()
                                    {
                                        //Marks not detected, rotate and try again
                                        ResetOdometry();

                                        wheelchairModel.IsRegulationActive = true;

                                        //Rotate
                                        outputBuffer[0] = 0xC7;
                                        outputBuffer[1] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD >> 8);
                                        outputBuffer[2] = (Byte)(WheelchairModel.MARK_DETECT_ROTATION_ANGLE_MRAD & 0x00FF);
                                        UARTsend(outputBuffer);
                                    });
                                }
                            }

                            break;

                        //Driving to next waypoint
                        case WheelchairModel.AutoParkingPhase.PATH_FOLLOWING:
                            {
                                //Transform target coordinates
                                Double targetX = worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.X;
                                Double targetY = worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.Y;
                                Double tempPhi;

                                //Racalculate coordinates
                                if (!RecalculateRefPointToMarksCenter(ref targetX, ref targetY, out tempPhi))
                                {
                                    error = true;
                                }

                                //Check if wheelchair is in the target position
                                if (Math.Sqrt((Double)(Math.Pow((wheelchairModel.X - targetX / 10 /*mm to cm*/), 2) + Math.Pow((wheelchairModel.Y - targetY / 10 /*mm to cm*/), 2))) < WheelchairModel.TARGET_COORD_ALLOWED_ERROR_CM)
                                {
                                    //Target reached, remove point from buffer
                                    if (worldMap.TrackPoints.Count == 1)
                                    {
                                        //Last point reached, end
                                        wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.END;
                                    }
                                    else
                                    {
                                        //Remove point
                                        worldMap.TrackPoints.Remove(worldMap.TrackPoints[worldMap.TrackPoints.Count - 1]);
                                    }
                                }

                                #region Navigate to next point

                                //Invoke
                                this.Invoke((MethodInvoker)delegate()
                                {
                                    //Set desired coordinates
                                    //Set desired X
                                    outputBuffer[0] = 0xD0;
                                    outputBuffer[1] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.X) >> 8);
                                    outputBuffer[2] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.X) & 0x00FF);
                                    UARTsend(outputBuffer);

                                    //Set desired Y
                                    outputBuffer[0] = 0xD1;
                                    outputBuffer[1] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.Y) >> 8);
                                    outputBuffer[2] = (Byte)((Int32)(100 /*m to cm*/* worldMap.TrackPoints[worldMap.TrackPoints.Count - 1].Coordinates.Y) & 0x00FF);
                                    UARTsend(outputBuffer);

                                    //Set regulation flag
                                    wheelchairModel.IsRegulationActive = true;

                                    //Start regulation
                                    outputBuffer[0] = 0xC7;
                                    outputBuffer[1] = 0x00;
                                    outputBuffer[2] = 0x00;
                                    UARTsend(outputBuffer);
                                });

                                //Switch state and find marks again
                                wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.MARKS_TRACKING;

                                #endregion
                            }

                            break;

                        //End script
                        default:
                        case WheelchairModel.AutoParkingPhase.END:

                            //Invoke
                            this.Invoke((MethodInvoker)delegate()
                            {
                                buttonSTOP_Click(this, null);
                            });

                            return;
                    }

                    //Wait for regulation asks to end
                    while (wheelchairModel.IsRegulationActive)
                    {
                        Thread.Sleep(timerLog.Interval);
                    }

                    //Ligh up correct label
                    VisualizePhase(wheelchairModel.CurrentPhase, error);
                }
            }
            catch (ThreadAbortException)
            {
                //Restart algorithm
                wheelchairModel.CurrentPhase = WheelchairModel.AutoParkingPhase.WAITING_FOR_START;
                VisualizePhase(wheelchairModel.CurrentPhase, false);
            }
            catch (Exception)
            { /**/}
        }

        /// <summary>
        /// Com status changed event handler.
        /// </summary>
        private void FormMain_COMStatusChanged()
        {
            try
            {
                //Enable / disable all form's controls
                foreach (Control formControl in this.Controls)
                {
                    if (formControl is Button && (String)formControl.Tag == "ON_CLOSED_PORT_DISABLE_TAG")
                    {
                        if (this.InvokeRequired)
                        {
                            if (serialPortComPort.IsOpen)
                                this.Invoke((MethodInvoker)delegate() { formControl.Enabled = true; });
                            else
                                this.Invoke((MethodInvoker)delegate() { formControl.Enabled = false; });

                            continue;
                        }

                        if (serialPortComPort.IsOpen)
                            formControl.Enabled = true;
                        else
                            formControl.Enabled = false;
                    }
                }

                //Enable / disable auto parking start button
                if (this.InvokeRequired)
                {
                    if (serialPortComPort.IsOpen)
                        this.Invoke((MethodInvoker)delegate() { buttonSTARTAutoParking.Enabled = true; });
                    else
                        this.Invoke((MethodInvoker)delegate() { buttonSTARTAutoParking.Enabled = false; });
                }
            }
            catch (ObjectDisposedException)
            { /*Form main already disposed -> nothing to do*/}
            catch (Exception)
            { /*Do not process exceptions thrown in this section*/}
        }

        /// <summary>
        /// Handler called when application exits.
        /// </summary>
        /// <param name="sender">Sender.</param>
        /// <param name="e">Event arguments.</param>
        private void ApplicationExit_EventHandler(object sender, EventArgs e)
        {
            //Stop and dispose capture
            if (capture != null)
            {
                capture.Stop();
                capture.Dispose();
            }

            //Wait for UDPreceive thread to abort (just for sure)
            UDPreceiveThread.Abort();
            UDPreceiveThread.Join();

            //Wait for auto parking thread to abort (just for sure)
            if (AutoParkingThread != null)
            {
                AutoParkingThread.Abort();
                AutoParkingThread.Join();
            }
        }

        #endregion

        /// <summary>
        /// Form Main constructor.
        /// </summary>
        public FormMain()
        {
            InitializeComponent();

            //World map bitmap
            frameWorldMap = new Bitmap(pbWorldMap.Width, pbWorldMap.Height);

            //Fill serial ports
            foreach (string port in SerialPort.GetPortNames())
                comboBoxPortSelection.Items.Add(port);

            if (comboBoxPortSelection.Items.Count == 0)
            {
                comboBoxPortSelection.Items.Add("No port found");
                buttonOpenPort.Enabled = false;
            }

            //Select default comboboxes' items
            comboBoxMeanOfControl.SelectedIndex = (Int32)ComboboxControlIndexes.SPEED_REGULATOR;

            //Init com port guard thread
            threadCOMPortStatusGuard = new Thread(delegate() { COMPortStatusGuard_Method(this); });
            threadCOMPortStatusGuard.IsBackground = true;
            threadCOMPortStatusGuard.Start();

            //UDP cients init
            outgoingUDPClient = new UdpClient();
            incomingUDPClient = new UdpClient(ipEp);

            //Init user response auto reset event
            arUserResponseTimout = new AutoResetEvent(false);

            //Run UDP receiver thread
            UDPreceiveThread = new Thread(new ThreadStart(UDPDataReceiver));
            UDPreceiveThread.IsBackground = true;
            UDPreceiveThread.Priority = ThreadPriority.Normal;
            UDPreceiveThread.Start();

            //Register events
            COMStatusChanged += new COMPortStatusChangedDelegate(FormMain_COMStatusChanged);
            Application.ApplicationExit += ApplicationExit_EventHandler;
            capture.ImageGrabbed += ImageGrabbed_EventHandler;

            //Start capturing frames - captured frame handled by imageGrabbed event
            capture.Start();
        }

        #region Auto generated control events handlers

        private void comboBoxPortSelection_SelectedIndexChanged(object sender, EventArgs e)
        {
            //Try to change port name
            try
            {
                if (serialPortComPort.IsOpen == false)
                    serialPortComPort.PortName = comboBoxPortSelection.Text;
            }
            catch (Exception exc)
            {
                MessageBox.Show(exc.Message.ToString(), "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
            }
        }

        private void buttonOpenPort_Click(object sender, EventArgs e)
        {
            if (!serialPortComPort.IsOpen)
            {
                //Try to open the port
                try
                {
                    serialPortComPort.Open();
                }
                catch (Exception exc)
                {
                    MessageBox.Show(exc.Message.ToString(), "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
                }

                if (serialPortComPort.IsOpen)
                {
                    //Handle port controls
                    comboBoxPortSelection.Enabled = false;
                    labelI2CPortStatus.Text = comboBoxPortSelection.Text + " open";
                    buttonOpenPort.BackColor = Color.MediumSpringGreen;
                    buttonOpenPort.Text = "CLOSE";

                    //Enable START button + send stop command immediately
                    buttonSTOP_Click(this, null);

                    //Save port name for future use
                    WheelchairControl.Properties.Settings.Default.Setting_PortName = serialPortComPort.PortName;
                    WheelchairControl.Properties.Settings.Default.Save();
                }
                else
                {
                    labelI2CPortStatus.Text = comboBoxPortSelection.Text + " opening failed";
                }
            }
            else
            {
                //Stop motors first
                buttonSTOP_Click(this, null);

                serialPortComPort.Close();

                if (!serialPortComPort.IsOpen)
                {
                    //Handle port controls
                    comboBoxPortSelection.Enabled = true;
                    labelI2CPortStatus.Text = comboBoxPortSelection.Text + " closed";
                    buttonOpenPort.BackColor = Color.Red;
                    buttonOpenPort.Text = "OPEN";

                    //Disable START button
                    buttonSTART.Enabled = false;
                }
                else
                {
                    labelI2CPortStatus.Text = comboBoxPortSelection.Text + " closing failed";
                }
            }
        }

        private void buttonRefresh_Click(object sender, EventArgs e)
        {
            //Refresh available ports
            if (serialPortComPort.IsOpen == false)
            {
                comboBoxPortSelection.Items.Clear();

                foreach (string port in SerialPort.GetPortNames())
                    comboBoxPortSelection.Items.Add(port);

                if (comboBoxPortSelection.Items.Count == 0)
                {
                    comboBoxPortSelection.Items.Add("No port found");
                    buttonOpenPort.Enabled = false;
                }
                else
                {
                    buttonOpenPort.Enabled = true;
                }

                comboBoxPortSelection.SelectedIndex = 0;
            }
        }     

        private void buttonDelete_Click(object sender, EventArgs e)
        {
            if (MessageBox.Show("Do you really want to delete the log?", "Are you sure?", MessageBoxButtons.YesNo, MessageBoxIcon.Question) == DialogResult.Yes)
                textBoxData.Clear();
        }

        private void buttonReadData2_Click(object sender, EventArgs e)
        {
            Byte[] inBuffer = new Byte[12];
            Int32 temp;

            temp = UARTreceive(0xF2, ref inBuffer);

            if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK)      //Wrong data received
            {
                return;
            }

            textBoxData.AppendText(  Environment.NewLine + Environment.NewLine +
                                     "DATA 2 : Refreshed!" + Environment.NewLine + Environment.NewLine +
                                     "Max. Accel.: " + inBuffer[0].ToString() + Environment.NewLine +
                                     "Kp: " + inBuffer[1].ToString() + Environment.NewLine +
                                     "Kd: " + inBuffer[2].ToString() + Environment.NewLine +
                                     "Ki: " + inBuffer[3].ToString() + Environment.NewLine + Environment.NewLine +
                                     "Rounds [imp/50ms] of right wheel: " + ((SByte)inBuffer[4]).ToString() + Environment.NewLine +
                                     "Rounds [imp/50ms] of left wheel: " + ((SByte)inBuffer[5]).ToString() + Environment.NewLine +
                                     "Impulses of right wheel: " + (((SByte)inBuffer[6] << 8) + (inBuffer[7])).ToString() + Environment.NewLine +
                                     "Impulses of left wheel: " + (((SByte)inBuffer[8] << 8) + (inBuffer[9])).ToString() + Environment.NewLine + Environment.NewLine +
                                     "Speed regulator deadband: " + inBuffer[10].ToString() + Environment.NewLine);
        }

        private void buttonReadData1_Click(object sender, EventArgs e)
        {
            
            Byte[] inBuffer = new Byte[12];
            Int32 temp;

            temp = UARTreceive(0xF1, ref inBuffer);

            if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK)      //Wrong data received
            {
                return;
            }

            textBoxData.AppendText(  Environment.NewLine + Environment.NewLine +
                                     "DATA 1 : Refreshed!" + Environment.NewLine + Environment.NewLine +
                                     "SRF08 Front address: " + inBuffer[8].ToString("X") + Environment.NewLine +
                                     "SRF08 Rear address: " + inBuffer[9].ToString("X") + Environment.NewLine + Environment.NewLine +
                                     "Imp. per round right wheel: " + (2 * inBuffer[0]).ToString() + Environment.NewLine +
                                     "Imp. per round left wheel: " + (2 * inBuffer[1]).ToString() + Environment.NewLine +
                                     "Right wheel circumference [cm]: " + ((inBuffer[2] << 8) + (inBuffer[3])).ToString() + Environment.NewLine +
                                     "Left wheel circumference [cm]: " + ((inBuffer[4] << 8) + (inBuffer[5])).ToString() + Environment.NewLine +
                                     "Wheel track [cm]: " + ((inBuffer[6] << 8) + (inBuffer[7])).ToString() + Environment.NewLine + Environment.NewLine +
                                     "SRF08 rear sensor correction [cm]: " + ((SByte)inBuffer[10]).ToString() + Environment.NewLine);

        }

        private void buttonReadData3_Click(object sender, EventArgs e)
        {
            Byte[] inBuffer = new Byte[12];
            Int32 temp;

            temp = UARTreceive(0xF3, ref inBuffer);

            if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK)      //Wrong data received
            {
                return;
            }

            textBoxData.AppendText(  Environment.NewLine + Environment.NewLine +
                                     "DATA of the regulator and SRF08 : Refreshed!" + Environment.NewLine + Environment.NewLine +
                                     "Omega: " + inBuffer[0].ToString() + Environment.NewLine +
                                     "L: " + inBuffer[1].ToString() + Environment.NewLine +
                                     "Ksi: " + inBuffer[2].ToString() + Environment.NewLine +
                                     "Ky: " + (((SByte)inBuffer[3] << 8) + (inBuffer[4])).ToString() + Environment.NewLine +
                                     "Kfi: " + (((SByte)inBuffer[5] << 8) + (inBuffer[6])).ToString() + Environment.NewLine + Environment.NewLine +
                                     "Desired speed [cm/s]: " + (inBuffer[7] - 128).ToString() + Environment.NewLine +
                                     "Desired y0 [cm]: " + inBuffer[8].ToString() + Environment.NewLine + Environment.NewLine +
                                     "SRF08 front distance [cm]: " + inBuffer[9].ToString() + Environment.NewLine +
                                     "SRF08 rear distance [cm]: " + inBuffer[10].ToString() + Environment.NewLine);
        }

        private void buttonSTOP_Click(object sender, EventArgs e)
        {
            motorControlAllowed = false;

            buttonSTART.Enabled = true;

            //End auto parking thread
            if (AutoParkingThread != null)
                AutoParkingThread.Abort();

            SendStopSequence();

            //Stop log
            if (logWheelchairRunDataToFileToolStripMenuItem.Text == "Stop log")
                logWheelchairRunDataToFileToolStripMenuItem_Click(sender, null);
        }

        private void timerWatchdog_Tick(object sender, EventArgs e)
        {
                if (serialPortComPort.IsOpen)
                {
                    if (!timerLog.Enabled)  //If logging timer is enabled, MCU is polled periodically, no need to run this
                    {
                        UInt32[] tempData = new UInt32[RX_DATA_LENGTH - 1];

                        for (Byte i = 0; i < tempData.Length; i++)
                            tempData[i] = 0x00;

                        UARTsend(tempData);
                    }

                    //Blink label
                    if (ssLabelUARTWatchdog.BackColor == Color.LimeGreen)
                        ssLabelUARTWatchdog.BackColor = ssMain.BackColor;
                    else
                        ssLabelUARTWatchdog.BackColor = Color.LimeGreen;
                }
                else
                {
                    ssLabelUARTWatchdog.BackColor = this.BackColor;
                }
        }

        private void timerJoystick_Tick(object sender, EventArgs e)
        {
            if (comboBoxUserInput.SelectedIndex == (Int32)ComboboxUserInputIndexes.JOYSTICK && joystick != null)
            {
                Int32 stateX, stateY, stateZ;   //Joy axes' states

                joystick.GetData();

                //Read hw buttons
                if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_UPPER_TRIGGER_1))
                {
                    buttonSTOP_Click(joystick, null);
                }
                else if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_TRIGGER))
                {
                    buttonSTART_Click(joystick, null);
                }

                //Recalculate joystick data
                stateX = joystick.State.X;
                stateY = JOYSTICK_RANGE - joystick.State.Y;
                stateZ = JOYSTICK_RANGE - joystick.State.Z;

                //Determine if values are in threshold
                if (JoystickXinThreshold(stateX))
                {
                    stateX = JOYSTICK_RANGE / 2;
                }

                if (JoystickYinThreshold(stateY))
                {
                    stateY = JOYSTICK_RANGE / 2;
                }

                //Visualize values
                progressBarX.Value = stateX;
                progressBarY.Value = stateY;
                progressBarZ.Value = stateZ;

                labelX.Text = stateX.ToString();
                labelY.Text = stateY.ToString();
                labelZ.Text = stateZ.ToString();

                //Sned commands to motors
                if (motorControlAllowed)
                {
                    SendCommandsToMotors(stateX, stateY, stateZ);
                }
            }
            else if (comboBoxUserInput.SelectedIndex == (Int32)ComboboxUserInputIndexes.WIFI)
            {
                Int32 stateX, stateY, stateZ;   //Joy axes' states

                //Read joy's buttons and if any of them was clicked, switch control from wifi to joy
                if (joystick != null)
                {
                    joystick.GetData();

                    //Read hw buttons
                    if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_UPPER_TRIGGER_1))
                    {
                        buttonSTOP_Click(joystick, null);
                        comboBoxUserInput.SelectedIndex = (Byte)ComboboxUserInputIndexes.JOYSTICK;

                        return;
                    }
                    else if (joystick.ButtonClicked((Byte)USB_Joystick.JoystickButton.BUTTON_TRIGGER))
                    {
                        buttonSTART_Click(joystick, null);
                        comboBoxUserInput.SelectedIndex = (Byte)ComboboxUserInputIndexes.JOYSTICK;

                        return;
                    }
                }
                
                //Read daa from fake joystick
                stateX = wifiJoy.X;
                stateY = wifiJoy.Y;
                stateZ = wifiJoy.Z;

                //Check thresholds
                if (JoystickXinThreshold(stateX))
                {
                    stateX = JOYSTICK_RANGE / 2;
                }

                if (JoystickYinThreshold(stateY))
                {
                    stateY = JOYSTICK_RANGE / 2;
                }

                //Visualize data
                progressBarX.Value = stateX;
                progressBarY.Value = stateY;
                progressBarZ.Value = stateZ;

                labelX.Text = stateX.ToString();
                labelY.Text = stateY.ToString();
                labelZ.Text = stateZ.ToString();

                //Drive motors
                if (motorControlAllowed)
                {
                    SendCommandsToMotors(stateX, stateY, stateZ);
                }
            }
        }

        private void FormMain_Load(object sender, EventArgs e)
        {
            #region Intialize Joystick

            //Initialize joystick
            try
            {
                joystick = new USB_Joystick(0, JOYSTICK_RANGE, 0.2, this);
                joystick.PresetValues();
            }
            catch (Exception)
            {
                //Display warning
                buttonRedetectJoystick.Visible = true;

                MessageBox.Show("Failed to intialize joystick!", "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
            }

            #endregion

            #region Load settings

            //Load user input combobox's state
            comboBoxUserInput.SelectedIndex = (Int32)WheelchairControl.Properties.Settings.Default.Setting_UserInput;

            //Load joystick threshold
            numericUpDownThresholdPerc.Value = WheelchairControl.Properties.Settings.Default.Setting_JoystickThresh;

            //Load auto open enable checkbox's state
            checkBoxOpenPortOnStartup.Checked = WheelchairControl.Properties.Settings.Default.Setting_AutoOpenPort;

            //Try to open previously used port
            Int32 portNameIndex = comboBoxPortSelection.Items.IndexOf(WheelchairControl.Properties.Settings.Default.Setting_PortName);

            if (portNameIndex >= 0)
            {
                comboBoxPortSelection.SelectedIndex = portNameIndex;

                //Optionally open port
                if (checkBoxOpenPortOnStartup.Checked)
                    buttonOpenPort_Click(this, null);
            }

            //Try to load IP
            textBoxTargetIP.Text = WheelchairControl.Properties.Settings.Default.Setting_TargetIP;

            //Try to load log file default path
            labelLogFilePath.Text = WheelchairControl.Properties.Settings.Default.Setting_LogPath;

            //Load comboboxDetectionMethod's selected index
            comboBoxFeatureDetectionMethod.SelectedIndex = WheelchairControl.Properties.Settings.Default.Setting_DetectionMethod;

            //Try to load left model image, init feature detectors
            try
            {
                using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath))
                using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
                {
                    ibModelImageLeft.Image = modelImageBGR;

                    numericUpDownLeftModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);

                    featureDetectorLeft = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageLeft.Image.Bitmap), (Byte)numericUpDownLeftModelReferenceHue.Value);
                }
            }
            catch (Exception)
            {
                //Empty image
                using(Image<Bgr,Byte> errorImage = new Image<Bgr, Byte>(ibModelImageLeft.Size))
                {
                    ibModelImageLeft.Image = errorImage;
                    numericUpDownLeftModelReferenceHue.Value = (Byte)numericUpDownLeftModelReferenceHue.Minimum;
                    featureDetectorLeft = new FeatureDetector(errorImage, (Byte)numericUpDownLeftModelReferenceHue.Value);
                }
            }

            //Try to load right model image
            try
            {
                using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath))
                using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
                {
                    ibModelImageRight.Image = modelImageBGR;

                    numericUpDownRightModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);

                    featureDetectorRight = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageRight.Image.Bitmap), (Byte)numericUpDownRightModelReferenceHue.Value);
                }
            }
            catch (Exception)
            {
                //Empty image
                using (Image<Bgr, Byte> errorImage = new Image<Bgr, Byte>(ibModelImageRight.Size))
                {
                    ibModelImageRight.Image = errorImage;
                    numericUpDownRightModelReferenceHue.Value = (Byte)numericUpDownRightModelReferenceHue.Minimum;
                    featureDetectorRight = new FeatureDetector(errorImage, (Byte)numericUpDownRightModelReferenceHue.Value);
                }
            }

            //Load world map rendering option
            comboBoxRenderingMode.SelectedIndex = WheelchairControl.Properties.Settings.Default.Setting_WorldMapRenderingMode;
            numericUpDownWorldMapScale.Value = WheelchairControl.Properties.Settings.Default.Setting_WorldMapMaxScale;

            //Load world map scale
            tbWorldMapScale.Value = (Int32)WheelchairControl.Properties.Settings.Default.Setting_WorldMapScale;

            //Load wheelchair drawing settings
            checkBoxDrawWheelchair.Checked = WheelchairControl.Properties.Settings.Default.Setting_DrawWheelchair;

            //Load user confirmation checkbox state
            checkBoxUserResponseDemanded.Checked = WheelchairControl.Properties.Settings.Default.Setting_ConfirmMarksDetection;
            
            //Load watchdog loopback checkbox settings
            checkBoxWatchdogResponseEnabled.Checked = WheelchairControl.Properties.Settings.Default.Setting_WatchdogLoopbackEnabled;

            //Load jpeg compression level
            numericUpDownJpegCompression.Value = WheelchairControl.Properties.Settings.Default.Setting_JpegCompression;

            #endregion

            #region Other inits

            //Set marks center as reference point (default and recommended value)
            comboBoxCoordinatesReference.SelectedIndex = (Byte)MapReferenceCoordinates.MARKS_CENTER;

            //Load calibration coordinates
            LoadCalibrationCoordinates();
            
            //Clear log to delete logged events caused by application start
            textBoxData.Clear();

            #endregion
        }

        private void buttonSTART_Click(object sender, EventArgs e)
        {
            motorControlAllowed = true;

            buttonSTART.Enabled = false;
        }

        private void comboBoxMeanOfControl_SelectedIndexChanged(object sender, EventArgs e)
        {
            //Stop motors
            buttonSTOP_Click(sender, e);

            //Toggle menu items
            switch (comboBoxMeanOfControl.SelectedIndex)
            {
                default:

                    feedforwardMomentControlToolStripMenuItem.Checked = false;
                    speedRegulatorToolStripMenuItem.Checked = false;

                    break;

                case (Int32)ComboboxControlIndexes.MOMENT:

                    feedforwardMomentControlToolStripMenuItem.Checked = true;
                    speedRegulatorToolStripMenuItem.Checked = false;

                    break;

                case (Int32)ComboboxControlIndexes.SPEED_REGULATOR:

                    feedforwardMomentControlToolStripMenuItem.Checked = false;
                    speedRegulatorToolStripMenuItem.Checked = true;

                    break;
            }
        }

        private void buttonReadOdometryData_Click(object sender, EventArgs e)
        {
            Byte[] inBuffer = new Byte[12];
            Int32 temp;

            temp = UARTreceive(0xF0, ref inBuffer);

            if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK)      //Wrong data received
            {
                return;
            }

            textBoxData.AppendText(Environment.NewLine + Environment.NewLine +
                                     "DATA of odomety : Refreshed!" + Environment.NewLine + Environment.NewLine +
                                     "X: " + (((SByte)inBuffer[0] << 8) + (inBuffer[1])).ToString() + " [cm]" + Environment.NewLine +
                                     "Y: " + (((SByte)inBuffer[2] << 8) + (inBuffer[3])).ToString() + " [cm]" + Environment.NewLine +
                                     "Phi: " + (((SByte)inBuffer[4] << 8) + (inBuffer[5])).ToString() + " [mrad] = " + ((Double)((180/(1000 * Math.PI))*(((SByte)inBuffer[4] << 8) + (inBuffer[5])))).ToString("F2") + "°" + Environment.NewLine + Environment.NewLine +
                                     "SRF08 front distance: " + inBuffer[6].ToString() + " [cm]" + Environment.NewLine +
                                     "SRF08 rear distance: " + inBuffer[7].ToString() + " [cm]" + Environment.NewLine + Environment.NewLine +
                                     "Position regulator enabled: " + (inBuffer[8] == 0 ? "False" : "True") + Environment.NewLine +
                                     "Wall-following enabled: " + (inBuffer[9] == 0 ? "False" : "True") + Environment.NewLine);           
        }

        private void timerLog_Tick(object sender, EventArgs e)
        {
            Byte[] inBuffer = new Byte[12];
            Int32 temp;

            //Acquire data
            temp = UARTreceive(0xF0, ref inBuffer);

            if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK)      //Wrong data received
            {
                ssLabelMCUPoll.BackColor = Color.Red;

                return;
            }

            try
            {
                //Read phi and convert it to +-180° range
                double phi = (double)((((SByte)inBuffer[(Byte)OdometryPacketByte.PHI_BASE] << 8) + inBuffer[(Byte)OdometryPacketByte.PHI_BASE + 1]) / 1000.0);
                phi -= (2 * Math.PI * Math.Floor((phi + Math.PI) / (2 * Math.PI)));
                phi *= (180.0 / Math.PI);

                if (swLogWriter != null)
                {
                    swLogWriter.WriteLine((System.DateTime.Now - swLogStartTime).Seconds.ToString()
                                            + "." + (System.DateTime.Now - swLogStartTime).Milliseconds.ToString("D3") + ";"
                                            + inBuffer[(Byte)OdometryPacketByte.SRF08_FRONT_DIST].ToString() + ";"         //Front SRF08 distance
                                            + inBuffer[(Byte)OdometryPacketByte.SRF08_REAR_DIST].ToString() + ";"     //Rear SRF08 distance
                                           + (((SByte)inBuffer[(Byte)OdometryPacketByte.X_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.X_BASE + 1])).ToString() + ";"     //X
                                            + (((SByte)inBuffer[(Byte)OdometryPacketByte.Y_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.Y_BASE + 1])).ToString() + ";"    //Y
                                            + ((Int16)Math.Truncate(phi)).ToString());        //Phi

                    swLogWriter.Flush();

                    //Blink the logging button - symbolize received data
                    if (logWheelchairRunDataToFileToolStripMenuItem.BackColor == SystemColors.Control)
                        logWheelchairRunDataToFileToolStripMenuItem.BackColor = Color.LimeGreen;
                    else
                        logWheelchairRunDataToFileToolStripMenuItem.BackColor = SystemColors.Control;
                }

                //Actualize wheelchair model coordinates
                wheelchairModel.SetCoordinates((((SByte)inBuffer[(Byte)OdometryPacketByte.X_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.X_BASE + 1])),
                                                (((SByte)inBuffer[(Byte)OdometryPacketByte.Y_BASE] << 8) + (inBuffer[(Byte)OdometryPacketByte.Y_BASE + 1])),
                                                (((SByte)inBuffer[(Byte)OdometryPacketByte.PHI_BASE] << 8) + inBuffer[(Byte)OdometryPacketByte.PHI_BASE + 1]));

                //Stop motors if regulation is done
                if (inBuffer[(Byte)OdometryPacketByte.ENABLE_POSITION_REGULATOR] == 0 && inBuffer[(Byte)OdometryPacketByte.ENABLE_WALL_FOLLOWING] == 0)   //Read status bytes
                {
                    SendStopSequence();

                    //Reset flag
                    wheelchairModel.IsRegulationActive = false;
                }
                else
                {
                    //Set flag
                    wheelchairModel.IsRegulationActive = true;
                }
            }
            catch (Exception exc)
            {
                textBoxData.AppendText("TimerLog: " + exc.Message);

                return;
            }

            //Blink timer active label
            if (ssLabelMCUPoll.BackColor == Color.LimeGreen)
                ssLabelMCUPoll.BackColor = ssMain.BackColor;
            else
                ssLabelMCUPoll.BackColor = Color.LimeGreen;
        }

        private void buttonResetCoor_Click(object sender, EventArgs e)
        {
            UInt32[] outputBuffer = new UInt32[RX_DATA_LENGTH - 1];

            //Stop motors -> position regulator could start the wheelchair if odometry is reset
            buttonSTOP_Click(this, null);

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

        private void buttonShowFormManualControl_Click(object sender, EventArgs e)
        {
            //Stop motors
            buttonSTOP_Click(this, null);

            //Show form, pass common objects, set focus on form
            FormManualControl.GetInstance(serialPortComPort, UARTsend).Show();
            FormManualControl.GetInstance(serialPortComPort, UARTsend).BringToFront();
        }

        private void buttonShowFormSettings_Click(object sender, EventArgs e)
        {
            //Stop motors
            buttonSTOP_Click(this, null);

            //Show form, pass common objects, set focus on form
            FormSettings.GetInstance(serialPortComPort, UARTsend).Show();
            FormSettings.GetInstance(serialPortComPort, UARTsend).BringToFront();
        }

        private void btnShowFormRegulators_Click(object sender, EventArgs e)
        {
            //Stop motors
            buttonSTOP_Click(this, null);

            //Show form, pass common objects, set focus on form
            FormRegulators.GetInstance(serialPortComPort, UARTsend,logWheelchairRunDataToFileToolStripMenuItem_Click,logWheelchairRunDataToFileToolStripMenuItem).Show();
            FormRegulators.GetInstance(serialPortComPort, UARTsend, logWheelchairRunDataToFileToolStripMenuItem_Click, logWheelchairRunDataToFileToolStripMenuItem).BringToFront();
        }

        private void buttonOpenPort_EnabledChanged(object sender, EventArgs e)
        {
            if (!buttonOpenPort.Enabled)
                buttonOpenPort.BackColor = SystemColors.Control;
            else
                buttonOpenPort.BackColor = Color.Red;
        }

        private void buttonRedetectJoystick_Click(object sender, EventArgs e)
        {
            try     //Try to initialize joystick
            {
                joystick = new USB_Joystick(0, JOYSTICK_RANGE, 0.2, this);
                joystick.PresetValues();

                buttonRedetectJoystick.Visible = false;
            }
            catch (Exception)
            {

                MessageBox.Show("Failed to intialize joystick!", "Warning!", MessageBoxButtons.OK, MessageBoxIcon.Warning);
            }
        }

        private void checkBoxOpenPortOnStartup_CheckedChanged(object sender, EventArgs e)
        {
            //Save checkbox's status
            WheelchairControl.Properties.Settings.Default.Setting_AutoOpenPort = checkBoxOpenPortOnStartup.Checked;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void buttonReadRegulatorSettings_Click(object sender, EventArgs e)
        {
            Byte[] inBuffer = new Byte[12];
            Int32 temp;

            temp = UARTreceive(0xF4, ref inBuffer);

            if (temp != (Byte)RxReturnCodes.TRANSMISSION_OK)      //Wrong data received
            {
                return;
            }

            textBoxData.AppendText(Environment.NewLine + Environment.NewLine +
                                     "DATA of position regulator : Refreshed!" + Environment.NewLine + Environment.NewLine +
                                     "X desired: " + (((SByte)inBuffer[0] << 8) + (inBuffer[1])).ToString() + " [cm]" + Environment.NewLine +
                                     "Y desired: " + (((SByte)inBuffer[2] << 8) + (inBuffer[3])).ToString() + " [cm]" + Environment.NewLine +
                                     "Gamma: " + ((Byte)inBuffer[4]).ToString() + Environment.NewLine +
                                     "Epsilon: " + ((Byte)inBuffer[5]).ToString() + " [cm]" + Environment.NewLine +
                                     "Angle gain: " + ((Byte)inBuffer[6]).ToString() + Environment.NewLine +
                                     "Position gain: " + ((Byte)inBuffer[7]).ToString() + Environment.NewLine +
                                     "Constant speed: " + ((Byte)inBuffer[8]).ToString() + " [cmps]" + Environment.NewLine);
                                     
        }

        private void buttonRedetectJoystick_VisibleChanged(object sender, EventArgs e)
        {
            //Toggle main menu item enable property
            reconnectJoystickToolStripMenuItem.Enabled = buttonRedetectJoystick.Visible;
        }

        private void buttonSTOP_EnabledChanged(object sender, EventArgs e)
        {
            //Toggle main menu item enable property
            stopToolStripMenuItem.Enabled = buttonSTOP.Enabled;
        }

        private void buttonSTART_EnabledChanged(object sender, EventArgs e)
        {
            //Toggle main menu item enable property
            startToolStripMenuItem.Enabled = buttonSTART.Enabled;
        }

        private void exitToolStripMenuItem1_Click(object sender, EventArgs e)
        {
            //Exit with native exit code
            Application.Exit();
        }

        private void aboutToolStripMenuItem_Click(object sender, EventArgs e)
        {
            //Show infobox
            MessageBox.Show("Application:\tWheelchair control\n\t\tDiploma thesis\n\nAuthor:\t\tOndrej Vozda\nUniversity:\t\tFEEC BUT\nYear:\t\t2013", 
                "About", MessageBoxButtons.OK, MessageBoxIcon.Information);
        }

        private void feedforwardMomentControlToolStripMenuItem_Click(object sender, EventArgs e)
        {
            //Switch indexes
            comboBoxMeanOfControl.SelectedIndex = (Int32)ComboboxControlIndexes.MOMENT;
        }

        private void speedRegulatorToolStripMenuItem_Click(object sender, EventArgs e)
        {
            //Switch indexes
            comboBoxMeanOfControl.SelectedIndex = (Int32)ComboboxControlIndexes.SPEED_REGULATOR;
        }

        private void numericUpDownThresholdPerc_ValueChanged(object sender, EventArgs e)
        {
            //Update menu item
            joystickThresholdToolStripMenuItem.Text = "Joystick threshold: " + numericUpDownThresholdPerc.Value.ToString() +"%";

            //Save setting
            WheelchairControl.Properties.Settings.Default.Setting_JoystickThresh = (Byte)numericUpDownThresholdPerc.Value;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void buttonSaveLogToFile_Click(object sender, EventArgs e)
        {
            SaveFileDialog sfDialog = new SaveFileDialog();
            TextWriter writer = null;

            //Some initial settings here
            sfDialog.AddExtension = true;
            sfDialog.InitialDirectory = AppDomain.CurrentDomain.BaseDirectory;
            sfDialog.DefaultExt = "txt";
            sfDialog.Filter = "Text file (*.txt)|*.txt";
            sfDialog.FileName = "Log " + System.DateTime.Now.Hour.ToString("D2") + "_" + System.DateTime.Now.Minute.ToString("D2") + "_" + System.DateTime.Now.Second.ToString("D2");

            //Exit if OK button is not clicked
            if (sfDialog.ShowDialog() != DialogResult.OK)  
                return;

            //Init writer
            try
            {
                //Don't append, overwrite file
                writer = new StreamWriter(sfDialog.FileName,false);

                //Write to file
                foreach (String line in textBoxData.Lines)
                {
                    writer.WriteLine(line);
                }

                //Inform user
                MessageBox.Show("Log " + sfDialog.FileName + " saved!", "Success!", MessageBoxButtons.OK, MessageBoxIcon.Information);
            }
            catch(Exception)
            {
                //Error, show messagebox
                MessageBox.Show("Error, file not saved!", "Error!", MessageBoxButtons.AbortRetryIgnore, MessageBoxIcon.Error);
            }
            finally
            {
                //Close the file in any case
                if(writer != null)
                    writer.Close();
            }
        }

        private void comboBoxUserInput_SelectedIndexChanged(object sender, EventArgs e)
        {
            //Erase labels and stop the motors for sure
            progressBarX.Value = 0;
            progressBarY.Value = 0;
            progressBarZ.Value = 0;

            labelX.Text = "-";
            labelY.Text = "-";
            labelZ.Text = "-";

            buttonSTOP_Click(sender, e);

            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_UserInput = (Byte)comboBoxUserInput.SelectedIndex;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void buttonRefreshLocalIP_Click(object sender, EventArgs e)
        {
            //Get ip
            IPHostEntry host = Dns.GetHostEntry(Dns.GetHostName());

            foreach (IPAddress ip in host.AddressList)
                if (ip.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
                    labelLocalIP.Text = ip.ToString();
        }

        private void tabPageSettings_Enter(object sender, EventArgs e)
        {
            //Try to get local IP
            buttonRefreshLocalIP_Click(sender, e);
        }

        private void textBoxTargetIP_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyCode == Keys.Enter)
            {
                IPAddress targetIP;

                //Save the new IP, if it is valid
                if (IPAddress.TryParse(textBoxTargetIP.Text, out targetIP))
                {
                    WheelchairControl.Properties.Settings.Default.Setting_TargetIP = targetIP.ToString();

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

                    MessageBox.Show("IP address " + targetIP.ToString() + " saved!", "Success!", MessageBoxButtons.OK, MessageBoxIcon.Information);
                }
                else
                {
                    MessageBox.Show("IP not valid!", "Error!", MessageBoxButtons.OK, MessageBoxIcon.Error);
                }
            }
        }

        private void collapsibleGroupBoxWifiSettings_CollapseBoxClickedEvent(object sender)
        {
            const UInt16 COMBOBOXES_DISTANCE = 17;

            //Move COM settings group box
            collapsibleGroupBoxCOM.Top = collapsibleGroupBoxWifiSettings.Top + collapsibleGroupBoxWifiSettings.Height + COMBOBOXES_DISTANCE;
        }

        private void pbFrame_MouseClick(object sender, MouseEventArgs e)
        {
            //Switch tabs
            //Marks' reference selection
            if (tcActionSwitch.SelectedTab == tabPageSelectColor)
            {
                SelectColorMouseClickRoutine(e);

                return;
            }

            //Camera calibration points selection
            if (tcActionSwitch.SelectedTab == tabPageCalibrate)
            {
                CalibratePointsMouseClickRoutine(e);

                return;
            }

            //Other tabs can be added here
        }

        private void pbFrame_MouseMove(object sender, MouseEventArgs e)
        {
            Point cursorCoord = new Point(e.X, e.Y);
            PointF worldCoord = cameraCalibrator.TransformPoint(cursorCoord);

            //Write coordinates to status bar
            ssMainLabelPositionXY.Text = "X: " + e.X.ToString() + " Y: " + e.Y.ToString();
            ssMainLabelWorldXY.Text = "X: " + worldCoord.X.ToString("F3") + " Y: " + worldCoord.Y.ToString("F3");

            //Change cursor
            Cursor = Cursors.Cross;
        }

        private void pbFrame_MouseLeave(object sender, EventArgs e)
        {
            ssMainLabelPositionXY.Text = ssMainLabelWorldXY.Text = "X: --- Y: ---";

            Cursor = Cursors.Default;
        }

        private void pbWorldMap_MouseMove(object sender, MouseEventArgs e)
        {
            //Create data array from current mouse point
            Double[,] data = new Double[3, 1]{  {(Double)e.X},  //X
                                            {(Double)e.Y},  //Y
                                            {1.0}           //Scale
                                         };

            //Matrix from data
            Matrix<Double> matrixTemp = new Matrix<double>(data);

            //Transform vector using homography matrix
            matrixTemp = homMatWorldMapToWorld.Mul(matrixTemp);

            //Write coordinates to status bar
            ssMainLabelPositionXY.Text = "X: " + e.X.ToString() + " Y: " + e.Y.ToString();

            if(comboBoxCoordinatesReference.SelectedIndex == (Byte)MapReferenceCoordinates.WHEELCHAIR_CENTER)
            {
                //Coordinates' reference point is in center of the wheelchair
                ssMainLabelWorldXY.Text = "X: " + matrixTemp[0, 0].ToString("F3") + " Y: " + matrixTemp[1, 0].ToString("F3");
            }
            else
            {
                //Do the recalculations first
                Double xTemp = matrixTemp[0, 0];
                Double yTemp = matrixTemp[1, 0];
                Double phiTemp;

                if (RecalculateRefPointToMarksCenter(ref xTemp, ref yTemp, out phiTemp))
                {
                    //Display coordinates
                    ssMainLabelWorldXY.Text = "X: " + xTemp.ToString("F3") + " Y: " + yTemp.ToString("F3") + " Phi: " + (phiTemp / 1000.0).ToString("F3") + " rad ~ " + (180 * phiTemp / (1000 * Math.PI)).ToString("F2") + "°";
                }
                else
                {
                    //Error - display default label text
                    ssMainLabelPositionXY.Text = ssMainLabelWorldXY.Text = "X: --- Y: ---";
                }
            }
        }

        private void pbWorldMap_MouseLeave(object sender, EventArgs e)
        {
            ssMainLabelPositionXY.Text = ssMainLabelWorldXY.Text = "X: --- Y: ---";
        }

        private void tabPageLog_Enter(object sender, EventArgs e)
        {
            //Enable / disable buttons according to port status
            try
            {
                foreach (Control tabControl in tabPageLog.Controls)
                {
                    if (tabControl is Button && (String)tabControl.Tag == "ON_CLOSED_PORT_DISABLE_TAG")
                    {
                        if (this.InvokeRequired)
                        {
                            if (serialPortComPort.IsOpen)
                                this.Invoke((MethodInvoker)delegate() { tabControl.Enabled = true; });
                            else
                                this.Invoke((MethodInvoker)delegate() { tabControl.Enabled = false; });

                            continue;
                        }

                        if (serialPortComPort.IsOpen)
                            tabControl.Enabled = true;
                        else
                            tabControl.Enabled = false;
                    }
                }
            }
            catch (ObjectDisposedException)
            { /*Form main already disposed -> nothing to do*/}
        }

        private void cameraToolStripMenuItem_Click(object sender, EventArgs e)
        {
            //Switch tab
            tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.CAMERA;
        }

        private void settingsToolStripMenuItem2_Click(object sender, EventArgs e)
        {
            //Switch tab
            tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.SETTINGS;
        }

        private void logToolStripMenuItem_Click(object sender, EventArgs e)
        {
            //Switch tab
            tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.LOG;
        }

        private void tabControlMain_SelectedIndexChanged(object sender, EventArgs e)
        {
            //Disable all first, just for sure
            cameraToolStripMenuItem.Checked = false;
            settingsToolStripMenuItem2.Checked = false;
            logToolStripMenuItem.Checked = false;

            toolStripButtonCamera.Checked = false;
            toolStripButtonSettings.Checked = false;
            toolStripButtonLog.Checked = false;

            //Enable correct ones
            switch (tabControlMain.SelectedIndex)
            { 
                default: //Camera -> default tab
                case (Int32)MainPageTabsIndexes.CAMERA:

                    cameraToolStripMenuItem.Checked = true;
                    toolStripButtonCamera.Checked = true;

                    break;

                case (Int32)MainPageTabsIndexes.SETTINGS:

                    settingsToolStripMenuItem2.Checked = true;
                    toolStripButtonSettings.Checked = true;

                    break;

                case (Int32)MainPageTabsIndexes.LOG:

                    logToolStripMenuItem.Checked = true;
                    toolStripButtonLog.Checked = true;

                    break;
            }
        }

        private void toolStripButtonCamera_Click(object sender, EventArgs e)
        {
            //Switch tab
            tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.CAMERA;
        }

        private void toolStripButtonSettings_Click(object sender, EventArgs e)
        {
            //Switch tab
            tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.SETTINGS;
        }

        private void toolStripButtonLog_Click(object sender, EventArgs e)
        {
            //Switch tab
            tabControlMain.SelectedIndex = (Int32)MainPageTabsIndexes.LOG;
        }

        private void buttonCalibrateHomography_Click(object sender, EventArgs e)
        {
            //Calculate homography using defined points
            cameraCalibrator.CalculateHomography();
        }

        private void buttonLockMarks_Click(object sender, EventArgs e)
        {
            //Run method in separate thread 
            new Thread(delegate() { MarkLock_ThreadRoutine(false); }).Start();
        }

        private void firstToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates

            tcActionSwitch.SelectedTab = tabPageSelectColor;

            rbSelectColor1.Checked = true;

            pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
        }

        private void secondToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates

            tcActionSwitch.SelectedTab = tabPageSelectColor;

            rbSelectColor2.Checked = true;

            pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
        }

        private void upperLeftToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates

            tcActionSwitch.SelectedTab = tabPageCalibrate;

            rbUL.Select();

            pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
        }

        private void upperRightToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates

            tcActionSwitch.SelectedTab = tabPageCalibrate;

            rbUR.Select();

            pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
        }

        private void lowerRightToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates

            tcActionSwitch.SelectedTab = tabPageCalibrate;

            rbLR.Select();

            pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
        }

        private void lowerLeftToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Point pbFramePoint = pbFrame.PointToClient(new Point(cmCameraFrame.Left, cmCameraFrame.Top)); //Racalculate point to pbFrame coordinates

            tcActionSwitch.SelectedTab = tabPageCalibrate;

            rbLL.Select();

            pbFrame_MouseClick(sender, new MouseEventArgs(MouseButtons.Left, 1, pbFramePoint.X, pbFramePoint.Y, 0));
        }

        private void deleteLogToolStripMenuItem_Click(object sender, EventArgs e)
        {
            buttonDelete_Click(sender, e);
        }

        private void saveLogToFileToolStripMenuItem_Click(object sender, EventArgs e)
        {
            buttonSaveLogToFile_Click(sender, e);
        }

        private void readConfig1ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
            {
                //Switch tabs and redraw tab
                tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
                tabPageLog.Refresh();
            }

            buttonReadData1_Click(sender, e);
        }

        private void readConfig2ToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
            {
                //Switch tabs and redraw tab
                tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
                tabPageLog.Refresh();
            }

            buttonReadData2_Click(sender, e);
        }

        private void readWallfollowingRegulatorSettingsToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
            {
                //Switch tabs and redraw tab
                tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
                tabPageLog.Refresh();
            }

            buttonReadData3_Click(sender, e);
        }

        private void readOdometryDataToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
            {
                //Switch tabs and redraw tab
                tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
                tabPageLog.Refresh();
            }

            buttonReadOdometryData_Click(sender, e);
        }

        private void readPositionRegulatorDataToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (tabControlMain.SelectedIndex != (Byte)MainPageTabsIndexes.LOG)
            {
                //Switch tabs and redraw tab
                tabControlMain.SelectedIndex = (Byte)MainPageTabsIndexes.LOG;
                tabPageLog.Refresh();
            }

            buttonReadRegulatorSettings_Click(sender, e);
        }

        private void logWheelchairRunDataToFileToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (logWheelchairRunDataToFileToolStripMenuItem.Text == "Log to file")
            {
                if (!serialPortComPort.IsOpen)
                {
                    textBoxData.AppendText("Port closed, log failed!" + Environment.NewLine);

                    return;
                }

                try
                {
                //Start new log
                String path = @labelLogFilePath.Text + @"\" + System.DateTime.Today.Year.ToString("D4") + "-" + System.DateTime.Today.Month.ToString("D2") + "-"
                    + System.DateTime.Today.Day.ToString("D2") + "__" + System.DateTime.Now.Hour.ToString("D2") + "-"
                    + System.DateTime.Now.Minute.ToString("D2") + "-" + System.DateTime.Now.Second.ToString("D2") + ".csv";
                
                swLogWriter = new StreamWriter(path, false, Encoding.ASCII);

                //Write log start time
                swLogStartTime = DateTime.Now;

                //Enable logging timer
                timerLog.Enabled = true;

                logWheelchairRunDataToFileToolStripMenuItem.Text = "Stop log";
                }
                catch (IOException)
                {
                    textBoxData.AppendText("Failed to initialize new log\n");
                }
            }
            else
            {
                //Disable timer
                timerLog.Enabled = false;

                logWheelchairRunDataToFileToolStripMenuItem.Text = "Log to file";

                logWheelchairRunDataToFileToolStripMenuItem.BackColor = SystemColors.Control;

                ssLabelMCUPoll.BackColor = SystemColors.Control;

                //Close log
                try
                {
                    swLogWriter.Close();
                }
                catch (IOException)
                {
                    textBoxData.AppendText("Failed to close log\n");
                }
            }
        }

        private void logWheelchairRunDataToFileToolStripMenuItem_BackColorChanged(object sender, EventArgs e)
        {
            //Blink status label
            ssLabelLogToFile.BackColor = logWheelchairRunDataToFileToolStripMenuItem.BackColor;
        }

        private void collapsibleGroupBoxInputSettings_CollapseBoxClickedEvent(object sender)
        {
            const UInt16 COMBOBOXES_DISTANCE = 17;

            //Move log settings group box
            collapsibleGroupBoxLogSettings.Top = collapsibleGroupBoxInputSettings.Top + collapsibleGroupBoxInputSettings.Height + COMBOBOXES_DISTANCE;
        }

        private void labelLogFilePath_TextChanged(object sender, EventArgs e)
        {
            //Copy text to toolStripTextBox
            toolStripTextBoxLogRunDataFilePath.Text = labelLogFilePath.Text;
        }

        private void buttonChangeLogPath_Click(object sender, EventArgs e)
        {
            FolderBrowserDialog fbDialog = new FolderBrowserDialog();

            fbDialog.SelectedPath = @labelLogFilePath.Text;

            if (fbDialog.ShowDialog() == DialogResult.OK)
            {
                //Display path
                labelLogFilePath.Text = fbDialog.SelectedPath;

                //Save to settings
                WheelchairControl.Properties.Settings.Default.Setting_LogPath = fbDialog.SelectedPath;
                WheelchairControl.Properties.Settings.Default.Save();
            }
        }

        private void comboBoxFeatureDetectionMethod_SelectedIndexChanged(object sender, EventArgs e)
        {
            //Switch tabs
            switch (comboBoxFeatureDetectionMethod.SelectedIndex)
            { 
                default:
                case (Byte)ComboboxDetectionMethodIndexes.REFERENCE_HSV:

                    wizardTabControlDetectionMethod.SelectedTab = tabPageHSV;

                    break;

                case (Byte)ComboboxDetectionMethodIndexes.SURF_DETECTOR:

                    wizardTabControlDetectionMethod.SelectedTab = tabPageSurfDetector;

                    break;            
            }

            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_DetectionMethod = (Byte)comboBoxFeatureDetectionMethod.SelectedIndex;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void buttonLoadLeftModelImage_Click(object sender, EventArgs e)
        {
            OpenFileDialog ofDialog = new OpenFileDialog();

            try
            {
                ofDialog.InitialDirectory = @WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath;
            }
            catch(Exception)
            { ofDialog.InitialDirectory = Environment.CurrentDirectory; }

            if (ofDialog.ShowDialog() == DialogResult.OK)
            {
                try
                {
                    //Load image
                    using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@ofDialog.FileName))
                    using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
                    {
                        ibModelImageLeft.Image = modelImageBGR;

                        numericUpDownLeftModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);

                        featureDetectorLeft = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageLeft.Image.Bitmap), (Byte)numericUpDownLeftModelReferenceHue.Value);
                    }

                    //Save settings
                    WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath = ofDialog.FileName;
                    WheelchairControl.Properties.Settings.Default.Save();
                }
                catch (Exception)
                {
                    //Empty image
                    using (Image<Bgr, Byte> errorImage = new Image<Bgr, Byte>(ibModelImageLeft.Size))
                    {
                        ibModelImageLeft.Image = errorImage;
                        numericUpDownLeftModelReferenceHue.Value = (Byte)numericUpDownLeftModelReferenceHue.Minimum;
                        featureDetectorLeft = new FeatureDetector(errorImage, (Byte)numericUpDownLeftModelReferenceHue.Value);
                    }
                }
            }
        }

        private void buttonLoadRightModelImage_Click(object sender, EventArgs e)
        {
            OpenFileDialog ofDialog = new OpenFileDialog();

            try
            {
                ofDialog.InitialDirectory = @WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath;
            }
            catch (Exception)
            { ofDialog.InitialDirectory = Environment.CurrentDirectory; }

            if (ofDialog.ShowDialog() == DialogResult.OK)
            {
                try
                {
                    //Load image
                    using (Bitmap modelImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath))
                    using (Image<Bgr, Byte> modelImageBGR = new Image<Bgr, Byte>(modelImage))
                    {
                        ibModelImageRight.Image = modelImageBGR;

                        numericUpDownRightModelReferenceHue.Value = AutoDetectReferenceHue(modelImageBGR);

                        featureDetectorRight = new FeatureDetector(new Image<Bgr, Byte>(ibModelImageRight.Image.Bitmap), (Byte)numericUpDownRightModelReferenceHue.Value);
                    }

                    //Save settings
                    WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath = ofDialog.FileName;
                    WheelchairControl.Properties.Settings.Default.Save();
                }
                catch (Exception)
                {
                    //Empty image
                    using (Image<Bgr, Byte> errorImage = new Image<Bgr, Byte>(ibModelImageRight.Size))
                    {
                        ibModelImageRight.Image = errorImage;
                        numericUpDownRightModelReferenceHue.Value = (Byte)numericUpDownRightModelReferenceHue.Minimum;
                        featureDetectorRight = new FeatureDetector(errorImage, (Byte)numericUpDownRightModelReferenceHue.Value);
                    }
                }
            }
        }

        private void toolStripMenuItemRed_Click(object sender, EventArgs e)
        {
            //Determine which numericUpDown sent the menu request and change its value
            try
            {
                ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
                ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
                NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;

                nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Red;
            }
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
        }

        private void toolStripMenuItemGreen_Click(object sender, EventArgs e)
        {
            //Determine which numericUpDown sent the menu request and change its value
            try
            {
                ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
                ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
                NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;

                nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Green;
            }
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
        }

        private void toolStripMenuItemBlue_Click(object sender, EventArgs e)
        {
            //Determine which numericUpDown sent the menu request and change its value
            try
            {
                ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
                ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
                NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;

                nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Blue;
            }
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
        }

        private void toolStripMenuItemCyan_Click(object sender, EventArgs e)
        {
            //Determine which numericUpDown sent the menu request and change its value
            try
            {
                ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
                ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
                NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;

                nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Cyan;
            }
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
        }

        private void toolStripMenuItemMagenta_Click(object sender, EventArgs e)
        {
            //Determine which numericUpDown sent the menu request and change its value
            try
            {
                ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
                ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
                NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;

                nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Magenta;
            }
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
        }

        private void toolStripMenuItemYellow_Click(object sender, EventArgs e)
        {
            //Determine which numericUpDown sent the menu request and change its value
            try
            {
                ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
                ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
                NumericUpDown nudSender = cmStrip.SourceControl as NumericUpDown;
                
                nudSender.Value = (Byte)MyImageProcessingClasses.FeatureDetector.RefrenceHue.Yellow;
            }
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
        }

        private void numericUpDownLeftModelReferenceHue_ValueChanged(object sender, EventArgs e)
        {
            //Try to create new feature detector
            try
            {
                using (Bitmap ibImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_LeftModelImagePath))
                    featureDetectorLeft = new FeatureDetector(new Image<Bgr, Byte>(ibImage), (Byte)numericUpDownLeftModelReferenceHue.Value);
            }
            catch (Exception)
            {
                //If feature detector was damaged, inform user
                if (featureDetectorLeft == null)
                    MessageBox.Show("Feature detector error, please reload the model image or restart the application!", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }
        }

        private void numericUpDownRightModelReferenceHue_ValueChanged(object sender, EventArgs e)
        {
            //Try to create new feature detector
            try
            {
                using (Bitmap ibImage = (Bitmap)Bitmap.FromFile(@WheelchairControl.Properties.Settings.Default.Setting_RightModelImagePath))
                    featureDetectorRight = new FeatureDetector(new Image<Bgr, Byte>(ibImage), (Byte)numericUpDownRightModelReferenceHue.Value);
            }
            catch (Exception)
            {
                //If feature detector was damaged, inform user
                if (featureDetectorRight == null)
                    MessageBox.Show("Feature detector error, please reload the model image or restart the application!", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }
        }

        private void numericUpDownWorldMapScale_ValueChanged(object sender, EventArgs e)
        {
            //Change range
            tbWorldMapScale.Maximum = (Int32)numericUpDownWorldMapScale.Value;

            //Save values
            WheelchairControl.Properties.Settings.Default.Setting_WorldMapMaxScale = (Byte)numericUpDownWorldMapScale.Value;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void comboBoxRenderingMode_SelectedIndexChanged(object sender, EventArgs e)
        {
            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_WorldMapRenderingMode = (Byte)comboBoxRenderingMode.SelectedIndex;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void checkBoxWatchdogResponseEnabled_CheckedChanged(object sender, EventArgs e)
        {
            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_WatchdogLoopbackEnabled = checkBoxWatchdogResponseEnabled.Checked;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void numericUpDownJpegCompression_ValueChanged(object sender, EventArgs e)
        {
            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_JpegCompression = (Byte)numericUpDownJpegCompression.Value;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void toolStripMenuItemCalibPointDone_Click(object sender, EventArgs e)
        {
            //Determine which radiobutton sent the menu request and change its value
            try
            {
                ToolStripMenuItem tmItem = sender as ToolStripMenuItem;
                ContextMenuStrip cmStrip = tmItem.Owner as ContextMenuStrip;
                RadioButton rbSender = cmStrip.SourceControl as RadioButton;

                //Convert values
                String[] splittedValues = toolStripTextBoxPoint.Text.Split(';');

                float xCoord = float.Parse(splittedValues[0]);
                float yCoord = float.Parse(splittedValues[1]);

                //Update proper value
                switch (rbSender.Name)
                {
                    default: 
                        //Error
                        return;

                    case "rbUL":

                        CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_LEFT]
                            = new PointF(xCoord, yCoord);

                        break;

                    case "rbUR":

                        CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_RIGHT]
                            = new PointF(xCoord, yCoord);


                        break;

                    case "rbLR":

                        CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_RIGHT]
                            = new PointF(xCoord, yCoord);

                        break;

                    case "rbLL":

                        CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_LEFT]
                            = new PointF(xCoord, yCoord);

                        break;
                }

                //Finally reload coordinates
                LoadCalibrationCoordinates();
            }
            catch (FormatException) { /*Conversion failed, do not update*/}
            catch (OverflowException) { /*Conversion failed, do not update*/}
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
        }

        private void cmCalibrationCoordinates_Opened(object sender, EventArgs e)
        {
            //Determine which radiobutton sent the menu request and change its value
            try
            {
                ContextMenuStrip cmContainer = sender as ContextMenuStrip;
                RadioButton rbSender = cmContainer.SourceControl as RadioButton;

                //Update with proper value
                switch (rbSender.Name)
                {
                    default:
                        //Error
                        return;

                    case "rbUL":

                        toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_LEFT].X.ToString()
                            + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_LEFT].Y.ToString();

                        toolStripMenuItemTitleCalibPoints.Text = "Modify upper left calibration point";

                        break;

                    case "rbUR":

                        toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_RIGHT].X.ToString()
                            + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.UPPER_RIGHT].Y.ToString();

                        toolStripMenuItemTitleCalibPoints.Text = "Modify upper right calibration point";

                        break;

                    case "rbLR":

                        toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_RIGHT].X.ToString()
                            + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_RIGHT].Y.ToString();

                        toolStripMenuItemTitleCalibPoints.Text = "Modify lower right calibration point";

                        break;

                    case "rbLL":

                        toolStripTextBoxPoint.Text = CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_LEFT].X.ToString()
                            + "; " + CameraCalibrator.CalibrationPoints[(Byte)CameraCalibrator.PointLocation.LOWER_LEFT].Y.ToString();

                        toolStripMenuItemTitleCalibPoints.Text = "Modify lower left calibration point";

                        break;
                }

                //Redraw menu
                cmCalibrationCoordinates.Refresh();
            }
            catch (NullReferenceException)
            { /*Just for sure -> NullReferenceException should not occur, as cmReferenceHue can be invoked only by numericUpDowns*/}
            catch (Exception)
            {
                //Use default text
                toolStripTextBoxPoint.Text = "Enter X [m]; Y [m]:";
            }
        }

        private void tbWorldMapScale_ValueChanged(object sender, EventArgs e)
        {
            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_WorldMapScale = (Byte)tbWorldMapScale.Value;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void checkBoxDrawWheelchair_CheckedChanged(object sender, EventArgs e)
        {
            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_DrawWheelchair = checkBoxDrawWheelchair.Checked;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        private void tabControlJoyAutoParking_SelectedIndexChanged(object sender, EventArgs e)
        {
            //Check proper toolStripbutton
            switch (tabControlJoyAutoParking.SelectedIndex)
            { 
                default:
                case (Byte)JoyAutoParkingSelesctionIndex.JOYSTICK:

                    toolStripButtonJoystick.Checked = true;
                    toolStripButtonAutoParking.Checked = false;

                    break;

                case (Byte)JoyAutoParkingSelesctionIndex.AUTOPARKING:

                    toolStripButtonAutoParking.Checked = true;
                    toolStripButtonJoystick.Checked = false;

                    break;           
            }
        }

        private void toolStripButtonJoystick_Click(object sender, EventArgs e)
        {
            //Choose joystick tab
            tabControlJoyAutoParking.SelectedIndex = (Byte)JoyAutoParkingSelesctionIndex.JOYSTICK;
        }

        private void toolStripButtonAutoParking_Click(object sender, EventArgs e)
        {
            //Choose autoparking tab
            tabControlJoyAutoParking.SelectedIndex = (Byte)JoyAutoParkingSelesctionIndex.AUTOPARKING;
        }

        private void buttonSTARTAutoParking_Click(object sender, EventArgs e)
        {
            //Start logging
            logWheelchairRunDataToFileToolStripMenuItem_Click(sender, e);

            //Activate thread
            AutoParkingThread = new Thread(AutoParkingThreadMethod);
            AutoParkingThread.IsBackground = true;
            AutoParkingThread.Priority = ThreadPriority.Lowest;
            AutoParkingThread.Start();
        }

        private void checkBoxUserResponseDemanded_CheckedChanged(object sender, EventArgs e)
        {
            //Save settings
            WheelchairControl.Properties.Settings.Default.Setting_ConfirmMarksDetection = checkBoxUserResponseDemanded.Checked;
            WheelchairControl.Properties.Settings.Default.Save();
        }

        #endregion
    }
}
