using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Net.Sockets;
using System.IO;
using System.Numerics;
using RoboticManipulatorTracking;
using EpsonC3Com;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Cuda;
using TIS.Imaging;
using Sensors.Scanners.MicroEpsilon;


namespace Testovaci_aplikace_diplomka
{
    
    
    enum TrackingMethod
    {
        OpenCV,
        FeatureMatching,
        Markers

    }
    public partial class Form1 : Form
    {

        #region From RoScan3D
        //methods and classes taken from the RoScan3D project 

        public class ScanPoint : ICloneable
        {
            public Vector3 Point { get; set; }
            public bool Damaged { get; set; }
            public ushort Reflection { get; set; }
            public ushort ReflectionWidth { get; set; }

            public ScanPoint()
            {
                Point = new Vector3();
                Damaged = false;
            }
            public ScanPoint(bool damaged)
            {
                Point = new Vector3();
                Damaged = damaged;
            }
            public ScanPoint(Vector3 point, bool damaged)
            {
                Point = point;
                Damaged = damaged;
                Reflection = 0;
                ReflectionWidth = 0;
            }
            public ScanPoint(Vector3 point, bool damaged, ushort reflection, ushort width)
            {
                Point = point;
                Damaged = damaged;
                Reflection = reflection;
                ReflectionWidth = width;
            }
            public ScanPoint(ScanPoint other)
            {
                Point = new Vector3(other.Point.X, other.Point.Y, other.Point.Z);
                Damaged = other.Damaged;
                Reflection = other.Reflection;
                ReflectionWidth = other.ReflectionWidth;
            }

            public Object Clone()
            {
                return new ScanPoint(new Vector3(Point.X, Point.Y, Point.Z), Damaged, Reflection, ReflectionWidth);
            }
        }

        //laser scanner
        private IscanCONTROL scanner = new scanCONTROL2900BL(); //scanner driver class
        private string scannerIP = "10.0.41.69"; // IP address of scanner
        private List<ScanPoint> actualProfile = null; //actual profile acquired from scanner

        #region LASER SCANNER CONTROL
        /// <summary>Performs laser scanner initialization</summary>
        private void ConnectScanner()
        {
            if (scanner.ConnectETH(scannerIP)) //OPEN laser scanner
            {
                if (OnConnectedChanged != null)
                    OnConnectedChanged(this, EventArgs.Empty); //if connected -> raises event
            }
            else return;

            scanner.OnClosed += new EventHandler(RaiseChangeEvent); //handles change of connection
            scanner.OnConnected += new EventHandler(RaiseChangeEvent);

            scanner.LaserPower = LaserModes.FULL; //switch on laser
            scanner.IdleTime = 2200;
            scanner.ShutterTime = 100;
            scanner.ReflectionThreshold = 15; // 128;
            scanner.InvalidPointsFilter = true;
            scanner.AutoShutter = false;
            scanner.XRange = XRanges.PLUSMINUS_50;
            scanner.MedianFilter = NumSamples.SAMPLES_7;
            scanner.AverageFilter = NumSamples.SAMPLES_7;

            if (settings["scanner_model"] == "1") //it is scanControl 2900-100 BL
            {
                scanner.Resolution = 1280;

            }
            else //it is scanControl 2750-100
            {
                scanner.Resolution = 640;

            }

            scanner.StartMeas(); //start measuring

            //paramText = "scanCONTROL: threshold = " + scanner.ReflectionThreshold + ", shutter = " + scanner.ShutterTime + " (" + (scanner.AutoShutter ? "AUTO" : "MAN") + "), idle = " + scanner.IdleTime;
        }

        public delegate void ChangedEventHandler(object sender, EventArgs e);
        /// <summary>Status of robot or scanner connection changed</summary>
        public event ChangedEventHandler OnConnectedChanged;

        /// <summary>Raises change event</summary>
        private void RaiseChangeEvent(object sender, EventArgs e)
        {
            if (OnConnectedChanged != null)
                OnConnectedChanged(this, EventArgs.Empty);
        }

