using BBIWARG.Images;
using BBIWARG.Input.InputHandling;
using BBIWARG.Utility;
using System;
using System.Collections.Generic;

namespace BBIWARG.Recognition.FingerRecognition
{
    /// <summary>
    /// Detects fingers in the given depth and edge images. The finger detection searches for edges in the edge image and tries to find an initial finger slice. For each found finger slice, the finger detector tries to move along the finger direction to extend the finger slice trail. If the trail reaches its end, the finger detector removes the first few slices and starts the trail expansion in opposite direction. If the complete slice trail is long enough, the finger slices are sorted into correct order and the edges around the finger are removed to increase performance for the next finger detection.
    /// </summary>
    internal class FingerDetector
    {
        /// <summary>
        /// the coordinateConverter (used to calculate finger width (in millimeters))
        /// </summary>
        private CoordinateConverter coordinateConverter;

        /// <summary>
        /// the current depthImage
        /// </summary>
        private DepthImage depthImage;

        /// <summary>
        /// the current edge image in its adapted form (edges around detected fingers will be removed)
        /// </summary>
        private EdgeImage edgeImageAdapted;

        /// <summary>
        /// the current edge image in its original form
        /// </summary>
        private EdgeImage edgeImageOriginal;

        /// <summary>
        /// the detected fingers
        /// </summary>
        private List<Finger> fingers;

        /// <summary>
        /// Initializes a new instance of the FingerDetector class.
        /// </summary>
        /// <param name="coordinateConverter">The coordinate converter.</param>
        public FingerDetector(CoordinateConverter coordinateConverter)
        {
            this.coordinateConverter = coordinateConverter;
        }

        /// <summary>
        /// Detects fingers in the current frame using the depth and edge image. Afterwards the detected fingers are stored in the given frame data (detectedFingers).
        /// </summary>
        /// <param name="frameData">the current frame</param>
        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;
        }

