using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using OpenCVForUnity.CoreModule;
using OpenCVForUnity.ImgprocModule;
using System.Linq;
using Vuforia;

public class detection_script : MonoBehaviour
{
    Mat cameraImageMat;
    Mat stylizedMat = new Mat();
    Mat greyMat = new Mat();


    int width = 1050;
    int height = 1050;

    Texture2D outputTexture;
    MatOfPoint2f dstPoints;
    MatOfPoint2f dstPointsInv;


    Mat skullTextureMat;


    private GameObject pls;
    private Renderer rend;

    // Start is called before the first frame update
    void Start()
    {
        skullTextureMat = MatDisplay.LoadRGBATexture("Resources/flying_skull_tex.png");

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


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




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

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

    // Update is called once per frame
    void Update()
    {
        MatDisplay.SetCameraFoV(41.5f);

        Image cameraImage = CameraDevice.Instance.GetCameraImage(Image.PIXEL_FORMAT.RGBA8888);

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

            Imgproc.cvtColor(cameraImageMat, greyMat, Imgproc.COLOR_RGB2GRAY);


            Imgproc.threshold(greyMat, stylizedMat, 69, 255, Imgproc.THRESH_BINARY);


            List<MatOfPoint> contourList = new List<MatOfPoint>();
            Mat hierarchy = new Mat();
            Imgproc.findContours(stylizedMat, contourList, hierarchy, Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);

            //Imgproc.drawContours(cameraImageMat, contourList, -1, new Scalar(255,0,0), 2);

            List<MatOfPoint2f> squareContours = new List<MatOfPoint2f>();
            foreach (var contour in contourList)
            {
                MatOfPoint2f contour2f = new MatOfPoint2f();
                contour.convertTo(contour2f, CvType.CV_32FC2);

                double epsilon = 0.01f * Imgproc.arcLength(contour2f, true);
                MatOfPoint2f approx = new MatOfPoint2f();
                Imgproc.approxPolyDP(contour2f, approx, epsilon, true);

                if (approx.toList().Count == 4)
                {
                    Imgproc.drawContours(cameraImageMat, new List<MatOfPoint> { contour }, -1, new Scalar(255, 0, 0), 2);
                    squareContours.Add(approx);
                }
            }


            MatOfPoint2f square = findSquare(squareContours);
            if (square == null)
            {
                MatDisplay.DisplayMat(cameraImageMat, MatDisplaySettings.FULL_BACKGROUND);
                // TODO: Maybe display cam first
                return;
            }

            for (int i = 0; i < 4; i++)
            {
                double x = square.get(i, 0)[0];
                double y = square.get(i, 0)[1];
                Imgproc.circle(cameraImageMat, new Point(x, y), 10, new Scalar(0, 255, 0, 255));


            }

            Mat homo = ComputeHomo(square, dstPoints);


            Mat outputSkullMat = cameraImageMat.clone();
            //Mat outputSkullMat = skullTextureMat.clone();

            Imgproc.warpPerspective(skullTextureMat, outputSkullMat, homo.inv(), outputSkullMat.size());







            Mat dstCam = cameraImageMat.clone();


            Core.addWeighted(cameraImageMat, 0.95f, outputSkullMat, 0.7f, 0.0f, dstCam);

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




            /*
            Mat homo = new Mat();
            homo = ComputeHomo(square, dstPoints);

            Mat outputMat = cameraImageMat.clone();

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


            Mat outputSkullMat = skullTextureMat.clone();




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

            MatDisplay.MatToTexture(rectOutputMat, ref outputTexture);


            rend.sharedMaterial.mainTexture = outputTexture;

            */


            //MatDisplay.DisplayMat(cameraImageMat, MatDisplaySettings.FULL_BACKGROUND);

        }
    }



    bool checkSize(double outer, double inner)
    {
        print(outer);
        print(inner);
        return  (outer > inner && outer < (inner * 2));
    }

    MatOfPoint2f findSquare (List<MatOfPoint2f> squareContours)
    {
        foreach (var outer_square in squareContours)
        {
            var outer_maxX = outer_square.toList().Max(point => point.x);
            var outer_maxY = outer_square.toList().Max(point => point.y);

            var outer_minX = outer_square.toList().Min(point => point.x);
            var outer_minY = outer_square.toList().Min(point => point.y);

            var outer_size = (outer_maxX - outer_minX) * (outer_maxY - outer_minY);


            foreach (var inner_square in squareContours)
            {
                var inner_maxX = inner_square.toList().Max(point => point.x);
                var inner_maxY = inner_square.toList().Max(point => point.y);

                var inner_minX = inner_square.toList().Min(point => point.x);
                var inner_minY = inner_square.toList().Min(point => point.y);

                var inner_size = (inner_maxX - inner_minX) * (inner_maxY - inner_minY);

                if (outer_minX < inner_minX &&
                    outer_minY < inner_minY &&
                    outer_maxX > inner_maxX &&
                    outer_maxY > inner_maxY &&
                    checkSize(outer_size, inner_size))
                {
                
                    return outer_square;
                }
            }
        }
        return null;
    }


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

}