using BBIWARG.Images;
using BBIWARG.Input.InputHandling;
using BBIWARG.Recognition.FingerRecognition;
using BBIWARG.Utility;
using Emgu.CV;
using Emgu.CV.Structure;
using System;
using System.Collections.Generic;
using System.Drawing;

namespace BBIWARG.Recognition.HandRecognition
{
    /// <summary>
    /// Finds Hands by iterating over all fingers and flooding them. Each filled region is considered to be one hand and each finger belongs to one hand. To improve the hand contours, the hand mask are filled with the defects caused by overlapping fingers.
    /// </summary>
    internal class HandDetector
    {
        /// <summary>
        /// the depth image of the current frame
        /// </summary>
        private DepthImage depthImage;

        /// <summary>
        /// the fingers in the current frame
        /// </summary>
        private List<Finger> fingers;

        /// <summary>
        /// the hands in the current frame
        /// </summary>
        private List<Hand> hands;

        /// <summary>
        /// the modified depth image of the current frame (hand flooding changes depth image)
        /// </summary>
        private Image<Gray, byte> modifiedHandDepthImage;

        /// <summary>
        /// a mapping of hands and list of fingers, that don't belong to that specific hand
        /// </summary>
        private Dictionary<Hand, List<Finger>> otherHandsFingers;

        /// <summary>
        /// Detects Hands in the current frame and stores found hands in frameData.detectedHands
        /// </summary>
        /// <param name="frameData">the current frame</param>
        public void detectHands(FrameData frameData)
        {
            depthImage = frameData.DepthImage;
            fingers = frameData.TrackedFingers;

            createModifiedHandEdgeImage();
            findHands();
            fixOverlappingFingers();
            findCentroids();

            frameData.DetectedHands = hands;
        }

        /// <summary>
        /// Creates the modified hand image. The image is a copy of the original depth image with a contour around each finger (to prevent flood fill from filling through fingers).
        /// </summary>
        private void createModifiedHandEdgeImage()
        {
            modifiedHandDepthImage = depthImage.Image.Copy();

            foreach (Finger finger in fingers)
            {
                Int16 depthAtHand = depthImage.getDepthAt(finger.HandPoint);
                Point[] contour = finger.getContour(0f).ToArray();
                modifiedHandDepthImage.DrawPolyline(contour, false, new Gray(depthAtHand), 1);
            }
        }

        /// <summary>
        /// Merges two hands if they are separated by an overlapping finger or extends the hand mask through an overlapping finger.
        /// </summary>
        private void extendOrMergeThroughOverlappingFingers()
        {
            List<Hand> mergedHands = new List<Hand>();

            foreach (Hand hand in hands)
            {
                if (!mergedHands.Contains(hand))
                {
                    List<Hand> mergeHands = new List<Hand>();
                    foreach (Finger overlappingFinger in otherHandsFingers[hand])
                    {
                        FingerSlice midSlice = overlappingFinger.SliceTrail.MidSlice;
                        Vector2D midOut1 = midSlice.Start.moveWithinBound(depthImage.Size, midSlice.Direction.getInverse(), Parameters.FingerOutMargin);
                        Vector2D midOut2 = midSlice.End.moveWithinBound(depthImage.Size, midSlice.Direction, Parameters.FingerOutMargin);
                        Int16 depthAtMidOut1 = depthImage.getDepthAt(midOut1);
                        Int16 depthAtMidOut2 = depthImage.getDepthAt(midOut2);
                        bool midOut1InHand = hand.isInside(midOut1);
                        bool midOut2InHand = hand.isInside(midOut2);

                        Int16 maxDepth = depthImage.MaxDepth;
                        if (midOut1InHand != midOut2InHand && depthAtMidOut1 != maxDepth && depthAtMidOut2 != maxDepth && Math.Abs(depthAtMidOut1 - depthAtMidOut2) < Parameters.HandExtendMaxDifference)
                        {
                            Vector2D handPoint, handExtensionPoint;
                            if (midOut1InHand)
                            {
                                handPoint = midOut1;
                                handExtensionPoint = midOut2;
                            }
                            else
                            {
                                handPoint = midOut2;
                                handExtensionPoint = midOut1;
                            }

                            // check if pHandExtension is in other hand (if so -> merge with hand)
                            bool merge = false;
                            foreach (Hand mergeHand in hands)
                            {
                                if (mergeHand.isInside(handExtensionPoint) && !mergedHands.Contains(mergeHand))
                                {
                                    mergeHands.Add(mergeHand);
                                    merge = true;
                                    break;
                                }
                            }

                            // if no merge, extend hand
                            if (!merge)
                                extendToHand(hand, handExtensionPoint);
                        }
                    }
                    foreach (Hand mergeHand in mergeHands)
                    {
                        mergeToHand(hand, mergeHand);
                        mergedHands.Add(mergeHand);
                    }
                }
            }
            foreach (Hand mergedHand in mergedHands)
                hands.Remove(mergedHand);
        }

