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 = 5;
        public const int TOLERANCE_2D = 7;

        private IInputProvider inputProvider;
        private IVideoHandle videoHandle;

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


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

        public Vector getForeFingerPosition3D(uint handIndex)
        {
            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;
            lowestY = palmPosition[1];
            highestZ = palmPosition[2];

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

            return videoHandle.pixel2VertexPosition(foreFingerPosition);
        }


        private void checkPoint(Vector point)
        {
            bool isLowestY = point[1] <= lowestY;
            bool isHighestZ = point[2] >= highestZ;
            if (isLowestY || isHighestZ)
            {
                if (isLowestY)
                {
                    if (lowestY == foreFingerPosition[1])
                        foreFingerDistance = 0;
                    lowestY = point[1];
                }
                if (isHighestZ)
                {
                    if (highestZ == foreFingerPosition[2])
                        foreFingerDistance = 0;
                    highestZ = point[2];
                }

                Vector distanceVector = new DenseVector(3);
                videoHandle.pixel2VertexPosition(palmPosition).Subtract(videoHandle.pixel2VertexPosition(point), distanceVector);
                //palmPosition.Subtract(point, distanceVector);

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

        private void addNeighbours(Vector point)
        {
            Vector currentPoint;
            for (int y = -(int)Math.Min(TOLERANCE_2D, point[1]); y <= TOLERANCE_2D && point[1] + y < videoHandle.getHeight(); y++)
            {
                for (int x = -(int)Math.Min(TOLERANCE_2D, point[0]); x <= TOLERANCE_2D && point[0] + x < videoHandle.getWidth(); 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 (Math.Abs(currentPoint[2] - point[2]) <= TOLERANCE_Z && !seenPoints.Contains(currentPoint))
                    {
                        points.Add(currentPoint);
                        seenPoints.Add(currentPoint);
                    }
                }
            }
        }
    }
}