using System;
using System.Collections.Generic;
using System.Drawing;
using System.Threading;
using System.Windows.Forms;

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

//My namespaces
using MyImageProcessingClasses;
using World;

namespace WheelchairControl
{
    /// <summary>
    /// Main GUI form.
    /// <remarks>Image processing methods are separated in this file.</remarks>
    /// </summary>
    partial class FormMain
    {
        #region Private variables

        /// <summary>
        /// Camera capture.
        /// </summary>
        private Capture capture = new Capture();
        
        /// <summary>
        /// Framerate counter.
        /// </summary>
        private FPS_Counter fpsCounter = new FPS_Counter();

        /// <summary>
        /// Calibrator to calculate homography
        /// </summary>
        private CameraCalibrator cameraCalibrator = new CameraCalibrator(); 

        /// <summary>
        /// Right mark detector.
        /// </summary>
        private MarksDetector rightMarkDetector = new MarksDetector();

        /// <summary>
        /// Left mark detector.
        /// </summary>
        private MarksDetector leftMarkDetector = new MarksDetector();

        /// <summary>
        /// SURF feature detector to detect left mark.
        /// </summary>
        private FeatureDetector featureDetectorLeft;

        /// <summary>
        /// SURF feature detector to detect right mark.
        /// </summary>
        private FeatureDetector featureDetectorRight;

        /// <summary>
        /// World map that contains informations about marks, car, platform etc. 
        /// </summary>
        private WorldMap worldMap = new WorldMap();  

        /// <summary>
        /// Homography matrix for transformations from world map coordinates to real world coordinates.
        /// </summary>
        /// <remarks>This is affine homography matrix, therefore there is no need to normalize coordinates of transformed point!</remarks>
        private HomographyMatrix homMatWorldMapToWorld = new HomographyMatrix();

        /// <summary>
        /// Homography matrix for transformations from world coordinates to camera frame picturebox.
        /// </summary>
        /// <remarks>This is affine homography matrix, therefore there is no need to normalize coordinates of transformed point!</remarks>
        private HomographyMatrix homMatWorldToPb;

        /// <summary>
        /// Last captured image.
        /// </summary>
        private Image<Bgr, Byte> lastCaptured;

        /// <summary>
        /// Image thresholded with left reference color.
        /// </summary>
        private Image<Gray, Byte> imageThresholdedLeft;

        /// <summary>
        /// Image thresholded with right reference color.
        /// </summary>
        private Image<Gray, Byte> imageThresholdedRight;

        /// <summary>
        /// Left reference HSV color.
        /// </summary>
        private Hsv hsvSelectionLeft;

        /// <summary>
        /// Right reference HSV color.
        /// </summary>
        private Hsv hsvSelectionRight;

        /// <summary>
        /// Left mark detected by MarksDetector class.
        /// </summary>
        private Point leftDetectedMark;

        /// <summary>
        /// Right mark detected by MarksDetector class.
        /// </summary>
        private Point rightDetectedMark;

        /// <summary>
        /// Lock to guarantee marks mutual exclusion.
        /// </summary>
        private Object marksLock = new Object();

        /// <summary>
        /// Wheelchair model (odometry etc.).
        /// </summary>
        private WheelchairModel wheelchairModel = new WheelchairModel();

        //Others
        private Bitmap frameBitmap, frameWorldMap;  //Camera frame and world map frame bitmaps
        private Graphics graphFrame, graphWorld;    //Graphics connected to frameBitmap and frameWorldMap
        private float scalePrev = -1f;  //Previous world map scale

        #endregion

        #region Miscelaneous methods

