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_draw : MonoBehaviour
{

    public Camera cam;

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

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


    private int width = 600;
    private int height = 424;

    private MatOfPoint2f dstPoints;
    private Texture2D planeOutText;

    private Mat camImageMat;

    private Renderer planeRend;

    public int DarknessThreshold;

    private Mat drawingMat;


    void Start()
    {


        dstPoints = new MatOfPoint2f();
        dstPoints.alloc(4);
        /*dstPoints.put(0, 0, 0, height);
        dstPoints.put(1, 0, width, height);
        dstPoints.put(3, 0, width, 0);
        dstPoints.put(2, 0, 0, 0);*/

        dstPoints.put(2, 0, 0, 0);
        dstPoints.put(3, 0, width, 0);
        dstPoints.put(0, 0, 0, height);
        dstPoints.put(1, 0, width, height);

        planeRend = GameObject.Find("Plane").GetComponent<Renderer>();

        planeOutText = new Texture2D(width, height, TextureFormat.RGBA32, false);



    }

    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);
                    drawingMat = new Mat(camImageMat.rows(), camImageMat.cols(), CvType.CV_8UC4, new Scalar(0, 0, 0, 0));

                }
                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;
            Vector3 fingerPnt = fingerQuad.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);

            Vector3 hUV5 = worldToImage.MultiplyPoint3x4(fingerPnt);


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

            Vector2 uv5 = new Vector2(hUV5.x, hUV5.y) / hUV5.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));

            Point imgPnt5 = new Point(uv5.x, maxV - uv5.y);

            Mat camImgCopy = camImageMat.clone();
            Mat outputMat = camImgCopy.clone();


            if (false)
            {
                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 = Calib3d.findHomography(imagePoints, dstPoints);



            Mat output = camImgCopy.clone();
            //Mat outputSkullMat = skullTextureMat.clone();

            Imgproc.warpPerspective(camImageMat, output, homo, output.size());



            Imgproc.circle(output, new Point(45, 390), 5, new Scalar(255, 0, 0, 255));


            var r = output.get(390, 45)[0];
            var g = output.get(390, 45)[1];
            var b = output.get(390, 45)[2];

            var c_r = output.get(215, 45)[0];
            var c_g = output.get(215, 45)[1];
            var c_b = output.get(215, 45)[2];


            Mat yetAnotherClone = camImgCopy.clone();


            if (r+g+b < DarknessThreshold)
            {
                print("finger lol");

                var lelImg = output.clone();

                Imgproc.cvtColor(output, lelImg, Imgproc.COLOR_BGR2HSV);

                var newOutput = output.clone();
                Core.inRange(lelImg, new Scalar(r - 30, g - 30, b - 30), new Scalar(r + 30, g + 30, b + 30), newOutput);


                var nicePoint = LocateFinger(newOutput);
                if (nicePoint != null)
                {
                    print("Finger Found");
                    Imgproc.circle(drawingMat, nicePoint, 5, new Scalar(c_r, c_g, c_b, 255), -1);

                }


                MatDisplay.MatToTexture(newOutput, ref planeOutText);

                planeRend.sharedMaterial.mainTexture = planeOutText;

            }

            Mat circleClone = drawingMat.clone();

            Imgproc.warpPerspective(drawingMat, circleClone, homo.inv(), circleClone.size());

            Core.addWeighted(camImageMat, 0.95f, circleClone, 0.7f, 0.0f, yetAnotherClone);



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



            imagePoints.Dispose();


        }
    }

    Point LocateFinger(Mat fingerMat)
    {
        for (int i = 70; i < height-30; i++)
        {
            for (int j = 85; j < width-30; j++)
            {
                if (fingerMat.get(i, j)[0] == 255)
                {
                    return new Point(j, i);
                }
            }
        }

        return null;

    }

}