        /// <summary>Performs actions when terminating scanner measurement</summary>
        private void EndScanner()
        {
            scanner.StopMeas(); //stop continuous measuring
            scanner.LaserPower = LaserModes.OFF; //switch off laser
            if (scanner.Disconnect()) //Disconnect laser scanner
            {
                if (OnConnectedChanged != null)
                    OnConnectedChanged(this, EventArgs.Empty); //if connected -> raises event
            }
            else return;
        }

        #endregion

        private void ConnectColorCam()
        {
            if (colorCam == null)
                return;

            if (settings["colorcam_default"] == "True")
                colorCam.Device = colorCam.Devices[0]; //this is faster, but opens first camera
            else
            {
                int deviceIndex = Array.FindIndex<Device>(colorCam.Devices, item => item.Name.Contains(settings["colorcam_IP"]));
                if (deviceIndex < 0) // no device found
                    return;

                colorCam.Device = colorCam.Devices[deviceIndex];
            }

            colorCam.LiveDisplayWidth = colorCam.Width;
            colorCam.LiveDisplayHeight = colorCam.Height;
            colorCam.VideoFormat = colorCam.VideoFormats[1];

            //load color camera settings
            if (colorCam.DeviceValid)
            {
                colorCam.VCDPropertyItems.Load(File.ReadAllText(Tools.DataPath + @"\3DScanner.colorCam.config")); //load settings from file
            }

            colorCam.Visible = true;
        }

        

        /// <summary>Disconnect to color camera</summary>
        private void DisconnectColorCam()
        {
            if (colorCam == null)
                return;

            if (colorCam.LiveVideoRunning == true)
                colorCam.LiveStop();

            colorCam.Device = null;
            colorCam.Visible = false;
        }
        #endregion

        TrackerInterface tracker;
        Image<Bgr, byte> input_image = null;
        RobCoord.XYZ newCoor;
        bool trackingActive = false;
        bool moving = false;
        double distTokeep = 300;
        double distance = 300;
        bool initializationDone = false;


        //other
        private Tools.Settings settings = new Tools.Settings(Tools.DataPath + @"\3DScanner.config");

        //events

        /// <summary>Connected to device</summary>
        event EventHandler imageLoadedForTracking;
        event EventHandler newCoordinatesAvailable;

        public Form1()
        {
            InitializeComponent();
            GlobalTrackerClassSet.settings = settings;
            this.imageLoadedForTracking += new EventHandler(this.moveWithManipulator);
            this.newCoordinatesAvailable += new EventHandler(this.showNewCoor);
        }

        
        

        private void noveNastveniTrackeruToolStripMenuItem_Click(object sender, EventArgs e)
        {
            using (SelectDialog dialog = new SelectDialog())
            {
                if (dialog.ShowDialog() == System.Windows.Forms.DialogResult.OK)
                {
                    if (GlobalTrackerClassSet.trackMethod == TrackingMethod.OpenCV)
                    {
                        tracker = new OpenCvTracker(GlobalTrackerClassSet.openCVTracker);
                    }

                    if (GlobalTrackerClassSet.trackMethod == TrackingMethod.FeatureMatching)
                    {
                        tracker = new FeatureMatchingTracker(GlobalTrackerClassSet.detector, GlobalTrackerClassSet.matchMakeMethod);
                    }

                    if (GlobalTrackerClassSet.trackMethod == TrackingMethod.Markers)
                    {
                        MarkersTracker trc = new MarkersTracker();
                        trc.mType = GlobalTrackerClassSet.mType;
                        tracker = (TrackerInterface)trc;
                        
                    }
                    transformacniMaticeECToolStripMenuItem.Visible = true;
                    nastaveniKomunikaceToolStripMenuItem.Visible = false;
                    inicializovatTrackerToolStripMenuItem.Visible = false;
                    starTrackingButton.Visible = false;
                }
            }
        }

