using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Emgu.CV;
using Emgu.CV.Structure;

namespace bbiwarg
{
    class DepthImage
    {
        private int width;
        private int height;
        private Image<Gray, Int16> image;
        private Image<Gray, Int16> edges;
        private bool[,] fingerPoints;

        private Int16 minDepth;
        private Int16 maxDepth;

        public DepthImage(int width, int height, Image<Gray, Int16> image) {
            this.width = width;
            this.height = height;
            this.image = image;

            this.image = this.image.SmoothMedian(3);
            //minDepth
            findMinDepth();

            //maxDepth
            maxDepth = (Int16)(minDepth + 200);
            thresholdDepth(minDepth, maxDepth);

            //edges
            calculateEdges();

            //findFingerPoints
            findFingerPoints();
        }

        public int getWidth() {
            return width;
        }

        public int getHeight() {
            return height;
        }

        public Int16 getMinDepth() {
            return minDepth;
        }

        public Int16 getMaxDepth() {
            return maxDepth;
        }

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

        public Int16 getEdgeAt(int x, int y)
        {
            return edges.Data[y, x, 0];
        }

        public bool isFingerPoint(int x, int y) {
            return fingerPoints[x, y];
        }

        private void setDepthAt(int x, int y, Int16 value) {
            image.Data[y, x, 0] = value;
        }

        private void findMinDepth() {
            minDepth = Int16.MaxValue;
            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    Int16 depth = getDepthAt(x,y);
                    if (depth < minDepth)
                        minDepth = depth;
                }
            }
        }

        private void thresholdDepth(int min, int max)
        {
            image = image.ThresholdToZero(new Gray(min));
            image = image.ThresholdTrunc(new Gray(max));

            for (int x = 0; x < width; ++x)
            {
                for (int y = 0; y < height; ++y)
                {
                    if (getDepthAt(x, y) == 0)
                        setDepthAt(x, y, (short)max);
                }
            }
        }

        private void calculateEdges() {
            Image<Gray, Byte> tmp = image.Convert<Byte>(delegate(Int16 depth) { return (byte)(((depth - minDepth) * Byte.MaxValue) / (maxDepth - minDepth)); });
            Image<Gray, byte> tmp2 = tmp.Canny(150, 75, 3);
            edges = tmp2.Convert<Int16>(delegate(byte depth) { if(depth > 0) {return Int16.MaxValue;} else {return 0;}});//((depth * Int16.MaxValue) / Byte.MaxValue); });
        }

        private void findFingerPoints()
        {
            int maxFingerSize = 30;
            int minFingerSize = 10;
            fingerPoints = new bool[width, height];

            for (int y = 0; y < height; y++) {
                for (int x = 0; x < width; x++) {
                    if (getEdgeAt(x, y) > 0) {
                        //search horizontal
                        bool edgeRightFound = false;
                        int edgeRightX = x + minFingerSize;
                        while (!edgeRightFound && edgeRightX < width) {
                            if (getEdgeAt(edgeRightX, y) > 0)
                                edgeRightFound = true;
                            else
                                edgeRightX++;
                        }

                        if (edgeRightFound){
                            int midX = (edgeRightX + x) / 2;
                            Int16 depthLeft = getDepthAt(x, y);
                            Int16 depthMid = getDepthAt(midX, y);
                            Int16 depthRight = getDepthAt(edgeRightX, y);

                            if ((edgeRightX - x) < maxFingerSize && depthLeft > depthMid && depthMid < depthRight) {
                                fingerPoints[midX, y] = true;
                            } 
                        }

                        //search vertical
                        bool edgeBottomFound = false;
                        int edgeBottomY = y + minFingerSize;
                        while (!edgeBottomFound && edgeBottomY < height) {
                            if (getEdgeAt(x, edgeBottomY) > 0)
                                edgeBottomFound = true;
                            else
                                edgeBottomY++;
                        }

                        if (edgeBottomFound) {
                            int midY = (edgeBottomY + y) / 2;
                            Int16 depthTop = getDepthAt(x, y);
                            Int16 depthMid = getDepthAt(x, midY);
                            Int16 depthBottom = getDepthAt(x, edgeBottomY);

                            if ((edgeBottomY - y) < maxFingerSize && depthTop > depthMid && depthMid < depthBottom) {
                                fingerPoints[x, midY] = true;
                            }

                        }

                        x = edgeRightX - 1;
                    }
                }
            }
        }
    }
}