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

namespace BBIWARG.Recognition.PalmRecognition
{
    /// <summary>
    /// Detects palms by iterating over each hand, if the hand has exactly one finger (possible thumb) it calculates its convexity defects and checks if there is a convexity defect that matches the requirements for a thumb defect. If a thumb defect is found, the four palm points are generated and a new palm is created.
    /// </summary>
    internal class PalmDetector
    {
        /// <summary>
        /// the depth image of the current frame
        /// </summary>
        private DepthImage depthImage;

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

        /// <summary>
        /// the palms in the current frames
        /// </summary>
        private List<Palm> palms;

        /// <summary>
        /// Detects palms in the current frame and stores them in frameData.detectedPalms
        /// </summary>
        /// <param name="frameData">the current frame</param>
        public void detectPalms(FrameData frameData)
        {
            depthImage = frameData.DepthImage;
            hands = frameData.TrackedHands;

            findPalms();

            frameData.DetectedPalms = palms;
        }

        /// <summary>
        /// Finds the four palm points and creates a new palm
        /// </summary>
        /// <param name="hand">the hand of the palm</param>
        /// <param name="thumbDefect">the convexity defect of the thumb</param>
        /// <returns>a new palm</returns>
        private Palm createPalm(Hand hand, ConvexityDefect thumbDefect)
        {
            HandSide side = determineHandSide(thumbDefect);
            Vector2D wristUpper = findWristUpper(hand, thumbDefect);
            Vector2D fingersUpper = findFingersUpper(hand, thumbDefect, side);
            float palmWidth = findPalmWidth(hand, thumbDefect, side);
            float palmLength = wristUpper.getDistanceTo(fingersUpper);
            Vector2D directionWristFingers = thumbDefect.VectorLong.normalize();
            Vector2D directionUpperLower = thumbDefect.VectorLong.getOrthogonal(side == HandSide.Right).normalize();
            Vector2D wristLower = wristUpper + palmWidth * directionUpperLower;
            Vector2D fingersLower = wristUpper + 0.75f * palmLength * directionWristFingers + 0.75f * palmWidth * directionUpperLower;

            return new Palm(hand, thumbDefect, side, wristUpper, fingersUpper, fingersLower, wristLower);
        }

        /// <summary>
        /// Determines the handedness of the palm's hand
        /// </summary>
        /// <param name="thumbDefect">the convexity defect of the thumb</param>
        /// <returns>the handedness of the palm's hand</returns>
        private HandSide determineHandSide(ConvexityDefect thumbDefect)
        {
            if (thumbDefect.VectorShort.crossProduct(thumbDefect.VectorLong) < 0)
                return HandSide.Right;
            else
                return HandSide.Left;
        }

        /// <summary>
        /// Gets the convexity Defects from a hand mask.
        /// </summary>
        /// <param name="hand">the hand</param>
        /// <returns>the convexity defects</returns>
        private List<ConvexityDefect> findConvexityDefects(Hand hand)
        {
            List<ConvexityDefect> convexityDefects;
            using (MemStorage ms = new MemStorage())
            {
                Contour<Point> contour = hand.Mask.FindContours(Emgu.CV.CvEnum.CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, Emgu.CV.CvEnum.RETR_TYPE.CV_RETR_EXTERNAL);
                List<MCvConvexityDefect> mcvConvexityDefects = new List<MCvConvexityDefect>(contour.GetConvexityDefacts(ms, Emgu.CV.CvEnum.ORIENTATION.CV_CLOCKWISE));

                convexityDefects = new List<ConvexityDefect>();
                foreach (MCvConvexityDefect defect in mcvConvexityDefects)
                    convexityDefects.Add(new ConvexityDefect(defect));
            }
            return convexityDefects;
        }