        /// <summary>
        /// Racalculates coordinates from wheelchair's center reference frame to reference frame of marks center.
        /// </summary>
        /// <param name="X">X coordinate.</param>
        /// <param name="Y">Y coordinate.</param>
        /// <param name="Phi">Phi coordinate.</param>
        /// <returns>True if succeedes.</returns>
        private Boolean RecalculateRefPointToMarksCenter(ref Double X, ref Double Y, out Double Phi)
        {
            PointF leftMark, rightMark, markCenter;
            Double phiTranform;

            Phi = 0;  //Init out parameter

            if(worldMap.Marks.Count != (Byte)WorldMap.MarkPosition.MARKS_COUNT)
                return false;

            try
            {
                //Get marks
                leftMark = new PointF(worldMap.Marks[(Byte)WorldMap.MarkPosition.LEFT].Coordinates.X, worldMap.Marks[(Byte)WorldMap.MarkPosition.LEFT].Coordinates.Y);
                rightMark = new PointF((float)worldMap.Marks[(Byte)WorldMap.MarkPosition.RIGHT].Coordinates.X, (float)worldMap.Marks[(Byte)WorldMap.MarkPosition.RIGHT].Coordinates.Y);

                //Get marks' center
                markCenter = new PointF((float)((rightMark.X + leftMark.X) / 2.0), (float)((rightMark.Y + leftMark.Y) / 2.0));

                /*Phi (wheelchair coordinate) is calculated with 90° offset -> Y axis is oriented in the direction of car, and X axis runs through the marks to the right,
                so when 0° angle is measured between marks,
                wheelchair's angle in the reference frame is in fact 90° etc.*/
                Phi = ((1000 * Math.PI/2.0) -  (1000 * Math.Atan2(rightMark.Y - leftMark.Y, rightMark.X - leftMark.X)));

                //Angle used for tranformation (90° - phi)
                phiTranform = (Math.PI / 2.0) - (Phi / 1000.0);

                #region Transformation
                {
                    //Transform coordinates (rotation + translation)

                    //Precalculate trigonotmetric functions' values
                    Double tempCos = Math.Cos(phiTranform);
                    Double tempSin = Math.Sin(phiTranform); 

                    //Create data arrays
                    Double[,] matrixData = new Double[3, 3] {
                        {tempCos, tempSin, (-tempCos*markCenter.X) + (-tempSin*markCenter.Y)},
                        {-tempSin, tempCos, (tempSin*markCenter.X) + (-tempCos*markCenter.Y)},
                        {0.0, 0.0, 1}
                        };

                    Double[,] coordinatesData = new Double[3, 1] { 
                        { X },
                        { Y },
                        { 1.0 }
                        };

                    //Crate matrices
                    Matrix<Double> tranformationMatrix = new Matrix<Double>(matrixData);
                    //CvInvoke.cvInvert(tranformationMatrix.Ptr, tranformationMatrix.Ptr, Emgu.CV.CvEnum.SOLVE_METHOD.CV_SVD);
                    Matrix<Double> sourcePointVector = new Matrix<Double>(coordinatesData);

                    //Transform
                    Matrix<Double> matrixResult = tranformationMatrix.Mul(sourcePointVector);

                    //Assign value
                    X = matrixResult[0, 0];
                    Y = matrixResult[1, 0];
                }
                #endregion

                return true;
            }
            catch (Exception)
            { 
                //Error occured
                return false;
            }
        }

        /// <summary>
        /// Tries to detect reference hue of model image automatically.
        /// </summary>
        /// <param name="modelImage">Image to be analyzed.</param>
        /// <returns>Reference hue.</returns>
        private Byte AutoDetectReferenceHue(Image<Bgr, Byte> modelImage)
        {
            const Int32 BOUNDARY_WIDTH = 10;   //Width of white boundary surrounding model

            using (Image<Hsv, Byte> imageHSV = modelImage.Convert<Hsv, Byte>())
                return (Byte)(Math.Round(imageHSV[BOUNDARY_WIDTH, BOUNDARY_WIDTH].Hue,0));
        }

