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



namespace bbiwarg.Recognition.HandRecognition
{
    class HandDetector
    {
        private DepthImage depthImage;
        private Image<Gray, byte> modifiedHandDepthImage;
        private List<Finger> fingers;
        private Dictionary<Hand, List<Finger>> otherHandsFingers;

        public List<Hand> Hands { get; private set; }
        public Image<Gray, byte> HandMask { get; private set; }

        public HandDetector(DepthImage depthImage, List<Finger> fingers)
        {
            this.depthImage = depthImage;
            this.fingers = fingers;

            createModifiedHandEdgeImage();
            findHands();
            fixOverlappingFingers();
            findCentroids();
            createHandMask();
        }

        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);
            }
        }

        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);

                    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);
                }
            }
        }

        private Image<Gray, byte> getHandMask(Vector2D p)
        {
            Image<Gray, byte> mask = new Image<Gray, byte>(Parameters.ImageWidth + 2, Parameters.ImageHeight + 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, Parameters.ImageWidth, Parameters.ImageHeight));
        }

        private void fixOverlappingFingers()
        {
            extendOrMergeThroughOverlappingFingers();
            fillOverlappingFingers();
        }

        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(midSlice.Direction.getInverse(), Parameters.FingerOutMargin);
                        Vector2D midOut2 = midSlice.End.moveWithinBound(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 pHand, pHandExtension;
                            if (midOut1InHand)
                            {
                                pHand = midOut1;
                                pHandExtension = midOut2;
                            }
                            else
                            {
                                pHand = midOut2;
                                pHandExtension = midOut1;
                            }

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

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

        private void mergeToHand(Hand hand, Hand mergeHand)
        {
            hand.mergeWith(mergeHand);

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

        private void extendToHand(Hand hand, Vector2D p)
        {
            Image<Gray, byte> extendMask = getHandMask(p);
            int numPixels = extendMask.CountNonzero()[0];

            if (numPixels <= Parameters.HandExtensionMaxSize * Parameters.ImageNumPixels)
                hand.extendMask(extendMask);
        }

        private void fillOverlappingFingers()
        {
            foreach (Hand hand in Hands)
            {
                hand.fillOverlappingFingers(otherHandsFingers[hand]);
            }
        }

        private void findCentroids() {
            foreach (Hand hand in Hands)
                hand.findCentroid();
        }

        private void createHandMask()
        {
            HandMask = new Image<Gray, byte>(Parameters.ImageWidth, Parameters.ImageHeight);
            foreach (Hand hand in Hands)
                HandMask = HandMask.Or(hand.Mask);
        }
    }
}