using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using OpenCVForUnity;
using System;
using OpenCVForUnity.RectangleTrack;
using HoloLensWithOpenCVForUnityExample;
#if WINDOWS_UWP
using System.Threading.Tasks;
#else
using System.Threading;
#endif

public class Test : MonoBehaviour {

    HoloLensWithOpenCVForUnityExample.HololensCameraStreamToMatHelper camToStreamHelper;

    DetectThread dt, dt2;
    /// <summary>
    /// The min detection size ratio.
    /// </summary>
    public float m_minDetectionSizeRatio = 0.07f;

    public string m_cascadeFileName = "cascade.xml";

    public string m_cascadeFileName2 = "cascade.xml";

    public GameObject m_TestCube, m_TestCube2;

    //Mat grayMat;

    // Use this for initialization
    void Start ()
    {
        camToStreamHelper = gameObject.GetComponent<HoloLensWithOpenCVForUnityExample.HololensCameraStreamToMatHelper>();
        camToStreamHelper.frameMatAcquired += OnFrameMatAcquired;
        camToStreamHelper.Initialize();


    }

    // Update is called once per frame
    void Update()
    {
        if (camToStreamHelper.IsPlaying() && camToStreamHelper.DidUpdateThisFrame())
        {

            Mat rgbaMat = camToStreamHelper.GetMat();

            dt2.setupDetect(rgbaMat, Imgproc.COLOR_RGBA2GRAY);

            //Imgproc.cvtColor(rgbaMat, grayMat, );
            //Imgproc.equalizeHist(grayMat, grayMat);
        }
    }
    // Callback
    public void OnFrameMatAcquired(Mat bgraMat, Matrix4x4 projectionMatrix, Matrix4x4 cameraToWorldMatrix)
    {
        // Checks if it's detecting already and starts a new Thread if not.
        dt. setupDetect(bgraMat, Imgproc.COLOR_BGRA2GRAY);

        //dt2.setupDetect(bgraMat);
    }

    public void OnWebCamTextureToMatHelperInitialized()
    {
        Mat webCamTextureMat = camToStreamHelper.GetMat();
        Mat grayMat = new Mat(webCamTextureMat.rows(), webCamTextureMat.cols(), CvType.CV_8UC1);

        Matrix4x4 projectionMatrix = camToStreamHelper.GetProjectionMatrix ();
        dt  = new DetectThread(grayMat, projectionMatrix, m_cascadeFileName,  m_minDetectionSizeRatio, gameObject.GetComponent<RectOverlay>(), m_TestCube);

        dt2 = new DetectThread(grayMat, projectionMatrix, m_cascadeFileName2, m_minDetectionSizeRatio, gameObject.GetComponent<RectOverlay>(), m_TestCube2);

    }


    public void OnWebCamTextureToMatHelperDisposed()
    {
        dt.Dispose();
    }

    /// <summary>
    /// Raises the web cam texture to mat helper error occurred event.
    /// </summary>
    /// <param name="errorCode">Error code.</param>
    public void OnWebCamTextureToMatHelperErrorOccurred(HoloLensWithOpenCVForUnityExample.WebCamTextureToMatHelper.ErrorCode errorCode)
    {
        Debug.Log("OnWebCamTextureToMatHelperErrorOccurred " + errorCode);
    }


    /// <summary>
    /// Raises the destroy event.
    /// </summary>
    void OnDestroy()
    {
        camToStreamHelper.frameMatAcquired -= OnFrameMatAcquired;
        camToStreamHelper.Dispose();
    }

    public class DetectThread
    {

        System.Object sync = new System.Object();
        private bool _IsDetecting = false;
        bool IsDetecting
        {
            get
            {
                lock (sync)
                    return _IsDetecting;
            }
            set
            {
                lock (sync)
                    _IsDetecting = value;
            }
        }

        private CascadeClassifier   m_cascade;
        private Mat                 m_grayMat;
        private Matrix4x4           m_projectionMatrix;
        private float               m_minDetectionSizeRatio;
        private RectOverlay         m_rectOverlay;
        private Mat                 m_bgraMat;
        private GameObject          m_TestCube;

        RectangleTracker                m_rectangleTracker;
        List<OpenCVForUnity.Rect>       m_resultObjects = new List<OpenCVForUnity.Rect>();

        public void setupDetect(Mat bgraMat, int imgproc)
        {
            if (!IsDetecting)
            {
                IsDetecting = true;
                Imgproc.cvtColor(bgraMat, m_grayMat, imgproc);
                Imgproc.equalizeHist(m_grayMat, m_grayMat);
                m_bgraMat = bgraMat;
                #if WINDOWS_UWP
                Task t;
                t = new Task(
                    async () =>
                    {
                        this.DetectObject();
                    }
                );
                #else
                Thread t;
                t = new Thread(new ThreadStart(this.DetectObject));
                #endif
                t.Start();
            }
        }


        public DetectThread(Mat grayMat, Matrix4x4 projectionMatrix, string cascadeFileName, float minDetectionSizeRatio, RectOverlay rectOverlay, GameObject testCube)
        {
            m_grayMat               =   grayMat;
            m_projectionMatrix      =   projectionMatrix;
            m_minDetectionSizeRatio =   minDetectionSizeRatio;
            m_rectOverlay           =   rectOverlay;
            m_TestCube              =   testCube;
            m_rectangleTracker      =   new RectangleTracker();
            m_cascade               =   new CascadeClassifier();

            m_cascade.load(Utils.getFilePath(cascadeFileName));
        }

        private void DetectObject()
        {
            MatOfRect objects = new MatOfRect();
            if (m_cascade != null)
                m_cascade.detectMultiScale(m_grayMat, objects, 1.1, 2, Objdetect.CASCADE_SCALE_IMAGE, // TODO: objdetect.CV_HAAR_SCALE_IMAGE
                    new Size(m_grayMat.cols() * m_minDetectionSizeRatio, m_grayMat.rows() * m_minDetectionSizeRatio), new Size());

            if (!objects.empty()) {
                OpenCVForUnity.Rect[] rects;

                m_rectangleTracker.UpdateTrackedObjects(objects.toList());
                m_rectangleTracker.GetObjects(m_resultObjects, true);
                rects = m_resultObjects.ToArray();
                DrawRects(rects, m_bgraMat.width(), m_bgraMat.height());

                Vector3 position = new Vector3(rects[0].x, rects[0].y, 1);
                m_TestCube.transform.position = Camera.main.ScreenToWorldPoint(position);

                m_bgraMat.Dispose();

            }

            IsDetecting = false;
        }

        private void DrawRects(OpenCVForUnity.Rect[] rects, float imageWidth, float imageHeight)
        {
            UnityEngine.Rect[] overlayRects = new UnityEngine.Rect[rects.Length];

            for (int i = 0; i < rects.Length; i++)
            {
                overlayRects[i] = new UnityEngine.Rect(rects[i].x / imageWidth
                    , rects[i].y / imageHeight
                    , rects[i].width / imageWidth
                    , rects[i].height / imageHeight);
            }
            m_rectOverlay.DrawRects(overlayRects);
        }

        public void Dispose()
        {
            if (m_grayMat != null)
                m_grayMat.Dispose();

            if (m_cascade != null)
                m_cascade.Dispose();

            m_rectangleTracker.Reset();
        }

    }
}