using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using bbiwarg.Images;
using bbiwarg.Utility;
using Emgu.CV.Structure;
using Emgu.CV;
using bbiwarg.Graphics;

namespace bbiwarg.Recognition.FingerRecognition
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImageOriginal;
        private EdgeImage edgeImageAdapted;
        private OutputImage outputImage;
        public List<Finger> Fingers { get; private set; }

        public FingerDetector(DepthImage depthImage, EdgeImage edgeImage, OutputImage outputImage)
        {
            this.depthImage = depthImage;
            this.edgeImageOriginal = edgeImage;
            this.edgeImageAdapted = edgeImage.copy();
            this.outputImage = outputImage;

            detectFingers();
            drawFingers();
        }

        private void detectFingers()
        {
            int maxX = depthImage.BottomRight.IntX;
            int maxY = depthImage.BottomRight.IntY;

            Fingers = new List<Finger>();

            for (int y = 1; y < maxY; y += 5)
            {
                for (int x = 1; x < maxX; x++)
                {
                    if (edgeImageAdapted.isEdgeAt(x, y))
                    {
                        Vector2D edgePoint = new Vector2D(x, y);
                        Vector2D edgeDirection = getEdgeDirection(edgePoint);
                        if (edgeDirection != null)
                        {
                            Vector2D dir = edgeDirection.getOrthogonal();
                            if (depthImage.getDepthAt(edgePoint - dir) < depthImage.getDepthAt(edgePoint + dir))
                                dir = dir.getInverse();

                            FingerSlice slice = findFingerSliceFromStartEdge(edgePoint, dir);
                            if (slice != null)
                            {
                                FingerSliceTrail trail = findFingerSliceTrail(slice, edgeDirection);
                                if (trail != null && trail.NumSlices > Constants.FingerMinNumSlices)
                                {
                                    createFingerFromTrail(trail);
                                }
                            }
                        }
                    }
                }
            }
        }

        private Vector2D getEdgeDirection(Vector2D edgePoint)
        {
            int x = edgePoint.IntX;
            int y = edgePoint.IntY;

            if (edgeImageAdapted.isEdgeAt(x, y - 1) && edgeImageAdapted.isEdgeAt(x, y + 1)) return new Vector2D(0, 1);
            else if (edgeImageAdapted.isEdgeAt(x - 1, y) && edgeImageAdapted.isEdgeAt(x + 1, y)) return new Vector2D(1, 0);
            else if (edgeImageAdapted.isEdgeAt(x - 1, y - 1) && edgeImageAdapted.isEdgeAt(x + 1, y + 1)) return new Vector2D(1, 1).normalize();
            else if (edgeImageAdapted.isEdgeAt(x + 1, y - 1) && edgeImageAdapted.isEdgeAt(x - 1, y + 1)) return new Vector2D(1, -1).normalize();
            else return null;
        }

        private FingerSliceTrail findFingerSliceTrail(FingerSlice startSlice, Vector2D startDirection)
        {
            FingerSliceTrail trail = new FingerSliceTrail(startSlice);

            Vector2D direction = startDirection;
            Vector2D position = startSlice.Mid + Constants.FingerStepSize * direction;

            if (position.isInBound(Vector2D.Zero, depthImage.BottomRight))
            {
                FingerSlice nextSlice = findFingerSliceFromMid(position, direction);
                if (nextSlice != null)
                {
                    trail.addSlice(nextSlice);
                    trail = expandTrail(trail);

                    if (trail.NumSlices > Constants.FingerMinNumSlices / 2)
                    {
                        trail.removeFirstSlices(Constants.FingerRemoveNumSlicesForCorrection);
                        trail.reverse();
                        trail = expandTrail(trail);
                        trail.reverse();
                        return trail;
                    }
                }
            }

            return null;

        }

        private FingerSliceTrail expandTrail(FingerSliceTrail trail)
        {
            Vector2D currentDirection = trail.getEndDirection();
            Vector2D currentPosition = trail.EndSlice.Mid + Constants.FingerStepSize * currentDirection;

            int gapCounter = 0;
            int numSlices = trail.NumSlices;

            FingerSlice lastSlice = trail.EndSlice;
            FingerSlice nextSlice;

            while (currentPosition.isInBound(Vector2D.Zero, depthImage.BottomRight) && gapCounter <= Math.Min(numSlices, Constants.FingerMaxGapCounter))
            {
                nextSlice = findFingerSliceFromMid(currentPosition, currentDirection);

                if (nextSlice != null && Math.Abs(nextSlice.Length - lastSlice.Length) <= Constants.FingerMaxSliceDifferencePerStep)
                {
                    gapCounter = 0;
                    numSlices++;
                    trail.addSlice(nextSlice);
                    currentDirection = trail.getEndDirection();
                    currentPosition = nextSlice.Mid + Constants.FingerStepSize * currentDirection;

                    lastSlice = nextSlice;
                }
                else
                {
                    gapCounter++;
                    currentPosition += currentDirection;
                }
            }

            return trail;
        }

        private FingerSlice findFingerSliceFromMid(Vector2D position, Vector2D direction)
        {
            if (edgeImageAdapted.isRoughEdgeAt(position)) return null;

            Vector2D dirStart = direction.getOrthogonal(true);
            Vector2D dirEnd = direction.getOrthogonal(false);

            Vector2D start = edgeImageAdapted.findNextEdge(position, dirStart, Constants.FingerMaxWidth);
            if (start == null) return null;

            Vector2D end = edgeImageAdapted.findNextEdge(position, dirEnd, Constants.FingerMaxWidth);
            if (end == null) return null;

            return getFingerSlice(start, end);
        }
        private FingerSlice findFingerSliceFromStartEdge(Vector2D start, Vector2D direction)
        {
            Vector2D searchStart = start + Constants.FingerSliceOverlapFactor * direction;
            Vector2D end = edgeImageAdapted.findNextEdge(searchStart, direction, Constants.FingerMaxWidth);
            if (end == null) return null;

            return getFingerSlice(start, end);
        }

        private FingerSlice getFingerSlice(Vector2D start, Vector2D end)
        {
            Vector2D direction = (end - start).normalize();
            Vector2D directionInv = direction.getInverse();
            Vector2D beforeStart = (start + Constants.FingerSliceOverlapFactor * directionInv).moveInBound(Vector2D.Zero, depthImage.BottomRight, direction);
            Vector2D behindEnd = (end + Constants.FingerSliceOverlapFactor * direction).moveInBound(Vector2D.Zero, depthImage.BottomRight, directionInv);

            FingerSlice slice = new FingerSlice(beforeStart, behindEnd);
            if (slice.Length >= Constants.FingerMinWidth && slice.Length <= Constants.FingerMaxWidth && fingerSliceDepthTest(slice))
                return slice;

            return null;
        }

        private bool fingerSliceDepthTest(FingerSlice fingerSlice)
        {
            Int16 depthStart = depthImage.getDepthAt(fingerSlice.Start);
            Int16 depthMid = depthImage.getDepthAt(fingerSlice.Mid);
            Int16 depthEnd = depthImage.getDepthAt(fingerSlice.End);
            return (depthStart > depthMid && depthMid < depthEnd);
        }

        private void createFingerFromTrail(FingerSliceTrail trail)
        {
            //bring finger in correct direction Tip->Hand
            trail = orderTrailTipToHand(trail);

            //create finger
            Finger finger = new Finger(trail);

            //add finger
            if (!isCrippleFinger(finger))
                Fingers.Add(finger);

            //remove edges around detected finger to improve performance
            edgeImageAdapted.removeEdgesInsidePolygon(finger.Contour.ToArray());
        }

        private bool isCrippleFinger(Finger finger)
        {
            FingerSlice midSlice = finger.SliceTrail.MidSlice;
            Vector2D direction = midSlice.Direction;
            Vector2D directionInv = direction.getInverse();
            Vector2D out1 = (midSlice.Start + Constants.FingerCrippleOutFactor * directionInv).moveInBound(Vector2D.Zero, depthImage.BottomRight, direction);
            Vector2D out2 = (midSlice.End + Constants.FingerCrippleOutFactor * direction).moveInBound(Vector2D.Zero, depthImage.BottomRight, directionInv);

            Int16 depthAtFinger = depthImage.getDepthAt(midSlice.Mid);
            Int16 depthAtOut1 = depthImage.getDepthAt(out1);
            Int16 depthAtOut2 = depthImage.getDepthAt(out2);
            int minDepthDifference = Math.Min(Math.Abs(depthAtFinger - depthAtOut1), Math.Abs(depthAtFinger - depthAtOut2));

            return (minDepthDifference < Constants.FingerCrippleOutMinDifference);
        }

        
        private FingerSliceTrail orderTrailTipToHand(FingerSliceTrail trail)
        {
            int numSlicesForDirectionDetection = Constants.FingerNumSlicesForDirectionDetection;
            int maxIndex = trail.NumSlices-1;
            float sumStart = 0;
            float sumEnd = 0;

            for (int i = 0; i < numSlicesForDirectionDetection; i++) {
                sumStart += trail[i].Length;
                sumEnd += trail[maxIndex - i].Length;
            }


            float avergaeStart = sumStart / numSlicesForDirectionDetection;
            float averageEnd = sumEnd / numSlicesForDirectionDetection;

            //check direction
            if (avergaeStart > averageEnd)
                trail.reverse();

            return trail;
        }
        
        private void drawFingers()
        {
            foreach (Finger finger in Fingers)
            {
                finger.draw(outputImage, false);
            }
        }
    }
}