        /// <summary>
        /// Handler called when capture grabs image from camera.
        /// </summary>
        /// <param name="sender">Sender.</param>
        /// <param name="e">Event arguments.</param>
        private void ImageGrabbed_EventHandler(object sender, EventArgs e)
        {
            //Retreive frame
            lastCaptured = capture.RetrieveBgrFrame(); //new Image<Bgr,Byte>(@"..\..\Test.jpg");

            //Signal captured frame
            fpsCounter.FrameCaptured();

            //Create graphics (components are drawn to temporary bitmap first to prevent blinking)
            frameBitmap = lastCaptured.ToBitmap();
            graphFrame = Graphics.FromImage(frameBitmap);

            #region Switch detection method

            //Get selected index using invoke
            Int32 detectionMethodIndex = 0;

            try
            {
                this.Invoke((MethodInvoker)delegate() { detectionMethodIndex = comboBoxFeatureDetectionMethod.SelectedIndex; });
            }
            catch (Exception ex)
            {
                if (ex is ObjectDisposedException || ex is NullReferenceException)
                { /*Form disposed, nothing to do*/ }
                else
                { throw; /*Other exceptions are passed further*/ }  
            }

            //Switch mark detection methods
            switch(detectionMethodIndex)
            {
                default:
                case (Int32)ComboboxDetectionMethodIndexes.REFERENCE_HSV: //Process images using detection in HSV color space

                    //Threshold image in HSV color space
                    imageThresholdedLeft = ImageProcessingTools.ThresholdImageInHSV(lastCaptured, hsvSelectionLeft);
                    imageThresholdedRight = ImageProcessingTools.ThresholdImageInHSV(lastCaptured, hsvSelectionRight);

                    //Show thresholded image
                    ibHSV1.Image = imageThresholdedLeft.Resize(1 / 3.0, Emgu.CV.CvEnum.INTER.CV_INTER_NN);
                    ibHSV2.Image = imageThresholdedRight.Resize(1 / 3.0, Emgu.CV.CvEnum.INTER.CV_INTER_NN);

                    //Find and draw centers of selected areas, draw L/R label
                    {
                        Rectangle leftRect = ImageProcessingTools.GetCenterRectangle(imageThresholdedLeft);
                        Rectangle rightRect = ImageProcessingTools.GetCenterRectangle(imageThresholdedRight);

                        graphFrame.FillEllipse(ImageProcessingTools.myBrushes[0], leftRect);
                        graphFrame.DrawString("L", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[1], leftRect.Location);
                        
                        graphFrame.FillEllipse(ImageProcessingTools.myBrushes[1], rightRect);
                        graphFrame.DrawString("R", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[0], rightRect.Location);
                    }

                    //Save detected point to mark detector class' buffer
                    leftMarkDetector.AddPoint(ImageProcessingTools.FindCenter(imageThresholdedLeft));
                    rightMarkDetector.AddPoint(ImageProcessingTools.FindCenter(imageThresholdedRight));

                    break;

                case (Int32)ComboboxDetectionMethodIndexes.SURF_DETECTOR:   //Use SURF detector
                    {
                    List<Image<Gray, Byte>> featuresList = new List<Image<Gray, byte>>();
                    List<Rectangle> rectangleList = new List<Rectangle>();
                    
                        //Detect and draw left mark
                        featureDetectorLeft.DetectFeature(lastCaptured, featuresList, rectangleList);

                        if (rectangleList.Count > 0)
                        {
                            //Draw
                            graphFrame.DrawRectangle(new Pen(ImageProcessingTools.myBrushes[0]), rectangleList[0]);

                            graphFrame.FillEllipse(ImageProcessingTools.myBrushes[0], 
                                ImageProcessingTools.GetCenterRectangle(rectangleList[0].Center()));

                            graphFrame.DrawString("L", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[0], rectangleList[0].Location);

                            //Add mark detector point
                            leftMarkDetector.AddPoint(rectangleList[0].Center());
                        }

                        //Clear lists
                        featuresList.Clear();
                        rectangleList.Clear();

                        //Detect and draw right mark
                        featureDetectorRight.DetectFeature(lastCaptured, featuresList, rectangleList);

                        if (rectangleList.Count > 0)
                        {
                            //Draw
                            graphFrame.DrawRectangle(new Pen(ImageProcessingTools.myBrushes[1]), rectangleList[0]);

                            graphFrame.FillEllipse(ImageProcessingTools.myBrushes[1], 
                                ImageProcessingTools.GetCenterRectangle(rectangleList[0].Center()));

                            graphFrame.DrawString("R", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[1], rectangleList[0].Location);

                            //Add mark detector point
                            rightMarkDetector.AddPoint(rectangleList[0].Center());
                        }
                    }

                    break;
            }

            #endregion

            //Draw locked marks & actualize world map
            lock (marksLock)
            {
                if (leftDetectedMark.X > 0 && leftDetectedMark.Y > 0)
                {
                    Rectangle tempRect = ImageProcessingTools.GetCenterRectangle(leftDetectedMark);

                    graphFrame.FillEllipse(ImageProcessingTools.myBrushes[2], tempRect);
                    graphFrame.DrawString("L", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[3], tempRect.Location);
                    worldMap.ModifyMark(WorldMap.MarkPosition.LEFT, cameraCalibrator.TransformPoint(leftDetectedMark), Color.GreenYellow);
                }

                if (rightDetectedMark.X > 0 && rightDetectedMark.Y > 0)
                {
                    Rectangle tempRect = ImageProcessingTools.GetCenterRectangle(rightDetectedMark);

                    graphFrame.FillEllipse(ImageProcessingTools.myBrushes[2], tempRect);
                    graphFrame.DrawString("R", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[3], tempRect.Location);
                    worldMap.ModifyMark(WorldMap.MarkPosition.RIGHT, cameraCalibrator.TransformPoint(rightDetectedMark), Color.Red);
                }
            }

            //Draw fps
            if (menuItemShowFPSIndicator.Checked)
                graphFrame.DrawString(fpsCounter.FPS.ToString("F2") + " FPS", Font, ImageProcessingTools.myBrushes[0], (PointF)ImageProcessingTools.defaultPoint);

            //Draw bitmap
            pbFrame.Image = frameBitmap;

            //Draw world map
            DrawWorldMap();
        }