        private void inicializovatTrackerToolStripMenuItem_Click(object sender, EventArgs e)
        {
            initializationDone = false;

            if (!colorCam.DeviceValid)
                ConnectColorCam();

            if (colorCam.DeviceValid)
            {
                if (!scanner.Connected)
                    ConnectScanner();

                if (scanner.Connected)
                    scanner.LaserPower = LaserModes.OFF;
                System.Threading.Thread.Sleep(1300);

                colorCam.LiveCaptureContinuous = false;
                colorCam.LiveStart();
                System.Threading.Thread.Sleep(1300);
                
                colorCam.MemorySnapImage();
                colorCam.LiveSuspend();

            }

                if (input_image != null)
            {
                CameraSettingButton.Visible = true;
                if (GlobalTrackerClassSet.trackMethod == TrackingMethod.Markers)
                {
                    MarkersTracker tracker_temporary = (MarkersTracker)tracker;
                    tracker_temporary.GetHsvLimitsFromPicture(input_image);
                    tracker = tracker_temporary;
                }

                if (tracker.initialize(input_image) == true)
                {
                    starTrackingButton.Visible = true;
                    TrackerInitStatusLabel.Text = "inicializace proběhla úspěšně";
                    CameraSettingButton.Visible = true;
                    initializationDone = true;
                }
                else
                {
                    TrackerInitStatusLabel.Text = "inicializuj znovu";
                }
            }
            else
            {
                TrackerInitStatusLabel.Text = "inicializuj znovu";
            }
        }


        private void transformacniMaticeECToolStripMenuItem_Click(object sender, EventArgs e)
        {
            using (TransECdialog dialog = new TransECdialog())
            {
                if (dialog.ShowDialog() == System.Windows.Forms.DialogResult.OK)
                {
                    tracker.SetEfectorCameraMatrix((float)(GlobalTrackerClassSet.roll_E_C*Math.PI/180), (float)(GlobalTrackerClassSet.pitch_E_C * Math.PI / 180), (float)(GlobalTrackerClassSet.yaw_E_C * Math.PI / 180), (float)(GlobalTrackerClassSet.x_E_C * Math.PI / 180), (float)(GlobalTrackerClassSet.y_E_C * Math.PI / 180), (float)(GlobalTrackerClassSet.z_E_C * Math.PI / 180));
                    nastaveniKomunikaceToolStripMenuItem.Visible = true;
                }
            }
        }

        private void nastaveniKomunikaceToolStripMenuItem_Click(object sender, EventArgs e)
        {
            NastaveniKomunikaceDialog dialog = new NastaveniKomunikaceDialog();
            dialog.ShowDialog();
            inicializovatTrackerToolStripMenuItem.Visible = true;
        }

        private void colorCam_ImageAvailable(object sender, TIS.Imaging.ICImagingControl.ImageAvailableEventArgs e)
        {
            if (GlobalTrackerClassSet.tcpInst.RecBlock.ReceiveData == "<Go")
            {
                moving = false;
            }
            pictureBoxNahled.Image = colorCam.ImageBuffers[e.bufferIndex].Bitmap;

            if ( !moving || tracker.initialized==false)
            {

                //if (CudaInvoke.HasCuda)
                //{
                //    CudaInvoke.Resize(colorCam.ImageBuffers[e.bufferIndex].Bitmap.ToImage<Bgr, byte>(), input_image, new Size(1280, 720), interpolation: Emgu.CV.CvEnum.Inter.Nearest);
                //    pictureBoxNahled.Image = colorCam.ImageBuffers[e.bufferIndex].Bitmap;
                //}
                //else
                //{
                input_image = colorCam.ImageBuffers[e.bufferIndex].Bitmap.ToImage<Bgr, byte>().Resize(1366, 768, Emgu.CV.CvEnum.Inter.Nearest);
                   
                //}


                // muže se hodit
                //colorCam.LiveShowLastBuffer
                //colorCam.LiveCaptureLastImage
                //.ImageActiveBuffer

                //pictureBox.Image = colorCam.ImageBuffers[e.bufferIndex].Bitmap;
                //mozna takto
                //input_image = colorCam.ImageActiveBuffer.Bitmap.ToImage<Bgr, byte>();
                if (input_image == null)
                    throw new Exception("Null image from colorcamera.");

                if (trackingActive)
                {
                    if (laserCheckBox.Checked)
                        scanner.LaserPower = LaserModes.FULL;

                    if (imageLoadedForTracking != null)
                        imageLoadedForTracking(this, EventArgs.Empty);


                }
            }
            
        }

