using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Threading;
using Emgu.CV;
using Emgu.CV.Structure;
using System.Drawing;
using System.Numerics;
using EpsonC3Com;


namespace RoboticManipulatorTracking
{
    
    enum Trackers
    {
        CSRT,
        KCF
        //MIL,
        //MOSSE,
        //TLD
    }
    class OpenCvTracker : TrackerInterface
    {
        private Emgu.CV.Tracking.Tracker tracker;
        public Trackers trackerUsed { get; }
        public bool initialized { get; set; } = false;
        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; } = 800;
        public Rectangle box { get; set; }
        public OpenCvTracker()
        {
            tracker = new Emgu.CV.Tracking.TrackerCSRT();
            trackerUsed = Trackers.CSRT;
        }
        public OpenCvTracker(Trackers tracker, double cameraAngleX = 60.7974554 * Math.PI / 180, double cameraAngleY = 47.498989 * Math.PI / 180, double objectDistToKeep = 0, double sphereRadius = 800)
        {
            switch (tracker)
            {
                case Trackers.CSRT:
                    this.tracker = new Emgu.CV.Tracking.TrackerCSRT();
                    trackerUsed = Trackers.CSRT;
                    break;
                case Trackers.KCF:
                    this.tracker = new Emgu.CV.Tracking.TrackerKCF();
                    trackerUsed = Trackers.KCF;
                    break;
                default:
                    this.tracker = new Emgu.CV.Tracking.TrackerCSRT();
                    trackerUsed = Trackers.CSRT;
                    break;
            }

            this.cameraAngleX = cameraAngleX;
            this.cameraAngleY = cameraAngleY;
            this.objectDistToKeep = objectDistToKeep;
            this.sphereRadius = sphereRadius;

        }
        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)
        {
            bool success;
            if (initialized)
            {
                Rectangle box1;
                Mat mat = img.Mat;
                success = tracker.Update(img.Mat, out box1);  
                box = box1;
                if (success)
                {
                    MCvPoint3D64f pointInPicture = new MCvPoint3D64f((box.X + (box.Width / 2)), (box.Y + (box.Height / 2)), 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 = 800)
        {
            bool success;
            if (initialized)
            {

                Rectangle box1;
                success = tracker.Update(img.Mat, out box1);
                box = box1;
                if (success)
                {
                    MCvPoint3D64f pointInPicture = new MCvPoint3D64f((box.X + (box.Width / 2)), (box.Y + (box.Height / 2)), 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 initialize(Image<Bgr, byte> img)
        {
            Rectangle initRect = new Rectangle(0, 0, 0, 0);
            initRect = CvInvoke.SelectROI("Select Object", img, fromCenter: false, showCrosshair: true);
            //Mat mat = img.Mat;
            bool success = false;
            if (!initRect.IsEmpty)
            {
                if (initialized == true)
                {
                    tracker.Dispose();
                    switch (trackerUsed)
                    {
                        case Trackers.CSRT:
                            tracker = new Emgu.CV.Tracking.TrackerCSRT();
                            break;
                        case Trackers.KCF:
                            tracker = new Emgu.CV.Tracking.TrackerKCF();
                            break;
                        default:
                            tracker = new Emgu.CV.Tracking.TrackerCSRT();
                            break;
                    }

                    success = tracker.Init(img.Mat, initRect);
                }
                else
                {
                    success = tracker.Init(img.Mat, initRect);
                }
            }
            CvInvoke.DestroyWindow("Select Object");
            initialized = success;
            return success;
        }
    }
}