        /// <summary>
        /// Routine to detect marks. Run in separate thread.
        /// </summary>
        /// <param name="resetCounters">Reset detectors.</param>
        private void MarkLock_ThreadRoutine(Boolean resetCounters)
        {
            Point leftMark, rightMark;

            //Reset detectors if allowed
            if (resetCounters)
            {
                leftMarkDetector.Reset();
                rightMarkDetector.Reset();
            }

            //Wait until buffers are full
            while (!leftMarkDetector.IsBufferFull || !rightMarkDetector.IsBufferFull)
                Thread.Sleep(1000 / (Int32)Math.Floor(fpsCounter.FPS)); //Wait approximately one frame

            //Verify if marks were detected.
            if (!leftMarkDetector.DetectMark(out leftMark))
                leftMark = new Point(-1, -1); //Detection failed, add fake point instead 

            if (!rightMarkDetector.DetectMark(out rightMark))
                rightMark = new Point(-1, -1); //Detection failed, add fake point instead   

            //Save thread-safely
            lock (marksLock)
            {
                leftDetectedMark = leftMark;
                rightDetectedMark = rightMark;
            }
        }

        /// <summary>
        /// Draw world map to the picturebox.
        /// </summary>
        private void DrawWorldMap()
        {
            try
            {
                const float CIRCLE_WIDTH = 5.0f;
                Byte renderingModeIndex = (Byte)WorldMapRenderingModes.NONE;

                //Transformed points of the trajectory
                List<PointF> trajectoryPoints = new List<PointF>();

                //Crate graphics
                graphWorld = Graphics.FromImage(frameWorldMap);
                graphWorld.Clear(pbWorldMap.BackColor);
                
                //Select rendering mode
                try { this.Invoke((MethodInvoker)delegate() { renderingModeIndex = (Byte)comboBoxRenderingMode.SelectedIndex; }); }
                catch (Exception) { renderingModeIndex = (Byte)WorldMapRenderingModes.NONE; /*Just in case*/ }

                switch (renderingModeIndex)
                { 
                    default:
                    case (Byte)WorldMapRenderingModes.NONE:

                        graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.None;

                        break;

                    case (Byte)WorldMapRenderingModes.HIGH_SPEED:

                        graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighSpeed;

                        break;

                    case (Byte)WorldMapRenderingModes.HIGH_QUALITY:

                        graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;

                        break;

                    case (Byte)WorldMapRenderingModes.ANTIALIAS:

                        graphWorld.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias;

                        break;                
                }

                //Find transformation
                float scale = 1f;

                //Set scale - trackbar is owned by another thread - use invoker
                try
                {
                    if (tbWorldMapScale.InvokeRequired)
                        tbWorldMapScale.Invoke((MethodInvoker)(delegate()
                        {
                            scale = (float)(tbWorldMapScale.Value);
                        }));
                    else
                    {
                        scale = (float)(tbWorldMapScale.Value);
                    }
                }
                catch (ObjectDisposedException)
                {
                    //Set default scale
                    scale = 1f;
                }

                //Recalculate homography matrices only when neccessary
                if (scale != scalePrev)
                {
                    //Source points - points of the calibrating rectangle
                    List<PointF> selectedPoints = new List<PointF> { new PointF(-1*scale, scale),      //Upper left
                                                             new PointF(scale, scale),       //Upper right
                                                             new PointF(scale, -1 * scale),     //Lower right
                                                             new PointF(-1 * scale, -1 * scale) };  //Lower left

                    //Target point - corners of the picturebox
                    List<PointF> templatePoints = new List<PointF> {    new PointF(0f, 0f),
                                                                new PointF((float)pbWorldMap.Width, 0f),
                                                                new PointF((float)pbWorldMap.Width, (float)pbWorldMap.Height),
                                                                new PointF(0,(float)pbWorldMap.Height)};

                    //World coordinates to picturebox coordinates
                    homMatWorldToPb = CameraCalibration.FindHomography(selectedPoints.ToArray(), templatePoints.ToArray(),
                                                                                Emgu.CV.CvEnum.HOMOGRAPHY_METHOD.DEFAULT, 1.0);

                    //PbWorldMap coordinates to world coordinates - onversion of previous matrix
                    CvInvoke.cvInvert(homMatWorldToPb.Ptr, homMatWorldMapToWorld.Ptr, Emgu.CV.CvEnum.SOLVE_METHOD.CV_LU);

                    /*NOTE: Both homMatWorldToPb and homMatWorldMapToWorld matrices are affine homography matrices (or very close to them),
                    therefore, it is no need to normalize transformed coordinates with w'!*/

                    //Save scale
                    scalePrev = scale;
                }

                #region Draw map

                //Draw marks
                foreach (Mark mark in worldMap.Marks)
                {
                    //Create data array
                    Double[,] data = new Double[3, 1]{  {(Double)mark.Coordinates.X},  //X
                                                {(Double)mark.Coordinates.Y},  //Y
                                                {1.0}               //Scale
                                             };

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

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

                    graphWorld.FillEllipse(new SolidBrush(mark.MarkColor), (float)(matrixTemp[0, 0] - CIRCLE_WIDTH / 2), (float)(matrixTemp[1, 0] - CIRCLE_WIDTH / 2), CIRCLE_WIDTH, CIRCLE_WIDTH);
                }

                //Draw platform
                PointF[] platformRectTmp = Car.PlatformRectangle;

                for (Byte i = 0; i < Car.PlatformRectangle.Length; i++)
                {
                    //Create data array
                    Double[,] data = new Double[3, 1]{  {(Double)Car.PlatformRectangle[i].X},  //X
                                                {(Double)Car.PlatformRectangle[i].Y},      //Y
                                                {1.0}                   //Scale
                                             };

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

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

                    //Save point
                    platformRectTmp[i].X = (float)matrixTemp[0, 0];
                    platformRectTmp[i].Y = (float)matrixTemp[1, 0];
                }

                graphWorld.FillPolygon(new SolidBrush(Car.PLATFORM_COLOR), platformRectTmp);

                //Draw car
                PointF[] carRectTmp = Car.CarRectangle;

                for (Byte i = 0; i < Car.CarRectangle.Length; i++)
                {
                    //Create data array
                    Double[,] data = new Double[3, 1]{  {(Double)Car.CarRectangle[i].X},  //X
                                                {(Double)Car.CarRectangle[i].Y},      //Y
                                                {1.0}                   //Scale
                                             };

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

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

                    //Save point
                    carRectTmp[i].X = (float)matrixTemp[0, 0];
                    carRectTmp[i].Y = (float)matrixTemp[1, 0];
                }

                graphWorld.FillPolygon(new SolidBrush(Car.CAR_COLOR), carRectTmp);

                //Draw wheelchair
                Boolean drawWheelchair = false;
                PointF[] wheelchairRectangle = wheelchairModel.GetDefaultSurroundingRectangle();

                try
                {
                    //Invoke in formMain's thread
                    this.Invoke((MethodInvoker)delegate() { drawWheelchair = checkBoxDrawWheelchair.Checked; });
                }
                catch (Exception)
                { drawWheelchair = false; /*Just for case value was corrupted*/ }

                if (drawWheelchair) //Draw only if proper checkbox is checked
                {
                    for (Byte i = 0; i < wheelchairRectangle.Length; i++)
                    {
                        //Create data array
                        Double[,] data = new Double[3, 1]{  {(Double)(wheelchairRectangle[i].X / 1000.0)},  // X [mm]
                                                {(Double)(wheelchairRectangle[i].Y / 1000.0)},      //Y [mm]
                                                {1.0}                   //Scale
                                             };

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

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

                        //Save point
                        wheelchairRectangle[i].X = (float)matrixTemp[0, 0];
                        wheelchairRectangle[i].Y = (float)matrixTemp[1, 0];
                    }

                    graphWorld.FillPolygon(WheelchairModel.WHEELCHAIR_COLOR[0], wheelchairRectangle); //Wheelchair

                    graphWorld.FillPolygon(WheelchairModel.WHEELCHAIR_COLOR[1], new PointF[] { //Arrow
                        new PointF((wheelchairRectangle[3].X + wheelchairRectangle[0].X) / 2, (wheelchairRectangle[3].Y + wheelchairRectangle[0].Y) / 2),
                        new PointF((wheelchairRectangle[0].X + wheelchairRectangle[1].X) / 2, (wheelchairRectangle[0].Y + wheelchairRectangle[1].Y) / 2),
                        new PointF((wheelchairRectangle[1].X + wheelchairRectangle[2].X) / 2, (wheelchairRectangle[1].Y + wheelchairRectangle[2].Y) / 2)});
                }

                //Draw all track points
                foreach (TrackPoint trPoint in worldMap.TrackPoints)
                {
                    //Create data array
                    Double[,] data = new Double[3, 1]{  {(Double)trPoint.Coordinates.X},  //X
                                                {(Double)trPoint.Coordinates.Y},  //Y
                                                {1.0}               //Scale
                                             };

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

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

                    //Save trajectory point and draw it
                    trajectoryPoints.Add(new PointF((float)matrixTemp[0, 0], (float)matrixTemp[1, 0]));

                    graphWorld.FillEllipse(new SolidBrush(Color.Blue), (float)(matrixTemp[0, 0] - CIRCLE_WIDTH / 2), (float)(matrixTemp[1, 0] - CIRCLE_WIDTH / 2), CIRCLE_WIDTH, CIRCLE_WIDTH);
                }

                //Draw trajectory
                for (Byte i = 1; i < worldMap.TrackPoints.Count; i++)
                {
                    graphWorld.DrawLine(new Pen(Color.Blue), trajectoryPoints[i], trajectoryPoints[i - 1]);
                }

                if (trajectoryPoints.Count > 0)
                    graphWorld.DrawLine(new Pen(Color.Blue), trajectoryPoints[trajectoryPoints.Count - 1], new Point(pbWorldMap.Width / 2, pbWorldMap.Height / 2));

                //Draw cross
                graphWorld.DrawLine(new Pen(Color.Black), 0, pbWorldMap.Height / 2, pbWorldMap.Width, pbWorldMap.Height / 2);
                graphWorld.DrawLine(new Pen(Color.Black), pbWorldMap.Width / 2, 0, pbWorldMap.Width / 2, pbWorldMap.Height);

                //Draw scale
                graphWorld.DrawString(scale.ToString() + " m", ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[2],
                    new PointF(pbWorldMap.Width - ImageProcessingTools.myFont.Size * 4 /*Expected characters count = 4*/, pbWorldMap.Height / 2 - ImageProcessingTools.myFont.Height));

                //Draw status info
                String statusInfoString = null;
                Byte cbReferenceSelIndex = (Byte)MapReferenceCoordinates.MARKS_CENTER;

                try
                { 
                    //Always invoke, this thread is not member of STAThread apartment
                    this.Invoke((MethodInvoker)delegate() { cbReferenceSelIndex = (Byte)comboBoxCoordinatesReference.SelectedIndex; });
                }
                catch (Exception)
                { cbReferenceSelIndex = (Byte)MapReferenceCoordinates.MARKS_CENTER; /*Just in case value was corrupted*/ }

                if (cbReferenceSelIndex == (Byte)MapReferenceCoordinates.MARKS_CENTER)
                {
                    //Display odometry info
                    statusInfoString = "Wheelchair odometry:" + Environment.NewLine +
                            "X: " + (wheelchairModel.X / 1000.0).ToString("F2") + " m" + Environment.NewLine +
                            "Y: " + (wheelchairModel.Y / 1000.0).ToString("F2") + " m" + Environment.NewLine +
                            "Phi: " + (wheelchairModel.Phi / 1000.0).ToString("F3") + " rad ~ " + (180 * wheelchairModel.Phi / (1000.0 * Math.PI)).ToString("F2") + "°";
                }
                else
                {
                    //Wheelchair is reference point in this case, so its coordinates are always [0;0;0]
                    statusInfoString = "Wheelchair odometry:" + Environment.NewLine +
                            "X: 0.00 m" + Environment.NewLine +
                            "Y: 0.00 m" + Environment.NewLine +
                            "Phi: 0.000 rad ~ 0.00°";   
                }

                graphWorld.DrawString(statusInfoString, ImageProcessingTools.myFont, ImageProcessingTools.myBrushes[2], new PointF(0f, 0f));

                //Display image
                pbWorldMap.Image = frameWorldMap;

                #endregion
            }
            catch (Exception ex)
            {
                //Draw error image
                graphWorld = Graphics.FromImage(frameWorldMap);
                graphWorld.Clear(pbWorldMap.BackColor);

                graphWorld.DrawString("Error:" + Environment.NewLine + ex.Message.ToString(), new Font(FontFamily.GenericMonospace, 10f), 
                    MyImageProcessingClasses.ImageProcessingTools.myBrushes[2], new RectangleF(new Point(0, 0), pbWorldMap.Size));

                pbWorldMap.Image = frameWorldMap;
            }
        }

