using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Drawing;

using Emgu.CV;
using Emgu.CV.Structure;
using EpsonC3Com;
using System.Numerics;

namespace RoboticManipulatorTracking
{
    enum markersType
    {
        Circles,
        Biggest
    }
    class MarkersTracker : TrackerInterface
    {
        public Hsv upperLimit { get; set; }
        public Hsv lowerLimit { get; set; }
        public bool initialized { get; set; } = false;
        private LineSegment2DF initLine { get; set; }

        private Image<Gray, byte> imgBinarized;
        private Matrix4x4 e_c { get; set; }
        public double cameraAngleX { get; set; } = 60.7974554 * Math.PI / 180;
        public double cameraAngleY { get; set; } = 47.498989 * Math.PI / 180;
        public double objectDistToKeep { get; set; } = 0;
        public double sphereRadius { get; set; } = 700;
        public CircleF[] markersCircles { get; set; }

        public markersType mType { get; set; } = markersType.Circles;


        public MarkersTracker()
        {
            upperLimit = new Hsv();
            lowerLimit = new Hsv();
        }
        public MarkersTracker(Hsv upperLimit, Hsv lowerLimit, double cameraAngleX = 60.7974554 * Math.PI / 180, double cameraAngleY = 47.498989 * Math.PI / 180, double objectDistToKeep = 0, double sphereRadius = 600)
        {
            this.upperLimit = upperLimit;
            this.lowerLimit = lowerLimit;
            this.cameraAngleX = cameraAngleX;
            this.cameraAngleY = cameraAngleY;
            this.objectDistToKeep = objectDistToKeep;
            this.sphereRadius = sphereRadius;
        }

        public MarkersTracker(double cameraAngleX = 60.7974554 * Math.PI / 180, double cameraAngleY = 47.498989 * Math.PI / 180, double objectDistToKeep = 0, double sphereRadius = 600)
        {
            this.cameraAngleX = cameraAngleX;
            this.cameraAngleY = cameraAngleY;
            this.objectDistToKeep = objectDistToKeep;
            this.sphereRadius = sphereRadius;
        }
        private Triangle2DF FindMarkers(Image<Hsv, byte> imgHsv)
        {

            imgBinarized = imgHsv.InRange(lowerLimit, upperLimit);
            Triangle2DF markersTriangle;
            Emgu.CV.Util.VectorOfVectorOfPoint contours = new Emgu.CV.Util.VectorOfVectorOfPoint();
            Mat hier = new Mat();
            CvInvoke.FindContours(imgBinarized, contours, hier, Emgu.CV.CvEnum.RetrType.External, Emgu.CV.CvEnum.ChainApproxMethod.ChainApproxSimple);
            
            Dictionary<int, double> dict = new Dictionary<int, double>();
            for (int i = 0; i < contours.Size; i++)
            {
                CircleF circle = CvInvoke.MinEnclosingCircle(contours[i]);
                Rectangle rect = CvInvoke.BoundingRectangle(contours[i]);
                double area = CvInvoke.ContourArea(contours[i]);
                var contour = contours[i];
                var vf = new PointF[contour.Size];

                for (int ii = 0; ii < contour.Size; ii++)
                { vf[ii] = new PointF(contour[ii].X, contour[ii].Y); }

                Emgu.CV.Util.VectorOfPointF vvf = new Emgu.CV.Util.VectorOfPointF(vf);

                var c = new Emgu.CV.Util.VectorOfPointF();

                CvInvoke.ConvexHull(vvf, c, false, true);
                double hull_area = CvInvoke.ContourArea(c);
                double solidity = area / hull_area;
                //double aspect_ratio = rect.Width / rect.Height;
                double equi_diameter = Math.Sqrt(4 * area / Math.PI);
                double ratio_diameters = circle.Radius / (equi_diameter / 2);


                if (ratio_diameters > 0.80 && ratio_diameters < 1.25 && /*aspect_ratio > 0.85 && aspect_ratio < 1.15 &&*/ rect.Width > 20 && solidity > 0.85 && solidity < 1.15)
                {
                    dict.Add(i, area);
                }

            }

            var items = dict.OrderByDescending(v => v.Value).Take(3);
            PointF[] triangle_points = new PointF[3];
            double[] y = new double[3];
            markersCircles = new CircleF[items.Count<KeyValuePair<int,double>>()];
            if (items.Count() == 3)
            {
                int p = 0;
                
                foreach (var item in items)
                {
                    int key = int.Parse(item.Key.ToString());
                    CircleF circle = CvInvoke.MinEnclosingCircle(contours[key]);
                    markersCircles[p] = circle;
                    triangle_points[p] = circle.Center;
                    p++;
                }
                markersTriangle = new Triangle2DF(triangle_points[0], triangle_points[1], triangle_points[2]);

                return markersTriangle;
            }
            else
            {
                int p = 0;
                foreach (var item in items)
                {
                    int key = int.Parse(item.Key.ToString());
                    CircleF circle = CvInvoke.MinEnclosingCircle(contours[key]);
                    markersCircles[p] = circle;
                    p++;
                }
                return new Triangle2DF();
            }
        }

