using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Drawing;

using Emgu.CV;
using Emgu.CV.Structure;

using bbiwarg.Utility;
using bbiwarg.Images;
using bbiwarg.Recognition.FingerRecognition;
using bbiwarg.Recognition.HandRecognition;
using bbiwarg.Graphics;

namespace bbiwarg.Recognition.PalmRecognition
{
    class PalmDetector
    {
        public Contour<Point> PalmContour { get; private set; }
        public Vector2D ThumbDefectStart { get; private set; }
        public Vector2D ThumbDefectEnd { get; private set; }
        public Vector2D ThumbDefectDepth { get; private set; }
        private Hand palmHand, pointingHand;
        private List<MCvConvexityDefect> convexityDefects;
        private Kalman2DPositionFilter thumbDefectDepthFilter, thumbDefectStartFilter, thumbDefectEndFilter;
        private int numFramesNoHandFound;
        private Quadrangle lastPalmQuad;
        private List<Hand> hands;

        public Quadrangle PalmQuad { get; private set; }
        public Hand.HandSide PalmHandSide { get; private set; }

        public PalmDetector()
        {
            thumbDefectDepthFilter = new Kalman2DPositionFilter(Constants.PalmThumbDefectmXX, Constants.PalmThumbDefectmXY,
                                                                Constants.PalmThumbDefectmYY, Constants.PalmThumbDefectProcessNoise);
            thumbDefectStartFilter = new Kalman2DPositionFilter(Constants.PalmThumbDefectmXX, Constants.PalmThumbDefectmXY,
                                                                Constants.PalmThumbDefectmYY, Constants.PalmThumbDefectProcessNoise);
            thumbDefectEndFilter = new Kalman2DPositionFilter(Constants.PalmThumbDefectmXX, Constants.PalmThumbDefectmXY,
                                                                Constants.PalmThumbDefectmYY, Constants.PalmThumbDefectProcessNoise);
        }

        public void findPalmQuad(List<Hand> hands, Image<Gray, Byte> foregroundMask)
        {
            this.hands = hands;

            if (hands.Count == 1 && hands[0].Fingers.Count == 1)
            {
                palmHand = hands[0];
                pointingHand = null;
            }
            else if (hands.Count == 2)
            {
                if (hands[0].Fingers.Count > 1)
                {
                    pointingHand = hands[0];
                    palmHand = hands[1];
                }
                else if (hands[1].Fingers.Count > 1)
                {
                    pointingHand = hands[1];
                    palmHand = hands[0];
                }
                else if (hands[0].Fingers[0].LineSegment.Length > hands[1].Fingers[0].LineSegment.Length)
                {
                    pointingHand = hands[0];
                    palmHand = hands[1];
                }
                else
                {
                    pointingHand = hands[1];
                    palmHand = hands[0];
                }
            }

            if ((hands.Count == 1 && hands[0].Fingers.Count == 1) || hands.Count == 2)
            {
                findLongestPalmContour();
                findConvexityDefectsSortedByDepth();
                if (pointingHand != null)
                    removeConvexityDefectsCausedByFingers();
                findHandPoints(foregroundMask);
            }

            if (hands.Count == 0)
            {
                ++numFramesNoHandFound;
                if (numFramesNoHandFound == Constants.PalmNumFramesNoHandReset)
                    reset();
            }
            else
            {
                numFramesNoHandFound = 0;
            }
        }

        public void reset()
        {
            thumbDefectDepthFilter.reset();
            thumbDefectStartFilter.reset();
            thumbDefectEndFilter.reset();
            PalmContour = null;
            PalmQuad = null;
            PalmHandSide = Hand.HandSide.Left;
            numFramesNoHandFound = 0;
            lastPalmQuad = null;
        }

