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

using System.Drawing;
using System.Numerics;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Cuda;
using Emgu.CV.Features2D;
using Emgu.CV.Util;
using Emgu.CV.Flann;
using EpsonC3Com;


namespace RoboticManipulatorTracking 
{
    enum FeatureDetectorMetod
    {
        AKAZE,
        BRISK,
        KAZE,
        MSER,
        ORB,
        SIFT,
        DEFAULT

    }

    enum MatchMakingMetod
    {
        BRUTEFORCE,
        FLANN

    }
    class FeatureMatchingTracker : TrackerInterface
    {
        private FeatureDetectorMetod detectorMethod;

        private MatchMakingMetod matchingMethod;

        public KdTreeIndexParams indexParam { get; set; }  = new KdTreeIndexParams(5);

        //public AutotunedIndexParams indexParam = new AutotunedIndexParams();

        public SearchParams searchParam { get; set; } = new SearchParams();

        private Image<Gray, byte> selectedImage;
        private Matrix4x4 e_c { get; set; }
        public bool initialized { get; set; } = false;
        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 PointF[] points { get; set; }
        public FeatureMatchingTracker()
        {
            detectorMethod = FeatureDetectorMetod.SIFT;
            matchingMethod = MatchMakingMetod.FLANN;
        }
        public FeatureMatchingTracker(FeatureDetectorMetod detectorMethod, MatchMakingMetod matchingMethod, double cameraAngleX = 60.7974554 * Math.PI / 180, double cameraAngleY = 47.498989 * Math.PI / 180, double objectDistToKeep = 0, double sphereRadius = 700)
        {
            this.cameraAngleX = cameraAngleX;
            this.cameraAngleY = cameraAngleY;
            this.objectDistToKeep = objectDistToKeep;
            this.sphereRadius = sphereRadius;
            this.matchingMethod = matchingMethod;
            this.detectorMethod = detectorMethod;

        }

        

        public bool initialize(Image<Bgr, byte> img)
        {
            Rectangle initRect;
            initRect = CvInvoke.SelectROI("Select Object", img, fromCenter: false, showCrosshair: true);
            if (!initRect.IsEmpty)
            {
                img.ROI = initRect;
                selectedImage = img.Convert<Gray, byte>().Copy();
                CvInvoke.DestroyWindow("Select Object");
                initialized = true;
                return true;
            }
            CvInvoke.DestroyWindow("Select Object");
            initialized = false;
            return false;
        }

        private static PointF LineIntersection(LineSegment2DF line1, LineSegment2DF line2)
        {
            float a1 = line1.P2.Y - line1.P1.Y;
            float b1 = line1.P1.X - line1.P2.X;
            float c1 = a1 * line1.P1.X + b1 * line1.P1.Y;

            float a2 = line2.P2.Y - line2.P1.Y;
            float b2 = line2.P1.X - line2.P2.X;
            float c2 = a2 * line2.P1.X + b2 * line2.P1.Y;

            float delta = a1 * b2 - a2 * b1;
            //If lines are parallel, the result will be (NaN, NaN).
            return delta == 0 ? new PointF(float.NaN, float.NaN)
                : new PointF((b2 * c1 - b1 * c2) / delta, (a1 * c2 - a2 * c1) / delta);
        }

