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



namespace RoboticManipulatorTracking
{

    class EfektorCoordinates
    {
        public Matrix4x4 e_c { get; set; } //= new Matrix4x4(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
        public double cameraAngleX { get; set; } = 60.7974554 * Math.PI / 180;
        public double cameraAngleY { get; set; } = 47.498989 * Math.PI / 180;
        private double objectDistToKeep { get; set; } = 0;
        public double sphereRadius { get; set; } = 700;
        public double cz { get; set; } = 250;
        public double offsetX { get; set; } = 0;
        public double offsetY { get; set; } = 0;


        public EfektorCoordinates(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 EfektorCoordinates(float roll_E_C, float pitch_E_C, float yaw_E_C, float x_E_C, float y_E_C, float z_E_C, double cameraAngleX = 60.7974554 * Math.PI / 180, double cameraAngleY = 47.498989 * Math.PI / 180, double objectDistToKeep = 0, double sphereRadius = 700)
        {

            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);
            this.cameraAngleX = cameraAngleX;
            this.cameraAngleY = cameraAngleY;
            this.objectDistToKeep = objectDistToKeep;
            this.sphereRadius = sphereRadius;
        }

        public EfektorCoordinates(Matrix4x4 E_C, double cameraAngleX = 60.7974554 * Math.PI / 180, double cameraAngleY = 47.498989 * Math.PI / 180, double objectDistToKeep = 0, double sphereRadius = 700)
        {

            e_c = E_C;
            this.cameraAngleX = cameraAngleX;
            this.cameraAngleY = cameraAngleY;
            this.objectDistToKeep = objectDistToKeep;
            this.sphereRadius = sphereRadius;
        }

        public void TransMatrixTo6DCoordinates(Matrix4x4 mat, out RobCoord.XYZ sys)
        {
            //verze vypoctu eulerovych uhlu primo z trasformacni matice
            //mat = Matrix4x4.Transpose(mat);
            //double roll = Math.Atan2(mat.M21, mat.M11);
            //double pitch = Math.Atan2(-mat.M31, Math.Sqrt(1 - Math.Pow(mat.M31, 2)));
            //double yaw = Math.Atan2(mat.M32, mat.M33);
            //sys = new RobCoord.XYZ(Math.Round(mat.M14,2), Math.Round(mat.M24,2), Math.Round(mat.M34,2), Math.Round((roll * 180) / Math.PI,2), Math.Round((pitch * 180) / Math.PI,2), Math.Round((yaw * 180) / Math.PI,2));
           
            Quaternion rot;
            Vector3 scale;
            Vector3 trans;
            Matrix4x4.Decompose(mat, out scale, out rot, out trans);
            Quaternions quat = new Quaternions(rot.W, rot.X, rot.Y, rot.Z);
            double x = 0;
            double y = 0;
            double z = 0;
            quat.GetEuler(ref x, ref y, ref z);
            sys = new RobCoord.XYZ(Math.Round(trans.X,2), Math.Round(trans.Y,2), Math.Round(trans.Z,2), Math.Round((z * 180) / Math.PI,2), Math.Round((y * 180) / Math.PI,2), Math.Round((x * 180) / Math.PI,2));

            return;
        }

        private MCvPoint3D64f PixelsToLengthTranslation(MCvPoint3D64f Point, Size PictureSize)
        {
            MCvPoint3D64f length;
            if ((cameraAngleX == 0) || (cameraAngleY == 0))
            {
                length.X = Point.X;
                length.Y = Point.Y;
                length.Z = Point.Z;
                return (length);
            }

            double pixelInMetricX = (Math.Tan(cameraAngleX / 2) * Point.Z) / (PictureSize.Width / 2);
            double pixelInMetricY = (Math.Tan(cameraAngleY / 2) * Point.Z) / (PictureSize.Height / 2);
            length.X = Math.Round((Point.X - PictureSize.Width / 2) * pixelInMetricX + offsetX,2);
            length.Y = Math.Round((Point.Y - PictureSize.Height / 2) * pixelInMetricY + offsetY,2);
            length.Z = Point.Z;

            return (length);
        }