        /// <summary>
        /// Finds the upper finger end by walking from the defect.OuterLong in the finger direction, until it reaches the end of the middle finger below.
        /// </summary>
        /// <param name="hand">the hand</param>
        /// <param name="thumbDefect">the convexity defect of the thumb</param>
        /// <param name="side">the handedness</param>
        /// <returns>the position of the upper fingers end</returns>
        private Vector2D findFingersUpper(Hand hand, ConvexityDefect thumbDefect, HandSide side)
        {
            Vector2D fingersDirection = thumbDefect.VectorLong.normalize();
            Vector2D lowerDirection = fingersDirection.getOrthogonal(side == HandSide.Right).normalize();
            Vector2D fingersUpper = thumbDefect.OuterLong;
            Vector2D fingersUpperNext = fingersUpper + fingersDirection;
            bool handBelow = true;
            while (handBelow && fingersUpperNext.isInBound(depthImage.Size))
            {
                fingersUpper = fingersUpperNext;
                fingersUpperNext += fingersDirection;
                Vector2D below = fingersUpper.copy();
                handBelow = false;
                int distance = 0;
                while (!handBelow && distance < Parameters.FingerMaxWidth2D && below.isInBound(depthImage.Size))
                {
                    handBelow = hand.isInside(below);
                    below += lowerDirection;
                }
            }
            return fingersUpper;
        }

        /// <summary>
        /// Find Palms by checking each hand with only one finger, if that finger is a thumb.
        /// </summary>
        private void findPalms()
        {
            palms = new List<Palm>();
            foreach (Hand hand in hands)
            {
                if (hand.Fingers.Count == 1)
                {
                    List<ConvexityDefect> convexityDefects = findConvexityDefects(hand);
                    ConvexityDefect thumbDefect = findThumbDefect(hand.Fingers[0], convexityDefects);
                    if (thumbDefect != null)
                    {
                        Palm palm = createPalm(hand, thumbDefect);
                        palms.Add(palm);
                    }
                }
            }
        }

        /// <summary>
        /// Gets the palm width by checking for the maximum orthogonal length within the hand along multiple positions on the index finger (defect.inner to defect.outerLong)
        /// </summary>
        /// <param name="hand">the hand</param>
        /// <param name="thumbDefect">the convexity defect of the thumb</param>
        /// <param name="side">the handedness</param>
        /// <returns>the palm width</returns>
        private float findPalmWidth(Hand hand, ConvexityDefect thumbDefect, HandSide side)
        {
            Vector2D lowerDirection = thumbDefect.VectorLong.getOrthogonal(side == HandSide.Right).normalize();
            Vector2D current = thumbDefect.Inner;
            Vector2D step = thumbDefect.VectorLong / Parameters.PalmNumPositionsForPalmWidth;

            float maxWidth = float.MinValue;
            for (int i = 0; i < Parameters.PalmNumPositionsForPalmWidth; i++)
            {
                Vector2D lower = current;
                Vector2D lowerNext = lower + 10 * lowerDirection;
                while (lowerNext.isInBound(depthImage.Size) && hand.isInside(lowerNext))
                {
                    lower = lowerNext;
                    lowerNext += lowerDirection;
                }
                maxWidth = Math.Max(maxWidth, current.getDistanceTo(lower));
                current += step;
            }
            return maxWidth;
        }

        /// <summary>
        /// Checks all convexity defects sorted by length if they match the thumb defect requirements and returns the first match or null if none match.
        /// </summary>
        /// <param name="thumb">the possible thumb</param>
        /// <param name="convexityDefects">the convexity defects</param>
        /// <returns>the thumb defect or null</returns>
        private ConvexityDefect findThumbDefect(Finger thumb, List<ConvexityDefect> convexityDefects)
        {
            convexityDefects.Sort((cd1, cd2) => cd2.Depth.CompareTo(cd1.Depth));
            foreach (ConvexityDefect defect in convexityDefects)
            {
                if (defect.isPossibleThumbDefect(thumb))
                    return defect;
            }
            return null;
        }

        /// <summary>
        /// Finds the upper wrist end by walking from the defect.Inner towards the wrist direction, until it reaches the hand boundaries.
        /// </summary>
        /// <param name="hand">the hand</param>
        /// <param name="thumbDefect">the convexity defect of the thumb</param>
        /// <returns>the position of the upper wrist end</returns>
        private Vector2D findWristUpper(Hand hand, ConvexityDefect thumbDefect)
        {
            Vector2D wristDirection = thumbDefect.VectorLong.getInverse().normalize();
            Vector2D wristUpper = thumbDefect.Inner;
            Vector2D wristUpperNext = wristUpper + 5 * wristDirection;
            while (wristUpperNext.isInBound(depthImage.Size) && hand.isInside(wristUpperNext))
            {
                wristUpper = wristUpperNext;
                wristUpperNext += wristDirection;
            }
            return wristUpper;
        }
    }
}