using BBIWARG.Utility;
using Emgu.CV;
using Emgu.CV.Structure;
using System;
using System.Drawing;

namespace BBIWARG.Images
{
    /// <summary>
    /// DepthImage stores a processed version of the depth image read by the camera as an <see cref="Image"/>
    /// </summary>
    public class DepthImage
    {
        /// <summary>
        /// the processed depth image
        /// </summary>
        public Image<Gray, byte> Image { get; private set; }

        /// <summary>
        /// the maximum depth which is considered important
        /// </summary>
        public UInt16 MaxDepth { get; private set; }

        /// <summary>
        /// the minimum depth in the raw depth image
        /// </summary>
        public UInt16 MinDepth { get; private set; }

        /// <summary>
        /// image size of the depth image
        /// </summary>
        public ImageSize Size { get; private set; }

        /// <summary>
        /// Construct a DepthImage from the raw depth data, the image size and a confidenceImage.
        /// Filters the image using confidenceImage and also applies thresholding and smoothing.
        /// </summary>
        /// <param name="rawDepthData">the raw depth data</param>
        /// <param name="size">the image size</param>
        /// <param name="confidenceImage">the confidence image</param>
        public DepthImage(Image<Gray, UInt16> rawDepthImage)
        {
            Size = new ImageSize(rawDepthImage.Width, rawDepthImage.Height);
            
            // smooth with median filter
            rawDepthImage = rawDepthImage.SmoothMedian(Parameters.DepthImageMedianSize);

            // threshold min&maxDepth
            MinDepth = findMinDepth(rawDepthImage);
            MaxDepth = (UInt16)(MinDepth + Parameters.DepthImageDepthRange);
            
            // threshold (dst = (src > (MaxDepth - MinDepth)) ? MaxDepth - MinDepth : src)
            Image = (rawDepthImage - MinDepth).Convert<Gray, Int16>().ThresholdTrunc(new Gray(MaxDepth - MinDepth)).Convert<Gray, byte>();
            
            // smooth with median filter
            Image = Image.SmoothMedian(Parameters.DepthImageMedianSize);
        }

        /// <summary>
        /// Returns the depth in the processed image at a given point.
        /// </summary>
        /// <param name="point">the point</param>
        /// <returns>depth at the point</returns>
        public UInt16 getDepthAt(Point point)
        {
            return getDepthAt(point.X, point.Y);
        }

        /// <summary>
        /// Returns the depth in the processed image at a given position.
        /// </summary>
        /// <param name="x">x coordinate of the position</param>
        /// <param name="y">y coordinate of the position</param>
        /// <returns>the depth at the position</returns>
        public UInt16 getDepthAt(int x, int y)
        {
            try
            {
                return (UInt16)(MinDepth + Image.Data[y, x, 0]);
            }
            catch (IndexOutOfRangeException e)
            {
                return (UInt16)(MinDepth + Image.Data[0, 0, 0]);
            }
        }


        /// <summary>
        /// Returns the depth in the processed image at a given position.
        /// It will move the x,y more to the center of the palm!
        /// </summary>
        /// <param name="x">x coordinate of the position</param>
        /// <param name="y">y coordinate of the position</param>
        /// <returns>the depth at the position</returns>
        public UInt16 getDepthAtFixed(int x0, int y0, int x1, int y1)
        {
            try
            {
                int rx = (int) ((x1 - x0) * 0.15f + x0);
                int ry = (int) ((y1 - y0) * 0.15f + y0);

                return (UInt16)(MinDepth + Image.Data[ry, rx, 0]);
            }
            catch (IndexOutOfRangeException e)
            {
                return (UInt16)(MinDepth + Image.Data[0, 0, 0]);
            }
        }

        /// <summary>
        /// Sets the depth in the processed image.
        /// </summary>
        /// <param name="point">point where the depth is set</param>
        /// <param name="depth">new depth value</param>
        public void setDepthAt(Point point, UInt16 depth)
        {
            setDepthAt(point.X, point.Y, depth);
        }

        /// <summary>
        /// Sets the depth in the processed image.
        /// </summary>
        /// <param name="x">x coordinate of position to set depth</param>
        /// <param name="y">y coordinate of position to set depth</param>
        /// <param name="depth">new depth value</param>
        public void setDepthAt(int x, int y, UInt16 depth)
        {
            Image.Data[y, x, 0] = (byte)(depth - MinDepth);
        }

        /// <summary>
        /// Returns the minimum depth in an <see cref="Image"/>
        /// </summary>
        /// <param name="image">the image</param>
        /// <returns>the minimum depth</returns>
        private UInt16 findMinDepth(Image<Gray, UInt16> image)
        {
            // min and max values
            double[] min, max;

            // min and max locations
            Point[] minLoc, maxLoc;

            image.MinMax(out min, out max, out minLoc, out maxLoc);

            return (UInt16)min[0];
        }
    }
}