using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using MathNet.Numerics.LinearAlgebra.Single;

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

namespace bbiwarg.DataSource
{
    /**
     * Represents an depth image where the depth is given in distance to the camera in millimeters. 
     */
    class DepthImage
    {
        private int width, height;
        private Image<Gray, Int16> image;

        public DepthImage(int width, int height, short[] data)
        {
            this.width = width;
            this.height = height;
            
            image = new Image<Gray, Int16>(width, height);
            for (int i = 0; i < data.Length; ++i)
                setDepth(i % width, i / width, data[i]);
        }

        public int getWidth()
        {
            return width;
        }

        public int getHeight()
        {
            return height;
        }

        public short getDepth(int x, int y)
        {
            return image.Data[y, x, 0];
        }

        private void setDepth(int x, int y, short depth)
        {
            image.Data[y, x, 0] = depth;
        }

        public void filterMedian(int filterSize)
        {
            image = image.SmoothMedian(3);
        }

        public void thresholdDepth(int min, int max)
        {
            image = image.ThresholdToZero(new Gray(min));
            image = image.ThresholdBinaryInv(new Gray(max), new Gray(2000));
         }

        public void thresholdPosition(int minX, int maxX, int minY, int maxY)
        {
            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    if (x < minX || x > maxX || y < minY || y > maxY)
                        setDepth(x, y, 0);
                }
            }
        }

        public List<Vector> getRectPoints()
        {
            Image<Gray, Byte> tmp = image.Convert<Byte>(delegate(short s) { return (s == 2000) ? (byte) 1 : (byte) 0; });

            Contour<Point> contours = tmp.FindContours();
            MCvBox2D box = contours.GetMinAreaRect();
            Seq<Point> points = contours.GetConvexHull(Emgu.CV.CvEnum.ORIENTATION.CV_CLOCKWISE);

            List<Vector> result = new List<Vector>();
            foreach (Point p in points)
            {
                result.Add(new DenseVector(new float[]{p.X, p.Y}));
            }
            return result;
        }
    }
}