using OpenCVForUnity.CoreModule;
using OpenCVForUnity.ImgprocModule;
using OpenCVForUnity.ObjdetectModule;
using OpenCVForUnity.UnityUtils;
using OpenCVForUnity.UtilsModule;
using UnityEngine;
using OpenCVForUnity.Calib3dModule;
using System.Linq;
using Vuforia;

public class Homo_script : MonoBehaviour
{

    public Camera cam;

    public GameObject corner1;
    public GameObject corner2;
    public GameObject corner3;
    public GameObject corner4;

    //Default values: guessed, not calibrated
    public float fx = 650;
    public float fy = 650;
    public float cx = 320;
    public float cy = 240;


    private int width = 420;
    private int height = 420;

    private MatOfPoint2f dstPoints;
    private Texture2D outputTexture;

    private Mat camImageMat;
    private byte[] texData;

    private Renderer rend;

    
    private GameObject pls;

    private bool manualComputation = false;

    void Start()
    {


        dstPoints = new MatOfPoint2f();
        dstPoints.alloc(4);
        dstPoints.put(3, 0, width, height);
        dstPoints.put(2, 0, 0, height);
        dstPoints.put(1, 0, width, 0);
        dstPoints.put(0, 0, 0, 0);
        outputTexture = new Texture2D(width, height, TextureFormat.RGBA32, false);


        pls = GameObject.Find("flying_skull_001");
        rend = pls.GetComponent<Renderer>();

        // Debugging
        pls = GameObject.Find("pls");
        rend = pls.GetComponent<Renderer>();

    }

    void Update()
    {

        //Access camera image provided by Vuforia
        Image camImg = CameraDevice.Instance.GetCameraImage(Image.PIXEL_FORMAT.RGBA8888);

        if (camImg != null)
        {
            if (camImg != null)
            {
                if (camImageMat == null)
                {
                    camImageMat = new Mat(camImg.Height, camImg.Width, CvType.CV_8UC4);
                }
                camImageMat.put(0, 0, camImg.Pixels);
            }

            //---- TRANSFORM/PROJECT CORNER WORLD COORDINATES TO IMAGE COORDINATES ----

            Vector3 worldPnt1 = corner1.transform.position;
            Vector3 worldPnt2 = corner2.transform.position;
            Vector3 worldPnt3 = corner3.transform.position;
            Vector3 worldPnt4 = corner4.transform.position;

            //See lecture slides
            Matrix4x4 Rt = cam.transform.worldToLocalMatrix;
            Matrix4x4 A = Matrix4x4.identity;
            A.m00 = fx;
            A.m11 = fy;
            A.m02 = cx;
            A.m12 = cy;

            //See equation for pinhole camera model
            Matrix4x4 worldToImage = A * Rt;

            //Apply transform to get homogeneous image coordinates
            Vector3 hUV1 = worldToImage.MultiplyPoint3x4(worldPnt1);
            Vector3 hUV2 = worldToImage.MultiplyPoint3x4(worldPnt2);
            Vector3 hUV3 = worldToImage.MultiplyPoint3x4(worldPnt3);
            Vector3 hUV4 = worldToImage.MultiplyPoint3x4(worldPnt4);

            //hUV are the image coordinates in homogeneous coordinates, we need to normalize, i.e., divide by Z to get to Cartesian coordinates
            Vector2 uv1 = new Vector2(hUV1.x, hUV1.y) / hUV1.z;
            Vector2 uv2 = new Vector2(hUV2.x, hUV2.y) / hUV2.z;
            Vector2 uv3 = new Vector2(hUV3.x, hUV3.y) / hUV3.z;
            Vector2 uv4 = new Vector2(hUV4.x, hUV4.y) / hUV4.z;

            //Do not forget to alloc before putting values into a MatOfPoint2f (see Start() above)
            //We need to flip the v-coordinates, see coordinate system overview
            float maxV = camImg.Height - 1;

            MatOfPoint2f imagePoints = new MatOfPoint2f();
            imagePoints.alloc(4);

            imagePoints.put(0, 0, uv1.x, maxV - uv1.y);
            imagePoints.put(1, 0, uv2.x, maxV - uv2.y);
            imagePoints.put(2, 0, uv3.x, maxV - uv3.y);
            imagePoints.put(3, 0, uv4.x, maxV - uv4.y);


            //Debug draw points using OpenCV's drawing functions
            Point imgPnt1 = new Point(imagePoints.get(0, 0));
            Point imgPnt2 = new Point(imagePoints.get(1, 0));
            Point imgPnt3 = new Point(imagePoints.get(2, 0));
            Point imgPnt4 = new Point(imagePoints.get(3, 0));
            Mat camImgCopy = camImageMat.clone();
            Mat outputMat = camImgCopy.clone();


            if (true)
            {
                Imgproc.circle(camImageMat, imgPnt1, 5, new Scalar(255, 0, 0, 255));
                Imgproc.circle(camImageMat, imgPnt2, 5, new Scalar(0, 255, 0, 255));
                Imgproc.circle(camImageMat, imgPnt3, 5, new Scalar(0, 0, 255, 255));
                Imgproc.circle(camImageMat, imgPnt4, 5, new Scalar(255, 255, 0, 255));
            }



            Mat homo = new Mat();
            if (manualComputation)
            {
                homo = ComputeHomo(imagePoints, dstPoints);
            }
            else
            {
                homo = Calib3d.findHomography(imagePoints, dstPoints);
            }

            Imgproc.warpPerspective(camImgCopy, outputMat, homo, new Size(outputMat.width(), outputMat.height()));

            var rectOutputMat = new Mat(outputMat, new OpenCVForUnity.CoreModule.Rect(0, 0, width, height));

            MatDisplay.MatToTexture(rectOutputMat, ref outputTexture);

  
            //Display the Mat that includes video feed and debug points
            MatDisplay.DisplayMat(camImageMat, MatDisplaySettings.FULL_BACKGROUND);

            rend.sharedMaterial.mainTexture = outputTexture;

            // Makes camera prettier
            cam.fieldOfView = 2 * Mathf.Atan(camImg.Height * 0.5f / fy) * Mathf.Rad2Deg;

            imagePoints.Dispose();

        }
    }

    Mat ComputeHomo(MatOfPoint2f imgPoints, MatOfPoint2f destPoints)
    {


        Mat H = new Mat(8, 1, CvType.CV_32FC1);
        Mat A = new Mat(8, 8, CvType.CV_32FC1);
        Mat b = new Mat(8, 1, CvType.CV_32FC1);


        for (int i = 0; i < 4; i++)
        {
            var u = destPoints.get(i, 0)[0];
            var v = destPoints.get(i, 0)[1];

            b.put(i * 2, 0, u);
            b.put((i * 2)+1, 0, v);

            var x = imgPoints.get(i, 0)[0];
            var y = imgPoints.get(i, 0)[1];
            
            A.put(i*2, 0, x, y, 1, 0, 0, 0, -u*x, -u*y);
            A.put((i * 2)+1, 0, 0, 0, 0, x, y, 1, -v*x, -v*y);

        }

        Core.solve(A, b, H);

        Mat ShitsReal = new Mat(3, 3, CvType.CV_32FC1);
        ShitsReal.put(0, 0, H.get(0,0)[0], H.get(1,0)[0], H.get(2,0)[0]);
        ShitsReal.put(1, 0, H.get(3,0)[0], H.get(4,0)[0], H.get(5,0)[0]);
        ShitsReal.put(2, 0, H.get(6,0)[0], H.get(7,0)[0], 1);

        return ShitsReal;
    }

}