        /// <summary>
        /// Extends the hand mask of a given hand by flood filling starting from the given point.
        /// </summary>
        /// <param name="hand">the hand that should be extended</param>
        /// <param name="p">the flood fill starting point</param>
        private void extendToHand(Hand hand, Vector2D p)
        {
            Image<Gray, byte> extendMask = getHandMask(p);
            int numPixels = extendMask.CountNonzero()[0];

            if (numPixels <= Parameters.HandExtensionMaxRelativeSize * depthImage.Size.NumPixels)
                hand.extendMask(extendMask);
        }

        /// <summary>
        /// Fills holes caused by overlapping fingers.
        /// </summary>
        private void fillOverlappingFingers()
        {
            foreach (Hand hand in hands)
            {
                hand.fillOverlappingFingers(otherHandsFingers[hand]);
            }
        }

        /// <summary>
        /// Finds the hands centroids.
        /// </summary>
        private void findCentroids()
        {
            foreach (Hand hand in hands)
                hand.findCentroid();
        }

        /// <summary>
        /// Finds hands by flood filling from each finger (mask). All unassigned fingers that lie within the hand are assigned to the hand, all other fingers are mapped as fingers that don't belong to that specific hand.
        /// </summary>
        private void findHands()
        {
            hands = new List<Hand>();
            otherHandsFingers = new Dictionary<Hand, List<Finger>>();
            List<Finger> assignedFingers = new List<Finger>();

            foreach (Finger finger in fingers)
            {
                if (!assignedFingers.Contains(finger))
                {
                    Image<Gray, byte> handMask = getHandMask(finger.HandPoint);

                    int numPixels = handMask.CountNonzero()[0];
                    if (numPixels > Parameters.HandMaxSize * depthImage.Size.NumPixels)
                    {
                        assignedFingers.Add(finger);
                        break;
                    }

                    List<Finger> fingersOnHand = new List<Finger>();
                    List<Finger> fingersOnOtherHand = new List<Finger>();

                    foreach (Finger f in fingers)
                    {
                        if (!assignedFingers.Contains(f) && handMask.Data[f.HandPoint.IntY, f.HandPoint.IntX, 0] != 0)
                        {
                            fingersOnHand.Add(f);
                            assignedFingers.Add(f);
                        }
                        else
                            fingersOnOtherHand.Add(f);
                    }

                    Hand hand = new Hand(handMask, fingersOnHand);
                    otherHandsFingers.Add(hand, fingersOnOtherHand);
                    hands.Add(hand);
                }
            }
        }

        /// <summary>
        /// Fixes overlapping fingers by merging two hands if they are separated by a finger and/or fills holes caused by overlapping fingers.
        /// </summary>
        private void fixOverlappingFingers()
        {
            extendOrMergeThroughOverlappingFingers();
            fillOverlappingFingers();
        }

        /// <summary>
        /// Flood fills from a given point and returns the filled area as mask.
        /// </summary>
        /// <param name="p">flood fill starting point</param>
        /// <returns>the filled area as mask</returns>
        private Image<Gray, byte> getHandMask(Vector2D p)
        {
            Image<Gray, byte> mask = new Image<Gray, byte>(depthImage.Size.Width + 2, depthImage.Size.Height + 2);
            MCvConnectedComp comp = new MCvConnectedComp();
            CvInvoke.cvFloodFill(modifiedHandDepthImage, p, new MCvScalar(255), new MCvScalar(Parameters.HandFloodFillDownDiff), new MCvScalar(Parameters.HandFloodFillUpDiff), out comp, Emgu.CV.CvEnum.CONNECTIVITY.FOUR_CONNECTED, Emgu.CV.CvEnum.FLOODFILL_FLAG.DEFAULT, mask);
            return mask.Copy(new Rectangle(1, 1, depthImage.Size.Width, depthImage.Size.Height));
        }

        /// <summary>
        /// Merges two hands together and updates the list of other hands fingers.
        /// </summary>
        /// <param name="hand">the first hand (other hand will be merged to this one)</param>
        /// <param name="mergeHand">the second hand (this hand will be dropped afterwards)</param>
        private void mergeToHand(Hand hand, Hand mergeHand)
        {
            hand.mergeWith(mergeHand);

            foreach (Finger finger in mergeHand.Fingers)
                otherHandsFingers[hand].Remove(finger);

            otherHandsFingers.Remove(mergeHand);
        }
    }
}