        private static PointF[] MatchImageBrurteForce(Image<Gray, byte> templateImage, Image<Gray, byte> sceneImage, FeatureDetectorMetod method)
        {
            //try
            //{
            //init
            PointF[] finalPoints = null;
            Mat homography = null;
            VectorOfKeyPoint templateKeyPoints = new VectorOfKeyPoint();
            VectorOfKeyPoint sceneKeyPoints = new VectorOfKeyPoint();
            Mat templateDescriptors = new Mat();
            Mat sceneDescriptors = new Mat();

            VectorOfKeyPoint templateKeyPoints1 = new VectorOfKeyPoint();
            VectorOfKeyPoint sceneKeyPoints1 = new VectorOfKeyPoint();

            Mat mask;
            int k = 2;
            double uniquenessthreshold = 0.80;
            VectorOfVectorOfDMatch matches = new VectorOfVectorOfDMatch();

            int nonZeroCount = 0;

            //if (CudaInvoke.HasCuda)
            //{
            //    uniquenessthreshold = 0.70;
            //    CudaORBDetector cudaORB = new CudaORBDetector(20000);
            //    CudaFastFeatureDetector detect = new CudaFastFeatureDetector(20);
            //    using (GpuMat gpuTemplateImage = new GpuMat(templateImage.Mat))
            //    //extract features from the object image
            //    using (GpuMat gpuTemplateKeyPoints = new GpuMat())
            //    using (GpuMat gpuTemplateDescriptors = new GpuMat())
            //    using (CudaBFMatcher matcher = new CudaBFMatcher(DistanceType.Hamming))
            //    using (GpuMat gpuSceneImage = new GpuMat(sceneImage.Mat))
            //    using (GpuMat gpuSceneKeyPoints = new GpuMat())
            //    using (GpuMat gpuSceneDescriptors = new GpuMat())
            //    {

            //        detect.DetectRaw(gpuTemplateImage, templateKeyPoints);
            //        cudaORB.DetectRaw(gpuTemplateImage, templateKeyPoints);
            //        cudaORB.Compute(gpuTemplateImage, templateKeyPoints, gpuTemplateDescriptors);

            //        detect.DetectRaw(gpuSceneImage, sceneKeyPoints);
            //        cudaORB.DetectRaw(gpuSceneImage, sceneKeyPoints);
            //        cudaORB.Compute(gpuSceneImage, sceneKeyPoints, gpuSceneDescriptors);

            //        matcher.KnnMatch(gpuSceneDescriptors, gpuTemplateDescriptors, matches, k);

            //        mask = new Mat(matches.Size, 1, Emgu.CV.CvEnum.DepthType.Cv8U, 1);
            //            mask.SetTo(new MCvScalar(255));
            //            Features2DToolbox.VoteForUniqueness(matches, uniquenessthreshold, mask);

            //        nonZeroCount = CudaInvoke.CountNonZero(mask);


            //        Mat result = new Mat();

            //        if (nonZeroCount >= 50)
            //        {
            //            homography = Features2DToolbox.GetHomographyMatrixFromMatchedFeatures(templateKeyPoints, sceneKeyPoints, matches, mask, 2);

            //        }

            //        if (homography != null)
            //        {
            //            Rectangle rect = new Rectangle(System.Drawing.Point.Empty, templateImage.Size);
            //            PointF[] pts = new PointF[]
            //            {
            //            new PointF(rect.Left,rect.Bottom),
            //            new PointF(rect.Right,rect.Bottom),
            //            new PointF(rect.Right,rect.Top),
            //            new PointF(rect.Left,rect.Top)
            //            };

            //            pts = CvInvoke.PerspectiveTransform(pts, homography);
            //            System.Drawing.Point[] points = Array.ConvertAll<PointF, System.Drawing.Point>(pts, System.Drawing.Point.Round);
            //            LineSegment2DF line1 = new LineSegment2DF(points[0], points[2]);
            //            LineSegment2DF line2 = new LineSegment2DF(points[1], points[3]);
            //            PointF p = LineIntersection(line1, line2);
            //            PointF stred = new PointF((int)sceneImage.Width / 2, (int)sceneImage.Height / 2);
            //            LineSegment2DF l = new LineSegment2DF(p, stred);
            //            if (l.Length > sceneImage.Width / 5)
            //            {
            //                return finalPoints;
            //            }

            //            finalPoints = pts;

            //        }


            //    }
            //}
            //else
            //{
                //feature detection in template and scene
                Feature2D featureDetector;
                BFMatcher matcher;

                switch (method)
                {
                    case FeatureDetectorMetod.AKAZE:
                        featureDetector = new AKAZE();
                        matcher = new BFMatcher(DistanceType.L2);
                        break;
                    case FeatureDetectorMetod.BRISK:
                        featureDetector = new Brisk();
                        matcher = new BFMatcher(DistanceType.Hamming);
                        break;
                    case FeatureDetectorMetod.KAZE:
                        featureDetector = new KAZE();
                        matcher = new BFMatcher(DistanceType.L2);
                        break;
                    case FeatureDetectorMetod.ORB:
                        featureDetector = new ORBDetector();
                        matcher = new BFMatcher(DistanceType.Hamming);
                        break;
                    case FeatureDetectorMetod.SIFT:
                        featureDetector = new SIFT(edgeThreshold: 10, contrastThreshold: 0.03);
                        matcher = new BFMatcher(DistanceType.L2);
                        break;
                    default:
                        featureDetector = new SIFT();
                        matcher = new BFMatcher(DistanceType.L2);
                        break;
                }

                featureDetector.DetectAndCompute(templateImage, null, templateKeyPoints, templateDescriptors, false);
                featureDetector.DetectAndCompute(sceneImage, null, sceneKeyPoints, sceneDescriptors, false);

                //matching
                matcher.Add(templateDescriptors);
                matcher.KnnMatch(sceneDescriptors, matches, k);

                mask = new Mat(matches.Size, 1, Emgu.CV.CvEnum.DepthType.Cv8U, 1);
                mask.SetTo(new MCvScalar(255));

                Features2DToolbox.VoteForUniqueness(matches, uniquenessthreshold, mask);

                nonZeroCount = CvInvoke.CountNonZero(mask);


                if (nonZeroCount >= 20)
                {
                    nonZeroCount = Features2DToolbox.VoteForSizeAndOrientation(templateKeyPoints, sceneKeyPoints, matches, mask, 1.5, 36);
                }
                Mat result = new Mat();


                if (nonZeroCount >= 20)
                {
                    homography = Features2DToolbox.GetHomographyMatrixFromMatchedFeatures(templateKeyPoints, sceneKeyPoints, matches, mask, 2);

                }

                if (homography != null)
                {
                    Rectangle rect = new Rectangle(System.Drawing.Point.Empty, templateImage.Size);
                    PointF[] pts = new PointF[]
                    {
                        new PointF(rect.Left,rect.Bottom),
                        new PointF(rect.Right,rect.Bottom),
                        new PointF(rect.Right,rect.Top),
                        new PointF(rect.Left,rect.Top)
                    };

                    pts = CvInvoke.PerspectiveTransform(pts, homography);

                    System.Drawing.Point[] points = Array.ConvertAll<PointF, System.Drawing.Point>(pts, System.Drawing.Point.Round);
                    LineSegment2DF line1 = new LineSegment2DF(points[0], points[2]);
                    LineSegment2DF line2 = new LineSegment2DF(points[1], points[3]);
                    PointF p = LineIntersection(line1, line2);
                    PointF stred = new PointF((int)sceneImage.Width / 2, (int)sceneImage.Height / 2);
                    LineSegment2DF l = new LineSegment2DF(p, stred);
                    if (l.Length > sceneImage.Width / 5)
                    {
                        return finalPoints;
                    }

                    finalPoints = pts;

                }

        //parenthesis from cuda implementation
            //}

            return finalPoints;
            //}
            //catch (Exception ex)
            //{

            //    throw new Exception (ex.Message);
            //}

        }

