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

namespace bbiwarg.Recognition.PalmRecognition
{
    class PalmDetector
    {
        private DepthImage depthImage;
        private List<Hand> hands;

        public List<Palm> Palms;

        public PalmDetector(DepthImage depthImage, List<Hand> hands)
        {
            this.depthImage = depthImage;
            this.hands = hands;

            findPalms();
        }

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

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

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

        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.moveWithinBound(directionUpperLower, palmWidth);
            Vector2D fingersLower = (wristUpper + 0.75f * palmLength * directionWristFingers + 0.75f * palmWidth * directionUpperLower).moveInBound((directionUpperLower + directionWristFingers).getInverse().normalize());

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

        private HandSide determineHandSide(ConvexityDefect thumbDefect)
        {
            if (thumbDefect.VectorShort.crossProduct(thumbDefect.VectorLong) < 0)
                return HandSide.Right;
            else
                return HandSide.Left;
        }

        private Vector2D findWristUpper(Hand hand, ConvexityDefect thumbDefect)
        {
            Vector2D wristDirection = thumbDefect.VectorLong.getInverse().normalize();
            Vector2D wristUpper = thumbDefect.Inner.moveWithinBound(wristDirection, 5f);
            Vector2D wristUpperNext = wristUpper + wristDirection;
            while (wristUpperNext.isInBound() && hand.isInside(wristUpperNext))
            {
                wristUpper = wristUpperNext;
                wristUpperNext += wristDirection;
            }
            return wristUpper;
        }

        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())
            {
                fingersUpper = fingersUpperNext;
                fingersUpperNext += fingersDirection;
                Vector2D below = fingersUpper.copy();
                handBelow = false;
                int distance = 0;
                while (!handBelow && distance < Parameters.FingerMaxWidth && below.isInBound())
                {
                    handBelow = hand.isInside(below);
                    below += lowerDirection;
                }
            }
            return fingersUpper;
        }

        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.moveWithinBound(lowerDirection, 10f);
                float width = current.getDistanceTo(lower);
                while (lower.isInBound() && hand.isInside(lower))
                {
                    lower += lowerDirection;
                    width += 1;
                }
                maxWidth = Math.Max(maxWidth, width);
                current += step;
            }
            return maxWidth;
        }
    }
}