using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect.Fusion;
namespace RoboLibrary
{
    public class CameraToRobotCalibrator
    {
        //offset in mm compared to robot origo
        public float x_offset = -0.135f;
        public float y_offset = -1.095f;
        public float z_offset = 1.738f;

        //scale
        public float s_x = -1f;
        public float s_y = 1f;
        public float s_z = -1f;

        //camera rotated around its own origo
        public float x_rotation = 0f;
        public float y_rotation = 0f;
        public float z_rotation = 90;

        public Matrix4 TranslationMatrix;

        public ComputerVisionLibrary.Mesh ConvertToRobospace(ComputerVisionLibrary.Mesh mesh)
        {
            Matrix4 transformMatrix = CalculateTransformMatrix();
            List<Vector3> vertices = mesh.Vertices;
            List<Vector3> transformedVertices = new List<Vector3>();
            foreach (var v in vertices)
            {
                transformedVertices.Add(TransformVertex(v, transformMatrix));
            }
            mesh.Vertices = transformedVertices;
            return mesh;
        }

        Matrix4 t; //translation
        Matrix4 x; //rotation x
        Matrix4 y; //rotation y
        Matrix4 z; //rotation z
        Matrix4 s; //scale

        public float ToRadians(float degrees)
        {
            return (float)(degrees * (Math.PI / 180));
        }
        public void SetUpMatrixes()
        {
            x_rotation = ToRadians(x_rotation);
            y_rotation = ToRadians(y_rotation);
            z_rotation = ToRadians(z_rotation);

            t.M11 = 1; t.M12 = 0; t.M13 = 0; t.M14 = x_offset;
            t.M21 = 0; t.M22 = 1; t.M23 = 0; t.M24 = y_offset;
            t.M31 = 0; t.M32 = 0; t.M33 = 1; t.M34 = z_offset;
            t.M41 = 0; t.M42 = 0; t.M43 = 0; t.M44 = 1;


            x.M11 = 1; x.M12 = 0; x.M13 = 0; x.M14 = 0;
            x.M21 = 0; x.M22 = (float)Math.Cos(x_rotation); x.M23 = -(float)Math.Sin(x_rotation); x.M24 = 0;
            x.M31 = 0; x.M32 = (float)Math.Sin(x_rotation); x.M33 = (float)Math.Cos(x_rotation); x.M34 = 0;
            x.M41 = 0; x.M42 = 0; x.M43 = 0; x.M44 = 1;

            y = new Matrix4();
            y.M11 = (float)Math.Cos(y_rotation); y.M12 = 0; y.M13 = (float)Math.Sin(y_rotation); y.M14 = 0;
            y.M21 = 0; y.M22 = 1; y.M23 = 0; y.M24 = 0;
            y.M31 = -(float)Math.Sin(y_rotation); y.M32 = 0; y.M33 = (float)Math.Cos(y_rotation); y.M34 = 0;
            y.M41 = 0; y.M42 = 0; y.M43 = 0; y.M44 = 1;

            z = new Matrix4();
            z.M11 = (float)Math.Cos(z_rotation); z.M12 = -(float)Math.Sin(z_rotation); z.M13 = 0; z.M14 = 0;
            z.M21 = (float)Math.Sin(z_rotation); z.M22 = (float)Math.Cos(z_rotation); z.M23 = 0; z.M24 = 0;
            z.M31 = 0; z.M32 = 0; z.M33 = 1; z.M34 = 0;
            z.M41 = 0; z.M42 = 0; z.M43 = 0; z.M44 = 1;

            s = new Matrix4();
            s.M11 = s_x; s.M12 = 0; s.M13 = 0; s.M14 = 0;
            s.M21 = 0; s.M22 = s_y; s.M23 = 0; s.M24 = 0;
            s.M31 = 0; s.M32 = 0; s.M33 = s_z; s.M34 = 0;
            s.M41 = 0; s.M42 = 0; s.M43 = 0; s.M44 = 1;

        }

        public static Matrix4 Multiply(Matrix4 a, Matrix4 b)
        {
            Matrix4 c = new Matrix4();
            c.M11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31 + a.M14 * b.M41;
            c.M12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32 + a.M14 * b.M42;
            c.M13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33 + a.M14 * b.M43;
            c.M14 = a.M11 * b.M14 + a.M12 * b.M24 + a.M13 * b.M43 + a.M14 * b.M44;

            c.M21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31 + a.M24 * b.M41;
            c.M22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32 + a.M24 * b.M42;
            c.M23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33 + a.M24 * b.M43;
            c.M24 = a.M21 * b.M14 + a.M22 * b.M24 + a.M23 * b.M43 + a.M24 * b.M44;

            c.M31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31 + a.M34 * b.M41;
            c.M32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32 + a.M34 * b.M42;
            c.M33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33 + a.M34 * b.M43;
            c.M34 = a.M31 * b.M14 + a.M32 * b.M24 + a.M33 * b.M43 + a.M34 * b.M44;

            c.M41 = a.M41 * b.M11 + a.M42 * b.M21 + a.M43 * b.M31 + a.M44 * b.M41;
            c.M42 = a.M41 * b.M12 + a.M42 * b.M22 + a.M43 * b.M32 + a.M44 * b.M42;
            c.M43 = a.M41 * b.M13 + a.M42 * b.M23 + a.M43 * b.M33 + a.M44 * b.M43;
            c.M44 = a.M41 * b.M14 + a.M42 * b.M24 + a.M43 * b.M43 + a.M44 * b.M44;

            return c;
        }

        public Matrix4 CalculateTransformMatrix()
        {
            //rotate the Kinect 90 degrees around the z-axis
            //mirror x
            //mirror z
            Matrix4 rotated = z;
            Matrix4 mirrored = Multiply(s, rotated);
            Matrix4 translated = Multiply(t, mirrored);
            return translated;
        }

        public Vector3 TransformVertex(Vector3 vertex, Matrix4 transformM)
        {
            float x, y, z;
            x = transformM.M11 * vertex.X + transformM.M12 * vertex.Y + transformM.M13 * vertex.Z + transformM.M14 * 1;
            y = transformM.M21 * vertex.X + transformM.M22 * vertex.Y + transformM.M23 * vertex.Z + transformM.M24 * 1;
            z = transformM.M31 * vertex.X + transformM.M32 * vertex.Y + transformM.M33 * vertex.Z + transformM.M34 * 1;
            return new Vector3() { X = x, Y = y, Z = z };
        }
    }
}