        /// <summary>
        /// Sorts the finger slices in correct order an checks if the finger is a valid finger (<see cref="isCrippleFinger(Finger)"/>). If it is valid the finger is added to the list of detected fingers. Afterwards the edges around the finger are removed to suppress a new finger search for the same finger.
        /// </summary>
        /// <param name="trail">the slice trail of the possible finger</param>
        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());
        }

        /// <summary>
        ///  Tries to expand a trail along its direction.
        /// </summary>
        /// <param name="trail">the given finger slice trail</param>
        /// <param name="reversed">indicates in which direction the trail should be expanded</param>
        /// <returns>the expanded finger slice trail</returns>
        private FingerSliceTrail expandTrail(FingerSliceTrail trail, bool reversed = false)
        {
            if (reversed)
                trail.reverse();

            Vector2D currentDirection = trail.getEndDirection();
            Vector2D currentPosition = trail.EndSlice.Mid + 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.FingerMaxSliceLengthDifferencePerStep)
                {
                    gapCounter = 0;
                    numSlices++;
                    trail.addSlice(nextSlice);
                    currentDirection = trail.getEndDirection();
                    currentPosition = nextSlice.Mid + currentDirection;

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

            if (reversed)
                trail.reverse();

            return trail;
        }

        /// <summary>
        /// Tries to find a finger slice from a given position by searching in both orthogonal directions for an edge.
        /// </summary>
        /// <param name="position">the position somewhere in the middle of the possible finger</param>
        /// <param name="direction">the finger direction</param>
        /// <param name="reversed">indicates whether start and end should be swapped</param>
        /// <returns>the found finger slice or null</returns>
        private FingerSlice findFingerSliceFromMid(Vector2D position, Vector2D direction, bool reversed = false)
        {
            if (edgeImageAdapted.isRoughEdgeAt(position)) return null;

            UInt16 depth = depthImage.getDepthAt(position);
            int maxWidth2D = (int)coordinateConverter.convertLength3Dto2D(Parameters.FingerMaxWidth3D, depth);

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

            Vector2D startPositionStart = position + 0.5f * Parameters.FingerMinWidth2D * dirStart;
            Vector2D start = edgeImageAdapted.findNextRoughEdge(startPositionStart, dirStart, maxWidth2D);
            if (start == null) return null;

            Vector2D endPositionStart = position + 0.5f * Parameters.FingerMinWidth2D * dirEnd;
            Vector2D end = edgeImageAdapted.findNextRoughEdge(position, dirEnd, maxWidth2D);
            if (end == null) return null;

            FingerSlice slice = new FingerSlice(start, end);
            if (!fingerSliceDepthTest(slice) || slice.Length > maxWidth2D)
                return null;

            return slice;
        }

        /// <summary>
        /// Tries to find a finger slice from one point towards the given direction (searches for an edge).
        /// </summary>
        /// <param name="start">the start position</param>
        /// <param name="direction">the slice direction</param>
        /// <returns>the found finger slice or null</returns>
        private FingerSlice findFingerSliceFromStartEdge(Vector2D start, Vector2D direction)
        {
            Vector2D searchStart = start + Parameters.FingerMinWidth2D * direction;

            UInt16 depth = depthImage.getDepthAt(start);
            int maxWidth2D = (int)coordinateConverter.convertLength3Dto2D(Parameters.FingerMaxWidth3D, depth);

            Vector2D end = edgeImageAdapted.findNextRoughEdge(searchStart, direction, maxWidth2D);
            if (end == null)
                return null;

            FingerSlice slice = new FingerSlice(start, end);
            if (!fingerSliceDepthTest(slice))
                return null;

            return slice;
        }

        /// <summary>
        /// Creates a new FingerSliceTrail and expands it along its direction.
        /// </summary>
        /// <param name="startSlice">the initial finger slice</param>
        /// <param name="startDirection">the initial finger direction</param>
        /// <returns>the found FingerSliceTrail or null</returns>
        private FingerSliceTrail findFingerSliceTrail(FingerSlice startSlice, Vector2D startDirection)
        {
            FingerSliceTrail trail = new FingerSliceTrail(startSlice);

            Vector2D direction = startDirection;
            Vector2D position = startSlice.Mid + 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;
        }

        /// <summary>
        /// Checks if a possible finger slice is located on a finger. To pass this test, the depth value at the mid has to be lower than on the outside (start and end).
        /// </summary>
        /// <param name="slice">the possible finger slice</param>
        /// <returns>whether the slice is located on a finger</returns>
        private bool fingerSliceDepthTest(FingerSlice slice)
        {
            UInt16 depthStart = depthImage.getDepthAt(slice.Start.moveWithinBound(depthImage.Size, slice.Direction.getInverse(), Parameters.FingerContourMargin));
            UInt16 depthMid = depthImage.getDepthAt(slice.Mid);
            UInt16 depthEnd = depthImage.getDepthAt(slice.End.moveWithinBound(depthImage.Size, slice.Direction, Parameters.FingerContourMargin));
            return depthStart > depthMid && depthMid < depthEnd;
        }

        /// <summary>
        /// Gets the edge direction of the given edge point. The edge direction is either horizontal, vertical, diagonal (both) or null.
        /// </summary>
        /// <param name="edgePoint">the edge point</param>
        /// <returns>the edge direction at the given point</returns>
        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;
        }

        /// <summary>
        /// Checks whether the finger has enough space around itself (fingers from a closed hand shouldn't bee detected).
        /// </summary>
        /// <param name="finger">the finger</param>
        /// <returns>whether the finger has enough space around itself</returns>
        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);

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

            return minDepthDifference < Parameters.FingerMaxCrippleDifference;
        }

        /// <summary>
        /// Sorts a slice trail in the correct order, so that the start is at the finger tip and the end is at the hand. To guess the correct order the width of the first and last slice are compared.
        /// </summary>
        /// <param name="trail">the slice trail of the finger</param>
        /// <returns>the slice trail of the finger in correct order</returns>
        private FingerSliceTrail orderTrailTipToHand(FingerSliceTrail trail)
        {
            float startLength = trail.StartSlice.Length;
            float endLength = trail.EndSlice.Length;

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

            return trail;
        }
    }
}