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 = 5;

        private IVideoHandle source;
        private HashSet<Vector> seenPoints;
        private Vector palmPosition;
        private Vector foreFingerPosition;
        private float foreFingerDistance;



        public ForeFingerDetection(IVideoHandle source)
        {
            this.source = source;
        }

        public Vector getForeFingerPosition3D(uint handIndex)
        {
            seenPoints = new HashSet<Vector>();
            Vector palmPosition2D = source.getPalmPosition2D(handIndex);
            palmPosition = new DenseVector(3);
            palmPosition[0] = palmPosition2D[0];
            palmPosition[1] = palmPosition2D[1];
            palmPosition[2] = source.getDepth((int)palmPosition[0], (int)palmPosition[1]);

            foreFingerPosition = palmPosition;
            checkPoint(palmPosition);
            return foreFingerPosition;
        }

        private void checkNeighbours(Vector point)
        {
            for (int y = -(int)Math.Min(TOLERANCE_2D, point[1]); y <= TOLERANCE_2D && point[1] + y < source.getHeight(); y++)
            {
                for (int x = -(int)Math.Min(TOLERANCE_2D, point[0]); x <= TOLERANCE_2D && point[0] + x < source.getWidth(); x++)
                {
                    Vector currentPoint = new DenseVector(3);
                    currentPoint[0] = point[0] + x;
                    currentPoint[1] = point[1] + y;
                    currentPoint[2] = source.getDepth((int) currentPoint[0], (int) currentPoint[1]);
                    if(Math.Abs(currentPoint[2]-point[2]) <= TOLERANCE_Z)
                      checkPoint(currentPoint);
                }
            }
        }
        private void checkPoint(Vector point)
        {
            if (!seenPoints.Contains<Vector>(point))
            {
                seenPoints.Add(point);
                Vector distanceVector = new DenseVector(3);
                palmPosition.Subtract(point, distanceVector);
                float currentPointDistance = distanceVector.Norm(2);
                if (currentPointDistance >= foreFingerDistance)
                {
                    foreFingerPosition = point;
                    foreFingerDistance = currentPointDistance;
                    checkNeighbours(point);
                }
            }
        }
    }
}