        private void moveWithManipulator(object sender, EventArgs e)
        {
            if (GlobalTrackerClassSet.tcpInst != null)
            {
                try
                {
                    GlobalTrackerClassSet.tcpInst.SendCurPosP();
                    //readToList();
                }
                catch (FormatException)
                {
                    MessageBox.Show("Špatně zadané parametry");
                }

                bool success = false;
                double laserZdistance = 300;

                if (laserCheckBox.Checked)
                {
                    XZpoint[] measuredValues = scanner.GetValues();
                    if (measuredValues == null)
                        return;

                    if (measuredValues[0].Z > -7)
                    {
                        laserZdistance = measuredValues[0].Z;
                    }
                    else
                    {
                        foreach (var item in measuredValues)
                        { 
                            if (item.Z >= -7)
                            {
                                laserZdistance = item.Z;
                                break;
                            }
                        }
                    }
                    if (laserZdistance <= 10.0)
                        return;

                while (GlobalTrackerClassSet.tcpInst.RecBlock.State == TcpC3Com.ReceiveBlock.RecState.Waiting)
                { }

                
                    success = tracker.GetNewEfectorCoordiantes(input_image, laserZdistance, (float)(GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.u.Real * Math.PI / 180), (float)(GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.v.Real * Math.PI / 180), (float)(GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.w.Real * Math.PI / 180),
                    (float)GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.x.Real, (float)GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.y.Real, (float)GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.z.Real, out newCoor, objectDistToKeep: distTokeep, sphereRadius: 800);
                }
                else
                {
                    while (GlobalTrackerClassSet.tcpInst.RecBlock.State == TcpC3Com.ReceiveBlock.RecState.Waiting)
                    { }

                    success = tracker.GetNewEfectorCoordiantes(input_image, distance, (float)(GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.u.Real * Math.PI / 180), (float)(GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.v.Real * Math.PI / 180), (float)(GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.w.Real * Math.PI / 180),
                    (float)GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.x.Real, (float)GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.y.Real, (float)GlobalTrackerClassSet.tcpInst.RecBlock.ReceivedXYZ.z.Real, out newCoor, objectDistToKeep: distTokeep, sphereRadius: 800);
                }

                //vykreslovani pozice  v obraze
                if (success)
                {
                    switch (GlobalTrackerClassSet.trackMethod)
                    {
                        case TrackingMethod.OpenCV:
                            OpenCvTracker tracker1 = (OpenCvTracker)tracker;
                            CvInvoke.Rectangle(input_image, tracker1.box, new MCvScalar(0, 0, 255), 2);
                            break;
                        case TrackingMethod.FeatureMatching:
                            FeatureMatchingTracker tracker2 = (FeatureMatchingTracker)tracker;
                            Point[] polyline_points = Array.ConvertAll<PointF, Point>(tracker2.points, System.Drawing.Point.Round);
                            CvInvoke.Polylines(input_image, polyline_points, true, new MCvScalar(0, 0, 255), 5);
                            break;
                        case TrackingMethod.Markers:
                            MarkersTracker tracker3 = (MarkersTracker)tracker;
                            foreach (var item in tracker3.markersCircles)
                            {
                                input_image.Draw(item, new Bgr(255, 0, 0), 2);
                            }
                            Triangle2DF triangle = new Triangle2DF(tracker3.markersCircles[0].Center, tracker3.markersCircles[1].Center, tracker3.markersCircles[2].Center);
                            CircleF centroid = new CircleF(triangle.Centeroid, 5);
                            input_image.Draw(centroid, new Bgr(255, 0, 0));
                            break;
                        default:
                            break;
                    }

                    pictureBox.Image = input_image.ToBitmap<Bgr, byte>();

                    if (GlobalTrackerClassSet.tcpInst != null)
                    {
                        try
                        {

                            //kontroluje souradnice a pripadne je opravuje
                            //if (!coordFromTB.CheckCoord(true))
                            //    MessageBox.Show("Parametry mimo meze");

                            moving = true;
                            GlobalTrackerClassSet.tcpInst.SendGo(newCoor);
                        }
                        catch (FormatException)
                        {
                            MessageBox.Show("Špatně zadané parametry");
                        }
                    }

                }

                if (newCoordinatesAvailable != null)
                    newCoordinatesAvailable(this, EventArgs.Empty);

                


                

                
            }

        }

