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.Input.InputHandling;
using bbiwarg.Utility;
using Emgu.CV.Structure;
using Emgu.CV;

namespace bbiwarg.Recognition.FingerRecognition
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImageOriginal;
        private EdgeImage edgeImageAdapted;
        private List<Finger> fingers;

        public void detectFingers(FrameData frameData)
        {
            depthImage = frameData.DepthImage;
            edgeImageOriginal = frameData.EdgeImage;
            edgeImageAdapted = frameData.EdgeImage.copy();
            fingers = new List<Finger>();

            Vector2D maxPixel = depthImage.Size.MaxPixel;
            int maxX = maxPixel.IntX;
            int maxY = maxPixel.IntY;

            for (int y = 1; y < maxY; y += 4)
            {
                for (int x = 1; x < maxX; x += 2)
                {
                    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 > Parameters.FingerMinNumSlices)
                                {
                                    createFingerFromTrail(trail);
                                }
                            }
                        }
                    }
                }
            }

            frameData.DetectedFingers = fingers;
        }

        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 + Parameters.FingerStepSize * direction;

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

                    if (trail.NumSlices > Parameters.FingerMinNumSlices / 2)
                    {
                        trail.removeFirstSlices(Parameters.FingerRemoveNumSlicesForCorrection);
                        trail = expandTrail(trail, true);
                        return trail;
                    }
                }
            }

            return null;

        }

        private FingerSliceTrail expandTrail(FingerSliceTrail trail, bool reversed = false)
        {
            if (reversed)
                trail.reverse();

            Vector2D currentDirection = trail.getEndDirection();
            Vector2D currentPosition = trail.EndSlice.Mid + Parameters.FingerStepSize * currentDirection;

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

            FingerSlice lastSlice = trail.EndSlice;
            FingerSlice nextSlice;

            while (currentPosition.isInBound(depthImage.Size) && gapCounter < Math.Min(numSlices, Parameters.FingerMaxGapCounter))
            {
                nextSlice = findFingerSliceFromMid(currentPosition, currentDirection, reversed);

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

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

            if (reversed)
                trail.reverse();

            return trail;
        }

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

            Vector2D dirStart = direction.getOrthogonal(reversed);
            Vector2D dirEnd = direction.getOrthogonal(!reversed);

            Vector2D start = edgeImageAdapted.findNextRoughEdge(position, dirStart, Parameters.FingerMaxWidth);
            if (start == null) return null;

            Vector2D end = edgeImageAdapted.findNextRoughEdge(position, dirEnd, Parameters.FingerMaxWidth);
            if (end == null) return null;

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

            return getFingerSlice(start, end);
        }

        private FingerSlice getFingerSlice(Vector2D start, Vector2D end)
        {
            FingerSlice slice = new FingerSlice(start, end);
            if (slice.Length >= Parameters.FingerMinWidth && slice.Length <= Parameters.FingerMaxWidth && fingerSliceDepthTest(slice))
                return slice;

            return null;
        }

        private bool fingerSliceDepthTest(FingerSlice slice)
        {
            Int16 depthStart = depthImage.getDepthAt(slice.Start.moveWithinBound(depthImage.Size, slice.Direction.getInverse(), Parameters.FingerContourMargin));
            Int16 depthMid = depthImage.getDepthAt(slice.Mid);
            Int16 depthEnd = depthImage.getDepthAt(slice.End.moveWithinBound(depthImage.Size, slice.Direction, Parameters.FingerContourMargin));
            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.getContour(Parameters.FingerContourMargin).ToArray());
        }

        private bool isCrippleFinger(Finger finger)
        {
            FingerSlice midSlice = finger.SliceTrail.MidSlice;
            Vector2D out1 = midSlice.Start.moveWithinBound(depthImage.Size, midSlice.Direction.getInverse(), Parameters.FingerOutMargin);
            Vector2D out2 = midSlice.End.moveWithinBound(depthImage.Size, midSlice.Direction, Parameters.FingerOutMargin);

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

            return (minDepthDifference < Parameters.FingerMaxCrippleDifference);
        }


        private FingerSliceTrail orderTrailTipToHand(FingerSliceTrail trail)
        {
            float startLength = trail.StartSlice.Length;
            float endLength = trail.EndSlice.Length;

            if (startLength > endLength)
                trail.reverse();

            return trail;
        }
    }
}