        /// <summary>
        /// Routine for mouse click when point calibration tab is selected.
        /// </summary>
        /// <param name="e">Mouse event arguments</param>
        private void CalibratePointsMouseClickRoutine(MouseEventArgs e)
        {
            //Switch mouse button 
            switch (e.Button)
            {
                //Left button - calibrate points
                case MouseButtons.Left:
                    {
                        PointF mousePoint = new PointF((float)e.X, (float)e.Y);

                        //Select point to calibrate
                        if (rbUL.Checked)   //Upper left point
                        {
                            cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.UPPER_LEFT);

                            //Auto switch
                            rbUR.Select();

                            break;
                        }

                        if (rbUR.Checked)   //Upper right point
                        {
                            cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.UPPER_RIGHT);

                            //Auto switch
                            rbLR.Select();

                            break;
                        }

                        if (rbLR.Checked)   //Lower right point
                        {
                            cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.LOWER_RIGHT);

                            //Auto switch
                            rbLL.Select();

                            break;
                        }

                        if (rbLL.Checked)   //Lower left point
                        {
                            cameraCalibrator.AddCalibrationPoint(mousePoint, CameraCalibrator.PointLocation.LOWER_LEFT);

                            //Auto switch
                            rbUL.Select();

                            break;
                        }

                        break;
                    }

