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.Detectors.Fingers;

namespace bbiwarg.Detectors.Palm
{
    class PalmDetector
    {
        private int width, height;
        private DepthImage depthImage;
        private EdgeImage edgeImage;
        private PalmImage palmImage;

        private Image<Gray, Byte> handImage;
        private Image<Gray, Byte> pointingHandMask;

        private List<Finger> fingers;

        private Contour<Point> palmContour;
        private List<MCvConvexityDefect> convexityDefects;
        private Vector2D wristPoint, wristDirection;
        private LineSegment2DF wristLine, thumbLine;
        private Quad2D palmRect;

        public PalmDetector(DepthImage depthImage, EdgeImage edgeImage, FingerDetector fingerDetector, PalmImage palmImage)
        {
            width = depthImage.getWidth();
            height = depthImage.getHeight();
            this.depthImage = depthImage;
            this.edgeImage = edgeImage;
            this.palmImage = palmImage;

            handImage = depthImage.getImage().Convert<Byte>(delegate(short s) { return (s == depthImage.getMaxDepth()) ? (byte)0 : (byte)1; });
            
            fingers = getFingersWithoutThumb(fingerDetector);
            buildPointingHandMask();
            handImage = handImage.And(pointingHandMask);

            findLongestPalmContour();
            if (palmContour != null)
            {
                findConvexityDefactsSortedByDepth();
                removeConvexityDefectsNearFingerTips();

                findThumbLine();
                findWristLine();
                
                removePointsFromContour(wristLine, 1);
                removePointsFromContour(thumbLine, 1);

                if (palmContour.Count<Point>() != 0)
                {
                    findPalmRect();

                    draw();
                }
            }
        }

        public PalmRect getPalm()
        {
            return new PalmRect(palmRect);
        }

        private List<Finger> getFingersWithoutThumb(FingerDetector fingerDetector)
        {
            List<Finger> detectedFingers = fingerDetector.Fingers;
            Finger leftMost = null;
            float minX = float.MaxValue;
            foreach (Finger f in detectedFingers)
            {
                float midX = ((f.Hand + f.Tip) / 2).X;
                if (midX < minX)
                {
                    minX = midX;
                    leftMost = f;
                }
            }

            List<Finger> result = new List<Finger>();
            foreach (Finger f in detectedFingers)
            {
                if (f != leftMost)
                    result.Add(f);
            }
            return result;
        }

        private void fillFingerSlices(Image<Gray, Byte> image, byte val)
        {
            foreach (Finger f in fingers)
            {
                foreach (FingerSlice s in f.SliceTrail.getSlices())
                {
                    image.Draw(new LineSegment2DF(s.Start, s.End), new Gray(val), 1);
                }
            }
        }

        private Finger getLongestFinger()
        {
            float maxLength = 0;
            Finger longest = null;
            foreach (Finger f in fingers)
            {

                if (f.Line.Length > maxLength)
                {
                    maxLength = f.Line.Length;
                    longest = f;
                }
            }
            return longest;
        }

        private Point getPointInPointingHand()
        {
            Finger finger = getLongestFinger();
            if (finger == null)
                return new Point(0, 0);
            Vector2D direction = (finger.Hand - finger.Tip).normalize();
            Vector2D pos = finger.Hand + 20 * direction;
            return new Point(HelperFunctions.thresholdRange<int>(0, width - 1, (int) pos.X), HelperFunctions.thresholdRange<int>(0, height - 1, (int) pos.Y));
        }

        private void buildPointingHandMask()
        {
            pointingHandMask = new Image<Gray, byte>(width, height, new Gray(0));
            fillFingerSlices(pointingHandMask, 1);
            pointingHandMask = pointingHandMask.Dilate(1);
            pointingHandMask = pointingHandMask.Or(edgeImage.getImage().Convert<Byte>(delegate(byte b) { return (b == 0) ? (byte)0 : (byte)1; }));
            pointingHandMask = pointingHandMask.Dilate(1);

            MCvConnectedComp tmp = new MCvConnectedComp();
            CvInvoke.cvFloodFill(pointingHandMask.Ptr, getPointInPointingHand(), new MCvScalar(2), new MCvScalar(0), new MCvScalar(0), out tmp, 0, IntPtr.Zero);
            pointingHandMask = pointingHandMask.Convert<Byte>(delegate(byte b) { return (b == 2) ? (byte)0 : (byte)1; });
            pointingHandMask = pointingHandMask.Erode(1);
            fillFingerSlices(pointingHandMask, 0);
            pointingHandMask = pointingHandMask.Erode(2);
        }