        private Triangle2DF FindMarkers3Biggest(Image<Hsv, byte> imgHsv)
        {

            imgBinarized = imgHsv.InRange(lowerLimit, upperLimit);
            Triangle2DF markersTriangle;
            Emgu.CV.Util.VectorOfVectorOfPoint contours = new Emgu.CV.Util.VectorOfVectorOfPoint();
            Mat hier = new Mat();
            CvInvoke.FindContours(imgBinarized, contours, hier, Emgu.CV.CvEnum.RetrType.External, Emgu.CV.CvEnum.ChainApproxMethod.ChainApproxSimple);

            Dictionary<int, double> dict = new Dictionary<int, double>();
            for (int i = 0; i < contours.Size; i++)
            {
                double area = CvInvoke.ContourArea(contours[i]);
                var contour = contours[i];

                if (area>25)
                {
                    dict.Add(i, area);
                }

            }

            var items = dict.OrderByDescending(v => v.Value).Take(3);
            PointF[] triangle_points = new PointF[3];
            double[] y = new double[3];
            markersCircles = new CircleF[items.Count<KeyValuePair<int, double>>()];
            if (items.Count() == 3)
            {
                int p = 0;

                foreach (var item in items)
                {
                    int key = int.Parse(item.Key.ToString());
                    CircleF circle = CvInvoke.MinEnclosingCircle(contours[key]);
                    markersCircles[p] = circle;
                    triangle_points[p] = circle.Center;
                    p++;
                }
                markersTriangle = new Triangle2DF(triangle_points[0], triangle_points[1], triangle_points[2]);

                return markersTriangle;
            }
            else
            {
                int p = 0;
                foreach (var item in items)
                {
                    int key = int.Parse(item.Key.ToString());
                    CircleF circle = CvInvoke.MinEnclosingCircle(contours[key]);
                    markersCircles[p] = circle;
                    p++;
                }
                return new Triangle2DF();
            }
        }

        public Image<Gray, byte> GetBinarizedImage()
        {
            return imgBinarized;
        }
        public void SetEfectorCameraMatrix(float roll_E_C, float pitch_E_C, float yaw_E_C, float x_E_C, float y_E_C, float z_E_C)
        {
            e_c = Matrix4x4.CreateRotationX(yaw_E_C) * Matrix4x4.CreateRotationY(pitch_E_C) * Matrix4x4.CreateRotationZ(roll_E_C) * Matrix4x4.CreateTranslation(x_E_C, y_E_C, z_E_C);
        }