                default:
                    //No default action
                    break;
            }

            //Enable button if all point are specified
            if (cameraCalibrator.AllCalibrationPointsSpecified)
                buttonCalibrateHomography.Enabled = true;
            else
                buttonCalibrateHomography.Enabled = false;
        }

        /// <summary>
        /// Routine for mouse click when color calibration tab is selected.
        /// </summary>
        /// <param name="e">Mouse event arguments</param>
        private void SelectColorMouseClickRoutine(MouseEventArgs e)
        {
            //Switch mouse buttons
            switch (e.Button)
            {
                //Left button - calibrate color
                case MouseButtons.Left:

                    //check whether index is in bounds
                    if (e.X < capture.Width && e.Y < capture.Height)
                    {
                        if (comboBoxFeatureDetectionMethod.SelectedIndex == (Byte)ComboboxDetectionMethodIndexes.REFERENCE_HSV)
                        {
                            //HSV feature detection method is used -> select color to calibrate
                            if (rbSelectColor1.Checked)
                            {
                                //Paint label with selected color
                                lblSelectedColor1.BackColor = Color.FromArgb((Int32)lastCaptured[e.Y, e.X].Red, (Int32)lastCaptured[e.Y, e.X].Green, (Int32)lastCaptured[e.Y, e.X].Blue);

                                //Calculate HSV values
                                hsvSelectionLeft = lastCaptured.Convert<Hsv, Byte>()[e.Y, e.X];
                                lblHSV.Text = "Selected HSV: \nH: " + hsvSelectionLeft.Hue.ToString() + "\nS: " + hsvSelectionLeft.Satuation.ToString() + "\nV: " + hsvSelectionLeft.Value.ToString();

                                //Switch color selection
                                rbSelectColor2.Select();
                            }
                            else
                            {
                                //Paint label with selected color
                                lblSelectedColor2.BackColor = Color.FromArgb((Int32)lastCaptured[e.Y, e.X].Red, (Int32)lastCaptured[e.Y, e.X].Green, (Int32)lastCaptured[e.Y, e.X].Blue);

                                //Calculate HSV values
                                hsvSelectionRight = lastCaptured.Convert<Hsv, Byte>()[e.Y, e.X];
                                lblHSV.Text = "Selected HSV: \nH: " + hsvSelectionRight.Hue.ToString() + "\nS: " + hsvSelectionRight.Satuation.ToString() + "\nV: " + hsvSelectionRight.Value.ToString();

                                //Switch color selection
                                rbSelectColor1.Select();
                            }
                        }
                        else
                        { 
                            //If SURF detector is used, click is used only to diplay selected value for user info, but does not modify the reference colors
                            Hsv selectedHSV = lastCaptured.Convert<Hsv, Byte>()[e.Y, e.X];
                            lblHSV.Text = "Selected HSV: \nH: " + selectedHSV.Hue.ToString() + "\nS: " + selectedHSV.Satuation.ToString() + "\nV: " + selectedHSV.Value.ToString();
                        }
                    }

                    break;

                default:
                    //No default action
                    break;
            }
        }

        #endregion
    }
}