        private void findLongestPalmContour()
        {
            Contour<Point> contour = palmHand.Mask.FindContours(Emgu.CV.CvEnum.CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE,
                                                                Emgu.CV.CvEnum.RETR_TYPE.CV_RETR_EXTERNAL);

            PalmContour = contour;
            double maxPerimeter = 0;
            while (contour != null)
            {
                if (contour.Perimeter > maxPerimeter)
                {
                    maxPerimeter = contour.Perimeter;
                    PalmContour = contour;
                }
                contour = contour.HNext;
            }
        }

        private void findConvexityDefectsSortedByDepth()
        {
            convexityDefects = new List<MCvConvexityDefect>(PalmContour.GetConvexityDefacts(new MemStorage(), Emgu.CV.CvEnum.ORIENTATION.CV_CLOCKWISE));
            convexityDefects.Sort(delegate(MCvConvexityDefect d1, MCvConvexityDefect d2)
            {
                if (d1.Depth < d2.Depth)
                    return 1;
                else if (d1.Depth > d2.Depth)
                    return -1;
                return 0;
            });
        }

        private void removeConvexityDefectsCausedByFingers()
        {
            List<MCvConvexityDefect> newDefects = new List<MCvConvexityDefect>();
            foreach (MCvConvexityDefect d in convexityDefects)
            {
                bool fingerIntersectsStartEnd = false;
                float minFingerLineDist = float.MaxValue;
                foreach (Finger f in pointingHand.Fingers)
                {
                    Utility.LineSegment2D defectLine = new Utility.LineSegment2D(new Vector2D(d.StartPoint), new Vector2D(d.EndPoint));
                    Vector2D intersection = defectLine.Line.getIntersection(f.LineSegment.Line);
                    if (intersection != null &&
                        intersection.isInBox(defectLine.P1, defectLine.P2) && intersection.isInBox(f.LineSegment.P1, f.LineSegment.P2))
                    {
                        fingerIntersectsStartEnd = true;
                        break;
                    }

                    Vector2D mid = (new Vector2D(d.StartPoint) + new Vector2D(d.EndPoint)) / 2.0f;
                    float dist = f.LineSegment.getDistanceTo(mid);
                    if (dist < minFingerLineDist)
                        minFingerLineDist = dist;
                }

                if (minFingerLineDist >= Constants.PalmMinDefectMidFingerLineDistance && !fingerIntersectsStartEnd)
                    newDefects.Add(d);
            }
            convexityDefects = newDefects;
        }

        private MCvConvexityDefect? findThumbDefect()
        {
            foreach (MCvConvexityDefect d in convexityDefects)
            {
                Vector2D depth = new Vector2D(d.DepthPoint);
                Vector2D start = new Vector2D(d.StartPoint);
                Vector2D end = new Vector2D(d.EndPoint);

                float angle = (float)((depth - start).getAngleBetween(depth - end) * 180 / Math.PI);

                float l1 = (depth - start).Length;
                float l2 = (depth - end).Length;
                float startEndLengthQuotient = Math.Max(l1, l2) / Math.Min(l1, l2);

                float depthThumbLengthQuotient = d.Depth / palmHand.Fingers[0].LineSegment.Length;

                if (angle <= Constants.PalmMaxThumbDefectAngle &&
                    startEndLengthQuotient >= Constants.PalmMinThumbDefectStartEndLengthQuotient &&
                    startEndLengthQuotient <= Constants.PalmMaxThumbDefectStartEndLengthQuotient &&
                    depthThumbLengthQuotient >= Constants.PalmMinTumbDefectDepthThumbLengthQuotient &&
                    depthThumbLengthQuotient <= Constants.PalmMaxTumbDefectDepthThumbLengthQuotient)
                {
                    return d;
                }
            }

            return null;
        }

        float getForegroundPixelPercentage(Quadrangle quad, Image<Gray, Byte> foregroundMask)
        {
            float numPixelsInMask = quad.Mask.CountNonzero()[0];
            float numPixelsInMaskInForeground = (quad.Mask & foregroundMask).CountNonzero()[0];

            return numPixelsInMaskInForeground / numPixelsInMask;
        }