        public bool GetNewEfectorCoordiantes(Image<Bgr, byte> img, double objectDist, float roll_B_E, float pitch_B_E, float yaw_B_E, float x_B_E, float y_B_E, float z_B_E, out RobCoord.XYZ newSysCoor)
        {
            Triangle2DF markersTriangle;
            if (mType == markersType.Biggest)
            {
                markersTriangle = FindMarkers3Biggest(img.Convert<Hsv, byte>());
            }
            else
            {
                markersTriangle = FindMarkers(img.Convert<Hsv, byte>());
            }
            float rotation;
            if (!(markersTriangle.Equals(new Triangle2DF())))
            {
                if (initialized == false)
                {
                    rotation = 0;
                }
                else
                {
                    LineSegment2DF[] lines = new LineSegment2DF[] { new LineSegment2DF(markersTriangle.V0, markersTriangle.V1), new LineSegment2DF(markersTriangle.V0, markersTriangle.V2), new LineSegment2DF(markersTriangle.V1, markersTriangle.V2) };
                    Dictionary<int, double> dict_lines = new Dictionary<int, double>();
                    dict_lines.Add(0, lines[0].Length);
                    dict_lines.Add(1, lines[1].Length);
                    dict_lines.Add(2, lines[2].Length);
                    var index = dict_lines.OrderByDescending(v => v.Value).Take(1);
                    LineSegment2DF bigestLine = lines[index.First().Key];
                    rotation = (float)Math.Round(initLine.GetExteriorAngleDegree(bigestLine), 3);

                }
                MCvPoint3D64f pointInPicture = new MCvPoint3D64f(markersTriangle.Centeroid.X, markersTriangle.Centeroid.Y, objectDist);
                EfektorCoordinates newCoordinates = new EfektorCoordinates(e_c, cameraAngleX, cameraAngleY, objectDistToKeep, sphereRadius);
                Matrix4x4 b_e = Matrix4x4.CreateRotationX(yaw_B_E)* Matrix4x4.CreateRotationY(pitch_B_E)* Matrix4x4.CreateRotationZ(roll_B_E)* Matrix4x4.CreateTranslation(x_B_E, y_B_E, z_B_E) ;
                newSysCoor = newCoordinates.ComputeNewCoordinates(pointInPicture, img.Size, (float)(rotation * Math.PI / 180), b_e);

                //RobCoord.XYZ coor = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
                if (newSysCoor.u.Real == 0 && newSysCoor.v.Real == 0 && newSysCoor.w.Real == 0 && newSysCoor.x.Real == 0 && newSysCoor.y.Real == 0 && newSysCoor.z.Real == 0)
                {
                    return false;
                }
                return true;

            }
            else
            {
                newSysCoor = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
                return false;
            }

        }