        private void findLongestPalmContour()
        {
            Contour<Point> contour = handImage.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 findWristDirection()
        {
            PointF[] points = new PointF[palmContour.Count<Point>()];
            int index = 0;
            foreach (Point p in palmContour)
            {
                points[index] = new PointF(p.X, p.Y);
                ++index;
            }
            PointF direction, tmp;
            PointCollection.Line2DFitting(points, Emgu.CV.CvEnum.DIST_TYPE.CV_DIST_L2, out direction, out tmp);
            //Vector2D direction = new Vector2D(thumbLine.Direction);
            wristDirection = new Vector2D(-direction.Y, direction.X);
        }

        private void findConvexityDefactsSortedByDepth()
        {
            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 removeConvexityDefectsNearFingerTips()
        {
            List<MCvConvexityDefect> newDefects = new List<MCvConvexityDefect>();
            foreach (MCvConvexityDefect d in convexityDefects)
            {
                float minFingerTipDist = float.MaxValue;
                foreach (Finger f in fingers)
                {
                    float dist = f.Tip.getDistanceTo(new Vector2D(d.DepthPoint));
                    if (dist < minFingerTipDist)
                        minFingerTipDist = dist;
                }
                if (minFingerTipDist > 20)
                    newDefects.Add(d);
            }
            convexityDefects = newDefects;
        }

        private void findWristPoint()
        {
            if (convexityDefects.Count > 1)
                wristPoint = new Vector2D(convexityDefects[1].DepthPoint);
            else
                wristPoint = new Vector2D(-1, -1);
        }

        private void findWristLine()
        {
            findWristPoint();
            findWristDirection();
            wristLine = new LineSegment2DF(wristPoint - 1000 * wristDirection, wristPoint + 1000 * wristDirection);
        }

        private void findThumbLine()
        {
            if (convexityDefects.Count > 0)
            {
                MCvConvexityDefect thumbDefect = convexityDefects[0];
                Vector2D p1 = new Vector2D(thumbDefect.DepthPoint);
                Vector2D p2 = new Vector2D(thumbDefect.StartPoint);
                Vector2D direction = (p1 - p2).normalize();
                thumbLine = new LineSegment2DF(p1 - 1000 * direction, p1 + 1000 * direction);
            }
            else
            {
                thumbLine = new LineSegment2DF(new PointF(-1, -1), new PointF(-1, -1));
            }
        }

        private void removePointsFromContour(LineSegment2DF line, int sideToRemove)
        {
            Contour<Point> newContour = new Contour<Point>(new MemStorage());
            int index = 0;
            foreach (Point p in palmContour)
            {
                if (line.Side(p) != sideToRemove)
                {
                    newContour.Insert(index, p);
                    ++index;
                }
            }
            palmContour = newContour;
        }

        private void findPalmRect()
        {
            //palmRect = palmContour.GetMinAreaRect();

            Line2D thumbLine2 = new Line2D(new Vector2D(thumbLine.P1), new Vector2D(thumbLine.P2));
            Line2D wristLine2 = new Line2D(new Vector2D(wristLine.P1), new Vector2D(wristLine.P2));

            Vector2D intersection = thumbLine2.intersection(wristLine2);
            if (intersection != null)
            {
                Vector2D maxThumb = new Vector2D(0, 0), maxWrist = new Vector2D(0, 0);
                float maxDistanceThumb = 0, maxDistanceWrist = 0;
                foreach (Point p in palmContour)
                {
                    Vector2D v = new Vector2D(p);
                    Vector2D projected = thumbLine2.projectToLine(v);

                    float dist = projected.getDistanceTo(intersection);
                    if (dist > maxDistanceThumb)
                    {
                        maxDistanceThumb = dist;
                        maxThumb = projected;
                    }

                    projected = wristLine2.projectToLine(v);

                    dist = projected.getDistanceTo(intersection);
                    if (dist > maxDistanceWrist)
                    {
                        maxDistanceWrist = dist;
                        maxWrist = projected;
                    }
                }

                Vector2D origin = intersection;
                palmRect = new Quad2D(origin, (maxThumb - origin), (maxWrist - origin));
            }
            else
            {
                palmRect = null;
            }
        }

        private void draw()
        {
            palmImage.drawContour(palmContour);

            palmImage.drawLine(wristLine, PalmImageState.wristLine);
            palmImage.drawLine(thumbLine, PalmImageState.thumbLine);

            if (palmRect != null)
            {
                PointF[] vertices = palmRect.getVertices();
                for (int i = 0; i < 4; ++i)
                    palmImage.drawLine(new LineSegment2DF(vertices[i], vertices[(i + 1) % 4]), PalmImageState.palmRect);


                palmImage.drawGrid(new Vector2D(vertices[0]), new Vector2D(vertices[1]), new Vector2D(vertices[2]), new Vector2D(vertices[3]));
            }
        }
    }
}