        private MCvPoint3D64f SphereIntersection(Vector3 point)
        {
            double cx = 0;
            double cy = 0;
            

            double px = 0;
            double py = 0;
            double pz = cz;

            double vx = point.X - px;
            double vy = point.Y - py;
            double vz = point.Z - pz;

            double A = Math.Pow(vx, 2) + Math.Pow(vy, 2) + Math.Pow(vz, 2);
            double B = 2.0 * (px * vx + py * vy + pz * vz - vx * cx - vy * cy - vz * cz);
            double C = Math.Pow(px, 2) - 2 * px * cx + Math.Pow(cx, 2) + Math.Pow(py, 2) - 2 * py * cy + Math.Pow(cy, 2) +
                       Math.Pow(pz, 2) - 2 * pz * cz + Math.Pow(cz, 2) - Math.Pow(sphereRadius - cz, 2);

            // discriminant
            double D = Math.Pow(B, 2) - 4 * A * C;



            double t1 = (-B - Math.Sqrt(D)) / (2.0 * A);

            MCvPoint3D64f solution1 = new MCvPoint3D64f(px * (1 - t1) + t1 * point.X,
                                             py * (1 - t1) + t1 * point.Y,
                                             pz * (1 - t1) + t1 * point.Z);


            double t2 = (-B + Math.Sqrt(D)) / (2.0 * A);
            MCvPoint3D64f solution2 = new MCvPoint3D64f(px * (1 - t2) + t2 * point.X,
                                             py * (1 - t2) + t2 * point.Y,
                                             pz * (1 - t2) + t2 * point.Z);

            if ((D < 0) /*|| (t1 > 1) || (t1 < 0) || (t2 > 1) || (t2 < 0)*/)
            {
                return new MCvPoint3D64f(0, 0, 0);
            }

            if (D == 0)
            {
                return solution1;
            }

            // prefer a solution that's on the line segment itself
            if (Math.Abs(t1 - 0.5) < Math.Abs(t2 - 0.5))
            {
                return solution1;
            }

            return solution2;
        }
        private RobCoord.XYZ ComputeNewSphereIntersectionPosition(MCvPoint3D64f sphereIntersectionPoint, Matrix4x4 camera_efector, float object_rotaion)
        {
            RobCoord.XYZ NewCoordinates;
            Matrix4x4 b_e_new;
            double angleZ = -Math.Atan2(sphereIntersectionPoint.X, sphereIntersectionPoint.Y);
            double angleX = -Math.Acos((sphereIntersectionPoint.Z-cz) / Math.Sqrt(Math.Pow(sphereIntersectionPoint.X, 2) + Math.Pow(sphereIntersectionPoint.Y, 2) + Math.Pow((sphereIntersectionPoint.Z-cz), 2)));
            b_e_new = camera_efector * Matrix4x4.CreateRotationZ(object_rotaion) *
                Matrix4x4.CreateRotationX((float)angleX) * Matrix4x4.CreateRotationZ((float)angleZ) *
                Matrix4x4.CreateTranslation((float)sphereIntersectionPoint.X, (float)sphereIntersectionPoint.Y, (float)sphereIntersectionPoint.Z);
            TransMatrixTo6DCoordinates(b_e_new, out NewCoordinates);
            return NewCoordinates;
        }

        public RobCoord.XYZ ComputeNewCoordinates(MCvPoint3D64f PointInPicture, Size PictureSize, float object_rotaion, Matrix4x4 Base_efector)
        {
            RobCoord.XYZ NewCoordinates;
            Matrix4x4 b_e_new;
            MCvPoint3D64f translation = PixelsToLengthTranslation(PointInPicture, PictureSize);
            Matrix4x4 c_e;
            Matrix4x4 b_c;
            Matrix4x4.Invert(e_c, out c_e);

            if (objectDistToKeep == 0)
            {
                b_c = Matrix4x4.CreateRotationZ(object_rotaion) * Matrix4x4.CreateTranslation((float)translation.X, (float)translation.Y, 0) * e_c * Base_efector;
                b_e_new = c_e * b_c;
            }
            else
            {
                b_c = Matrix4x4.CreateRotationZ(object_rotaion) * Matrix4x4.CreateTranslation((float)translation.X, (float)translation.Y, (float)(translation.Z - objectDistToKeep)) * e_c * Base_efector;
                b_e_new = c_e * b_c;
            }

            if (Matrix4x4.Transpose(b_c).M34 < 50)
            {
                NewCoordinates = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
                return NewCoordinates;
            }

            TransMatrixTo6DCoordinates(b_e_new, out NewCoordinates);

            if ((NewCoordinates.z.Real > 50) && ((Math.Pow(NewCoordinates.x.Real, 2) + Math.Pow(NewCoordinates.y.Real, 2) + Math.Pow(NewCoordinates.z.Real, 2)) <= Math.Pow(sphereRadius, 2)))
            {
                return NewCoordinates;
            }


            Vector3 position = Vector3.Transform(new Vector3((float)translation.X,(float)translation.Y,(float)translation.Z),e_c * Base_efector);
            if ((Math.Pow(position.X, 2) + Math.Pow(position.Y, 2) + Math.Pow(position.Z, 2)) >= Math.Pow(sphereRadius, 2))
            {
                NewCoordinates = ComputeNewSphereIntersectionPosition(SphereIntersection(position), c_e, 0);
            }


                if ((NewCoordinates.z.Real > 50) && ((Math.Pow(NewCoordinates.x.Real, 2) + Math.Pow(NewCoordinates.y.Real, 2) + Math.Pow(NewCoordinates.z.Real, 2)) <= Math.Pow(sphereRadius, 2)))
            {
                return NewCoordinates;
            }
            NewCoordinates = new RobCoord.XYZ(0, 0, 0, 0, 0, 0);
            return NewCoordinates;
        }



    }



}


