using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra.Single;

using System.Drawing;


namespace bbiwarg.DataSource
{
    class ForeFingerDetection
    {

        /*
         * Returns the forefinger's position
         * all fingers (except der forefinger) have to be inactive
         * params:
         *  handIndex: which hand's forefinger's position
         */

        public const int TOLERANCE_Z = 3;
        public const int TOLERANCE_2D = 3;

        private IInputProvider inputProvider;
        private IVideoHandle videoHandle;

        private List<Vector> points;
        private HashSet<Vector> seenPoints;
        private Vector palmPosition;
        private Vector foreFingerPosition;
        private float foreFingerDistance;

        private int pixelWidth;
        private int pixelHeight;


        public ForeFingerDetection(IInputProvider inputProvider, IVideoHandle videoHandle)
        {
            this.inputProvider = inputProvider;
            this.videoHandle = videoHandle;
        }

        public Vector getForeFingerPosition3D(uint handIndex)
        {
            pixelWidth = videoHandle.getWidth();
            pixelHeight = videoHandle.getHeight();

            Vector palmPosition2D = inputProvider.getPalmPosition2D(handIndex);
            palmPosition = new DenseVector(3);
            palmPosition[0] = palmPosition2D[0];
            palmPosition[1] = palmPosition2D[1];
            palmPosition[2] = videoHandle.getDepth((int)palmPosition[0], (int)palmPosition[1]);

            points = new List<Vector>();
            seenPoints = new HashSet<Vector>();
            foreFingerPosition = palmPosition;
            foreFingerDistance = 0;

            points.Add(palmPosition);
            seenPoints.Add(palmPosition);
            for (int i = 0; i < points.Count(); i++)
            {
                checkPoint(points[i]);
            }


            if (points.Count() <= 10)
            {
                Console.WriteLine(palmPosition[2]);
            }

            if (seenPoints.Count() <= 100)
            {

            }
            return videoHandle.pixel2VertexPosition(foreFingerPosition);
        }


        private void checkPoint(Vector point)
        {
            Vector distanceVector = new DenseVector(3);
            videoHandle.pixel2VertexPosition(palmPosition).Subtract(videoHandle.pixel2VertexPosition(point), distanceVector);

            float currentPointDistance = distanceVector.Norm(2);
            if (currentPointDistance >= foreFingerDistance)
            {
                foreFingerPosition = point;
                foreFingerDistance = currentPointDistance;
            }
            addNeighbours(point);
        }

        private void addNeighbours(Vector point)
        {
            Vector currentPoint;
            int x_start = -(int)Math.Min(TOLERANCE_2D, point[0]);
            for (int y = -(int)Math.Min(TOLERANCE_2D, point[1]); y <= TOLERANCE_2D && point[1] + y < pixelHeight; y++)
            {
                for (int x = x_start; x <= TOLERANCE_2D && point[0] + x < pixelWidth; x++)
                {
                    currentPoint = new DenseVector(3);
                    currentPoint[0] = point[0] + x;
                    currentPoint[1] = point[1] + y;
                    currentPoint[2] = videoHandle.getDepth((int)currentPoint[0], (int)currentPoint[1]);
                    if ((!seenPoints.Contains(currentPoint) && point[1] <= palmPosition[1] && Math.Abs(currentPoint[2] - point[2]) <= TOLERANCE_Z)
                        || point == palmPosition)
                    {
                        points.Add(currentPoint);
                    }
                    seenPoints.Add(currentPoint);
                }
            }
        }

        public List<Vector> getHandPoints()
        {
            return points;
        }
    }
}