///-----------------------------------------------------------------
/// Namespace: <Cozmo>
/// Class: <ImageProcessor>
/// Description: <Converts a rendertexture to a opencv mat in order to use the canny algorithm.
///               After processing the mat will be converted to a render texture back again.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
/// 

using UnityEngine;
using OpenCvSharp;
using System.Threading.Tasks;

namespace Cozmo
{
    public class ImageProcessor : MonoBehaviour
    {
        [Header("RenderTexture")]
        [Tooltip("RenderTexture that will be passed to the LearningBrain.")]
        public RenderTexture renderTextureCropped;

        [Header("Debug Helper")]
        [Tooltip("Reference to the MeshRenderer that will show the processed Image from Cozmo")]
        public MeshRenderer processedImageRenderer;
        [Tooltip("Reference to the MeshRenderer that will show the processed and cropped Image from Cozmo")]
        public MeshRenderer processedImageRendererCropped;

        /// <summary>
        /// Center of Gravity in the cropped canny image
        /// </summary>
        public Point CenterOfGravity { get; private set; }

        // OpenCVSharp parameters
        private Mat cozmoImageMat;
        private Mat cannyImage;
        private Texture2D finalProcessedCozmoTexture;
        private Vec3b[] cozmoImageData;
        private byte[] cannyImageData;


        private int imWidth = 320;          // Width of the camera image from the virtual cozmo
        private int imHeight = 240;         // Height of the camera image from the virtual cozmo
        private int croppedImHeight = 120;  // Height of the cropped camera image from the virtual cozmo 
        private Camera textureCamera;       // Virtual Cozmo camera

        private Texture2D originalCozmoTexture;


        private void Start()
        {
            // Get reference to the cozmo camera
            textureCamera = GetComponent<Camera>();

            // Set image widths and heights based on the given RenderTextures
            imWidth = textureCamera.targetTexture.width;
            imHeight = textureCamera.targetTexture.height;

            // assign the processed targetTexture to the renderer to display the image
            processedImageRenderer.material.mainTexture = textureCamera.targetTexture;
            processedImageRendererCropped.material.mainTexture = renderTextureCropped;

            // initialize video / image with given size
            cozmoImageMat = new Mat(imHeight, imWidth, MatType.CV_8UC3);
            cozmoImageData = new Vec3b[imHeight * imWidth];
            cannyImage = new Mat(imHeight, imWidth, MatType.CV_8UC1);
            cannyImageData = new byte[croppedImHeight * imWidth];

            originalCozmoTexture = new Texture2D(imWidth, imHeight, TextureFormat.RGBA32, true, true);
            finalProcessedCozmoTexture = new Texture2D(imWidth, croppedImHeight, TextureFormat.RGBA32, true, true);
        }

        ///// <summary>
        ///// Gets called when a new image arrives from the camera this script lies on
        ///// </summary>
        ///// <param name="source"></param>
        ///// <param name="destination"></param>
        public void OnRenderImage(RenderTexture source, RenderTexture destination)
        {
            RenderTextureToTexture2D(source);
            TextureToMat(originalCozmoTexture);
            ProcessImage(cozmoImageMat);
            cannyImage = CropImage(cannyImage);
            FindCenterOfGravity(cannyImage);
            MatToTexture(cannyImage);

            Graphics.Blit(finalProcessedCozmoTexture, destination);
            Graphics.Blit(finalProcessedCozmoTexture, renderTextureCropped);
        }

        // Crop image to just see the middle of the original image 
        private Mat CropImage(Mat image)
        {
            //cut a fourth out of the top and bottom of the image 
            OpenCvSharp.Rect rectCroped = new OpenCvSharp.Rect(0, image.Height / 4, image.Width, image.Height / 2);
            Mat croppedImage = new Mat(image, rectCroped);
            return croppedImage;
        }

        // Convert the rendertexture to a texture2d
        private void RenderTextureToTexture2D(RenderTexture rTex)
        {
            RenderTexture.active = rTex;
            originalCozmoTexture.ReadPixels(new UnityEngine.Rect(0, 0, rTex.width, rTex.height), 0, 0);
            originalCozmoTexture.Apply();
        }

        // Convert Unity Texture2D object to OpenCVSharp Mat object
        private void TextureToMat(Texture2D source)
        {
            // Color32 array : r, g, b, a
            Color32[] c = source.GetPixels32();

            // Parallel for loop
            // convert Color32 object to Vec3b object
            // Vec3b is the representation of pixel for Mat
            Parallel.For(0, imHeight, i =>
            {
                for (var j = 0; j < imWidth; j++)
                {
                    var col = c[j + i * imWidth];
                    var vec3 = new Vec3b
                    {
                        Item0 = col.b,
                        Item1 = col.g,
                        Item2 = col.r
                    };
                    // set pixel to an array
                    cozmoImageData[j + i * imWidth] = vec3;
                }
            });

            // assign the Vec3b array to Mat
            cozmoImageMat.SetArray(0, 0, cozmoImageData);
        }


        // Simple example of canny edge detect
        private void ProcessImage(Mat _image)
        {
            Cv2.Canny(_image, cannyImage, 100, 100);
        }


        // Convert OpenCVSharp Mat object to Unity Texture2D object
        private void MatToTexture(Mat mat)
        {
            // cannyImageData is byte array, because canny image is binary
            mat.GetArray(0, 0, cannyImageData);
            // create Color32 array that can be assigned to Texture2D directly
            Color32[] c = new Color32[croppedImHeight * imWidth];

            // parallel for loop
            Parallel.For(0, croppedImHeight, i =>
            {
                for (var j = 0; j < imWidth; j++)
                {
                    byte vec = cannyImageData[j + i * imWidth];
                    var color32 = new Color32
                    {
                        r = vec,
                        g = vec,
                        b = vec,
                        a = 0
                    };
                    c[j + i * imWidth] = color32;
                }
            });


            finalProcessedCozmoTexture.SetPixels32(c);
            // update texture
            finalProcessedCozmoTexture.Apply();
        }

        // Find the Center of Gravity in the image
        private void FindCenterOfGravity(Mat processedImage)
        {
            // find moments of the image
            Moments m = new Moments(processedImage, true);
            CenterOfGravity = new Point(m.M10 / m.M00, m.M01 / m.M00);

#if UNITY_EDITOR
            // show the image with a point mark at the centroid
            Cv2.Circle(processedImage, CenterOfGravity, 5, new Scalar(128, 0, 0), -1);
            Cv2.Flip(processedImage, processedImage, FlipMode.X);
            Cv2.ImShow("Image with center", processedImage);
#endif
        }

    }
}