        private static PointF[] MatchImageFLANN(Image<Gray, byte> templateImage, Image<Gray, byte> sceneImage, FeatureDetectorMetod method)
        {
            //try
            //{
            //init
            PointF[] finalPoints = null;
            Mat homography = null;
            VectorOfKeyPoint templateKeyPoints = new VectorOfKeyPoint();
            VectorOfKeyPoint sceneKeyPoints = new VectorOfKeyPoint();
            Mat templateDescriptors = new Mat();
            Mat sceneDescriptors = new Mat();

            Mat mask;
            int k = 2;
            double uniquenessthreshold = 0.8;
            VectorOfVectorOfDMatch matches = new VectorOfVectorOfDMatch();



            //feature detection in template and scene
            Feature2D featureDetector;

            switch (method)
            {
                case FeatureDetectorMetod.KAZE:
                    featureDetector = new KAZE();
                    break;
                case FeatureDetectorMetod.SIFT:
                    featureDetector = new SIFT(edgeThreshold: 10, contrastThreshold: 0.03);
                    break;
                default:
                    featureDetector = new SIFT();
                    break;
            }


            featureDetector.DetectAndCompute(templateImage, null, templateKeyPoints, templateDescriptors, false);
            featureDetector.DetectAndCompute(sceneImage, null, sceneKeyPoints, sceneDescriptors, false);

            //matching
            KdTreeIndexParams indexParam = new KdTreeIndexParams(5);
            SearchParams searchParam = new SearchParams();

            FlannBasedMatcher matcher = new FlannBasedMatcher(indexParam, searchParam);


            matcher.Add(templateDescriptors);
            matcher.KnnMatch(sceneDescriptors, matches, k);

            mask = new Mat(matches.Size, 1, Emgu.CV.CvEnum.DepthType.Cv8U, 1);
            mask.SetTo(new MCvScalar(255));

            Features2DToolbox.VoteForUniqueness(matches, uniquenessthreshold, mask);
            
            int nonZeroCount = CvInvoke.CountNonZero(mask);


            if (nonZeroCount >= 100)
            {
                nonZeroCount = Features2DToolbox.VoteForSizeAndOrientation(templateKeyPoints, sceneKeyPoints, matches, mask, 1.5, 36*2);
            }
            if (nonZeroCount >= 30)
            {
                homography = Features2DToolbox.GetHomographyMatrixFromMatchedFeatures(templateKeyPoints, sceneKeyPoints, matches, mask, 8);
            }

            if (homography != null)
            {
                Rectangle rect = new Rectangle(System.Drawing.Point.Empty, templateImage.Size);
                PointF[] pts = new PointF[]
                {
                        new PointF(rect.Left,rect.Bottom),
                        new PointF(rect.Right,rect.Bottom),
                        new PointF(rect.Right,rect.Top),
                        new PointF(rect.Left,rect.Top)
                };

                pts = CvInvoke.PerspectiveTransform(pts, homography);
                System.Drawing.Point[] points = Array.ConvertAll<PointF, System.Drawing.Point>(pts, System.Drawing.Point.Round);

                LineSegment2DF line1 = new LineSegment2DF(points[0], points[2]);
                LineSegment2DF line2 = new LineSegment2DF(points[1], points[3]);
                PointF p = LineIntersection(line1, line2);
                PointF stred = new PointF((int)sceneImage.Width / 2, (int)sceneImage.Height / 2);
                LineSegment2DF l = new LineSegment2DF(p, stred);

                if (l.Length > sceneImage.Width/5)
                {
                    return finalPoints;
                }
                finalPoints = pts;
            }

            return finalPoints;

        }

        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)
        {
            //PointF[] points;
            if (initialized)
            {
                switch (matchingMethod)
                {
                    case MatchMakingMetod.FLANN:
                        points = MatchImageFLANN(selectedImage, img.Convert<Gray, byte>(), detectorMethod);
                        break;
                    case MatchMakingMetod.BRUTEFORCE:
                        points = MatchImageBrurteForce(selectedImage, img.Convert<Gray, byte>(), detectorMethod);
                        break;
                    default:
                        points = MatchImageFLANN(selectedImage, img.Convert<Gray, byte>(), detectorMethod);
                        break;
                }

                if (points != null)
                {
                    LineSegment2DF line1 = new LineSegment2DF(points[0], points[2]);
                    LineSegment2DF line2 = new LineSegment2DF(points[1], points[3]);
                    PointF p = LineIntersection(line1, line2);
                    MCvPoint3D64f pointInPicture = new MCvPoint3D64f(p.X, p.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, 0, b_e);

                    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;
                }
            }

            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 = 700)
        {   
            //PointF[] points;
            if (initialized)
            {
                switch (matchingMethod)
                {
                    case MatchMakingMetod.FLANN:
                        points = MatchImageFLANN(selectedImage, img.Convert<Gray, byte>(), detectorMethod);
                        break;
                    case MatchMakingMetod.BRUTEFORCE:
                        points = MatchImageBrurteForce(selectedImage, img.Convert<Gray, byte>(), detectorMethod);
                        break;
                    default:
                        points = MatchImageFLANN(selectedImage, img.Convert<Gray, byte>(),detectorMethod);
                        break;
                }

                if (points != null)
                {
                    LineSegment2DF line1 = new LineSegment2DF(points[0], points[2]);
                    LineSegment2DF line2 = new LineSegment2DF(points[1], points[3]);
                    PointF p = LineIntersection(line1, line2);
                    //doplnit podminku pro maximalni vzdalenost noveho bodu od stredu, muze byt i podminka o uhlu
                    PointF stred = new PointF((int)img.Width / 2, (int)img.Height / 2);
                    LineSegment2DF l = new LineSegment2DF(p, stred);
                    if (l.Length > 450)
                    {
                        newSysCoor = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
                        return false;
                    }
                    MCvPoint3D64f pointInPicture = new MCvPoint3D64f(p.X, p.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, 0, b_e);


                    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;
                }
            }
            newSysCoor = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
            return false;
            
        }
       
    }
}