        public bool GetNewEfectorCoordiantes(Image<Bgr, byte> img, double objectDist, float roll_B_E, float pitch_B_E, float yaw_B_E, float x_B_E, float y_B_E, float z_B_E, out RobCoord.XYZ newSysCoor, double cameraAngleX = 60.7974554 * Math.PI / 180, double cameraAngleY = 47.498989 * Math.PI / 180, double objectDistToKeep = 0, double sphereRadius = 600)
        {
            Triangle2DF markersTriangle;
            if (mType == markersType.Biggest)
            {
                markersTriangle = FindMarkers3Biggest(img.Convert<Hsv, byte>());
            }
            else
            {
                markersTriangle = FindMarkers(img.Convert<Hsv, byte>());
            }

            float rotation;
            if (!(markersTriangle.Equals(new Triangle2DF())))
            {
                if (initialized == false)
                {
                    rotation = 0;
                }
                else
                {
                    LineSegment2DF[] lines = new LineSegment2DF[] { new LineSegment2DF(markersTriangle.V0, markersTriangle.V1), new LineSegment2DF(markersTriangle.V0, markersTriangle.V2), new LineSegment2DF(markersTriangle.V1, markersTriangle.V2) };
                    Dictionary<int, double> dict_lines = new Dictionary<int, double>();
                    dict_lines.Add(0, lines[0].Length);
                    dict_lines.Add(1, lines[1].Length);
                    dict_lines.Add(2, lines[2].Length);
                    var index = dict_lines.OrderByDescending(v => v.Value).Take(1);
                    LineSegment2DF bigestLine = lines[index.First().Key];
                    rotation = (float) Math.Round(initLine.GetExteriorAngleDegree(bigestLine),3);

                }
                MCvPoint3D64f pointInPicture = new MCvPoint3D64f(markersTriangle.Centeroid.X, markersTriangle.Centeroid.Y, objectDist);
                EfektorCoordinates newCoordinates = new EfektorCoordinates(e_c, cameraAngleX, cameraAngleY, objectDistToKeep, sphereRadius);
                Matrix4x4 b_e = Matrix4x4.CreateRotationX(yaw_B_E) * Matrix4x4.CreateRotationY(pitch_B_E) * Matrix4x4.CreateRotationZ(roll_B_E) * Matrix4x4.CreateTranslation(x_B_E, y_B_E, z_B_E);
                newSysCoor = newCoordinates.ComputeNewCoordinates(pointInPicture, img.Size, (float)(rotation*Math.PI/180), b_e);

                //RobCoord.XYZ coor = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
                if (newSysCoor.u.Real == 0 && newSysCoor.v.Real == 0 && newSysCoor.w.Real == 0 && newSysCoor.x.Real == 0 && newSysCoor.y.Real == 0 && newSysCoor.z.Real == 0)
                {
                    return false;
                }
                return true;
            }
            else
            {
                newSysCoor = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
                return false;
            }
        }

        
        public Hsv[] GetHsvLimitsFromPicture(Image<Bgr, byte> img)
        {
            Rectangle selected_rectange = new Rectangle(0, 0, 0, 0);
            selected_rectange = CvInvoke.SelectROI("Select marker", img, fromCenter: false, showCrosshair: true);
            if (selected_rectange.IsEmpty)
            {
                lowerLimit = new Hsv(0, 0, 0);
                upperLimit = new Hsv(0, 0, 0);
                CvInvoke.DestroyWindow("Select marker");
                return new Hsv[] { lowerLimit, upperLimit };
            }
            Image<Hsv, byte> imageHSV = img.Convert<Hsv, byte>();
            imageHSV.ROI = selected_rectange;

            Image<Gray, byte>[] channels = imageHSV.Split();
            Image<Gray, byte> ImgHue = channels[0];
            Image<Gray, byte> ImgSat = channels[1];
            Image<Gray, byte> ImgVal = channels[2];

            DenseHistogram hist = new DenseHistogram(181, new RangeF(0.0f, 180.0f));

            hist.Calculate(new Image<Gray, byte>[] { ImgHue }, false, null);

            Mat m = new Mat();
            hist.CopyTo(m);

            double[] minV, maxV;
            Point[] minL, maxL;

            hist.MinMax(out minV, out maxV, out minL, out maxL);

            double LH;
            double LS;
            double LV;
            double HH;
            double HS;
            double HV;

            int offset = 20;
            int offset1 = 50;
            if (maxL[0].Y < (180 - offset))
            {
                HH = maxL[0].Y + offset;
            }
            else
            {
                HH = 179;
            }

            if (maxL[0].Y > offset)
            {
                LH = maxL[0].Y - offset;
            }
            else
            {
                LH = 1;
            }

            hist = new DenseHistogram(256, new RangeF(0.0f, 255.0f));

            hist.Calculate(new Image<Gray, byte>[] { ImgSat }, false, null);

            hist.MinMax(out minV, out maxV, out minL, out maxL);

            if (maxL[0].Y < (255 - offset1))
            {
                HS = maxL[0].Y + offset1;
            }
            else
            {
                HS = 255;
            }

            if (maxL[0].Y > offset1)
            {
                LS = maxL[0].Y - offset1;
            }
            else
            {
                LS = 0;
            }

            hist.Calculate(new Image<Gray, byte>[] { ImgVal }, false, null);

            hist.MinMax(out minV, out maxV, out minL, out maxL);


            if (maxL[0].Y < (255 - offset1))
            {
                HV = maxL[0].Y + offset1;
            }
            else
            {
                HV = 255;
            }

            if (maxL[0].Y > offset1)
            {
                LV = maxL[0].Y - offset1;
            }
            else
            {
                LV = 0;
            }

           lowerLimit = new Hsv(LH, LS, LV);
           upperLimit  = new Hsv(HH, HS, HV);
            CvInvoke.DestroyWindow("Select marker");
            return new Hsv[] { lowerLimit, upperLimit };

        }

        public bool initialize(Image<Bgr, byte> img)
        {
            initialized = false;
            if ((lowerLimit.Hue==0) && (lowerLimit.Value == 0) && (lowerLimit.Satuation == 0) && (upperLimit.Hue == 0) && (upperLimit.Value == 0) && (upperLimit.Satuation == 0))
            {
                return initialized;
            }
            
            Triangle2DF markersTriangle;
            if (mType == markersType.Biggest)
            {
                markersTriangle = FindMarkers3Biggest(img.Convert<Hsv, byte>());
            }
            else
            {
                markersTriangle = FindMarkers(img.Convert<Hsv, byte>());
            }

            if (markersTriangle.Equals(new Triangle2DF()))
            {
                return initialized;
            }

            LineSegment2DF[] lines = new LineSegment2DF[] { new LineSegment2DF(markersTriangle.V0, markersTriangle.V1), new LineSegment2DF(markersTriangle.V0, markersTriangle.V2), new LineSegment2DF(markersTriangle.V1, markersTriangle.V2) };
            Dictionary<int, double> dict_lines = new Dictionary<int, double>();
            dict_lines.Add(0, lines[0].Length);
            dict_lines.Add(1, lines[1].Length);
            dict_lines.Add(2, lines[2].Length);
            var index = dict_lines.OrderByDescending(v => v.Value).Take(1);
            initLine = lines[index.First().Key];
            initialized = true;

            return initialized;
        }
    }
}