        private void findHandPoints(Image<Gray, Byte> foregroundMask)
        {
            MCvConvexityDefect? thumbDefect = findThumbDefect();

            if (thumbDefect != null)
            {
                ThumbDefectDepth = new Vector2D(thumbDefect.Value.DepthPoint);
                ThumbDefectStart = new Vector2D(thumbDefect.Value.StartPoint);
                ThumbDefectEnd = new Vector2D(thumbDefect.Value.EndPoint);

                if (!thumbDefectDepthFilter.Initialized)
                {
                    thumbDefectDepthFilter.setInitialPosition(ThumbDefectDepth);
                    thumbDefectStartFilter.setInitialPosition(ThumbDefectStart);
                    thumbDefectEndFilter.setInitialPosition(ThumbDefectEnd);
                }
                else
                {
                    ThumbDefectDepth = thumbDefectDepthFilter.getCorrectedPosition(ThumbDefectDepth);
                    ThumbDefectStart = thumbDefectStartFilter.getCorrectedPosition(ThumbDefectStart);
                    ThumbDefectEnd = thumbDefectEndFilter.getCorrectedPosition(ThumbDefectEnd);
                }

                Vector2D handLength, handWidth, longestLineEndpoint, topLeft, bottomLeft, bottomRight, topRight;

                Vector2D startDepth = ThumbDefectStart - ThumbDefectDepth;
                Vector2D endDepth = ThumbDefectEnd - ThumbDefectDepth;

                if (startDepth.Length > endDepth.Length)
                {
                    handLength = startDepth;
                    longestLineEndpoint = ThumbDefectStart;
                    if (hands.Count == 1)
                    {
                        if (ThumbDefectStart.X > ThumbDefectDepth.X)
                            hands[0].Side = Hand.HandSide.Left;
                        else
                            hands[0].Side = Hand.HandSide.Right;
                    }
                }
                else
                {
                    handLength = endDepth;
                    longestLineEndpoint = ThumbDefectEnd;
                    if (hands.Count == 1)
                    {
                        if (ThumbDefectEnd.X > ThumbDefectDepth.X)
                            hands[0].Side = Hand.HandSide.Left;
                        else
                            hands[0].Side = Hand.HandSide.Right;
                    }
                }

                Quadrangle quad;
                if (palmHand.Side == Hand.HandSide.Left)
                {
                    handWidth = 0.85f * new Vector2D(-handLength.Y, handLength.X);
                    topLeft = longestLineEndpoint + 0.15f * handLength;
                    bottomLeft = ThumbDefectDepth - 0.4f * handLength;
                    bottomRight = bottomLeft + handWidth;
                    topRight = bottomRight + 1.2f * handLength - 0.3f * handWidth;
                    quad = new Quadrangle(bottomLeft, topLeft, topRight, bottomRight, foregroundMask.Width, foregroundMask.Height);
                }
                else
                {
                    handWidth = 0.85f * new Vector2D(handLength.Y, -handLength.X);
                    topRight = longestLineEndpoint + 0.15f * handLength;
                    bottomRight = ThumbDefectDepth - 0.4f * handLength;
                    bottomLeft = bottomRight + handWidth;
                    topLeft = bottomLeft + 1.2f * handLength - 0.3f * handWidth;
                    quad = new Quadrangle(topRight, bottomRight, bottomLeft, topLeft, foregroundMask.Width, foregroundMask.Height);
                }

                if ((lastPalmQuad == null ||
                    (quad.Area / lastPalmQuad.Area >= Constants.PalmMinAreaQuotient && quad.Area / lastPalmQuad.Area <= Constants.PalmMaxAreaQuotient)) &&
                    getForegroundPixelPercentage(quad, foregroundMask) >= Constants.PalmMinPrecentageQuadForeground)
                {
                    PalmQuad = quad;
                    PalmHandSide = palmHand.Side;
                    lastPalmQuad = PalmQuad;
                }
            }

            if (lastPalmQuad != null && getForegroundPixelPercentage(lastPalmQuad, foregroundMask) <= Constants.PalmMaxPrecentageQuadForegroundReset)
                reset();
        }
    }
}