        private void showNewCoor(object sender, EventArgs e)
        {
            RollLabel.Invoke((MethodInvoker)delegate {
                // Running on the UI thread
                RollLabel.Text = newCoor.u.Str;
            });

            PitchLabel.Invoke((MethodInvoker)delegate {
                // Running on the UI thread
                PitchLabel.Text = newCoor.v.Str;
            });

            YawLabel.Invoke((MethodInvoker)delegate {
                // Running on the UI thread
                YawLabel.Text = newCoor.w.Str;
            });

            XLabel.Invoke((MethodInvoker)delegate {
                // Running on the UI thread
                XLabel.Text = newCoor.x.Str;
            });

            YLabel.Invoke((MethodInvoker)delegate {
                // Running on the UI thread
                YLabel.Text = newCoor.y.Str;
            });

            ZLabel.Invoke((MethodInvoker)delegate {
                // Running on the UI thread
                ZLabel.Text = newCoor.z.Str;
            });

        }

        private void LiveVideoButton_Click(object sender, EventArgs e)
        {
            if (colorCam.DeviceValid)
            {
                colorCam.LiveCaptureContinuous = true;
                colorCam.LiveStart();
            }
        }

        private void CameraSettingButton_Click(object sender, EventArgs e)
        {
            colorCam.ShowPropertyDialog();
        }

        private void StopTrackingButton_Click(object sender, EventArgs e)
        {
            trackingActive = false;
            DistancOkButton.Enabled = true;
            laserCheckBox.Enabled = true;
            ToKeepOkButton.Enabled = true;
        }

        private void starTrackingButton_Click_1(object sender, EventArgs e)
        {

            trackingActive = true;
            DistancOkButton.Enabled = false;
            laserCheckBox.Enabled = false;
            ToKeepOkButton.Enabled = false;
            if (colorCam.DeviceValid)
            {
                //scanner.LaserPower = LaserModes.FULL;
                colorCam.LiveCaptureContinuous = false;
                colorCam.LiveStart();
                System.Threading.Thread.Sleep(1300);
                colorCam.LiveCaptureContinuous = true;

            }
        }

        private void RollLabel_Click(object sender, EventArgs e)
        {
            RollLabel.Text = newCoor.u.Str;
            PitchLabel.Text = newCoor.v.Str;
            YawLabel.Text = newCoor.w.Str;
            XLabel.Text = newCoor.x.Str;
            YLabel.Text = newCoor.y.Str;
            ZLabel.Text = newCoor.z.Str;
        }

        private void DistancOkButton_Click(object sender, EventArgs e)
        {
            distance = double.Parse(DistancTextBox.Text, System.Globalization.CultureInfo.InvariantCulture);
        }

        private void ToKeepOkButton_Click(object sender, EventArgs e)
        {
            distTokeep = double.Parse(distToKeepTextBox.Text, System.Globalization.CultureInfo.InvariantCulture);
        }

        private void laserCheckBox_CheckedChanged(object sender, EventArgs e)
        {
            if (laserCheckBox.Checked)
            {
                if (scanner.Connected)
                    scanner.LaserPower = LaserModes.FULL;
            }
            else
            {
                if (scanner.Connected)
                    scanner.LaserPower = LaserModes.OFF;

            }
        }
    }

    static class GlobalTrackerClassSet
    {
        public static TrackingMethod trackMethod { get; set; }
        public static MatchMakingMetod matchMakeMethod { get; set; }
        public static Trackers openCVTracker { get; set; }
        public static FeatureDetectorMetod detector { get; set; }
        public static markersType mType { get; set; }
        public static TcpC3Com tcpInst { get; set; }
        public static Tools.Settings settings { get; set; }
        public static float roll_E_C { get; set; } = 0;
        public static float pitch_E_C { get; set; } = 0;
        public static float yaw_E_C { get; set; } = 0;
        public static float x_E_C { get; set; } = 0;
        public static float y_E_C { get; set; } = 0;
        public static float z_E_C { get; set; } = 0;


    }
}
