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;
using bbiwarg.Graphics;

namespace bbiwarg.Detectors.Palm
{
    class PalmDetector
    {
        private int width, height;
        private OutputImage outputImage;
        private EdgeImage edgeImage;

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

        private List<Finger> fingers;

        private Contour<Point> palmContour;
        private List<MCvConvexityDefect> convexityDefects;
        private Vector2D thumbDefectStart;
        private Vector2D thumbDefectEnd;
        private Vector2D thumbDefectDepth;
        
        private Kalman2DPositionFilter thumbDefectDepthFilter, thumbDefectStartFilter, thumbDefectEndFilter;

        private bool valid = false;
        private Vector2D topLeft;
        private Vector2D topRight;
        private Vector2D bottomLeft;
        private Vector2D bottomRight;

        public Quadrangle PalmQuad { get; private set; }

        public PalmDetector()
        {
            // TODO: determine which fingers are index or thumb-fingers and detect palm in thumb-hand
            thumbDefectDepthFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
            thumbDefectStartFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
            thumbDefectEndFilter = new Kalman2DPositionFilter(1.0e-2f, 1.0e-2f);
        }

        public void findPalmQuad(DepthImage depthImage, EdgeImage edgeImage, OutputImage outputImage, List<Finger> trackedFingers)
        {
            this.width = depthImage.Width;
            this.height = depthImage.Height;

            this.edgeImage = edgeImage;
            this.outputImage = outputImage;

            // dst = (src > (MaxDepth - MinDepth)) ? 0 : 1
            handImage = depthImage.Image.ThresholdBinaryInv(new Gray(depthImage.MaxDepth - depthImage.MinDepth - 1), new Gray(1)).Convert<Gray, Byte>();

            fingers = getFingersWithoutThumb(trackedFingers);
            buildPointingHandMask();
            handImage = handImage.And(pointingHandMask);

            findLongestPalmContour();
            if (palmContour != null)
            {
                findConvexityDefectsSortedByDepth();
                removeConvexityDefectsCausedByFingers();

                findHandPoints();

                if (valid)
                {
                    draw();
                }
            }
        }

        public void resetFilters()
        {
            thumbDefectDepthFilter.reset();
            thumbDefectStartFilter.reset();
            thumbDefectEndFilter.reset();
        }

        private List<Finger> getFingersWithoutThumb(List<Finger> detectedFingers)
        {
            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.Slices)
                {
                    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.LineSegment.Length > maxLength)
                {
                    maxLength = f.LineSegment.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).moveInBound(0,0,width-1,height-1);
            return pos;
        }

        public OutputImage i1, i2, i3, i4, i5, i6, i7, i8, i9;
        private void buildPointingHandMask()
        {
            /*i1 = new OutputImage(width, height);
            i2 = new OutputImage(width, height);
            i3 = new OutputImage(width, height);
            i4 = new OutputImage(width, height);
            i5 = new OutputImage(width, height);
            i6 = new OutputImage(width, height);
            i7 = new OutputImage(width, height);
            i8 = new OutputImage(width, height);
            i9 = new OutputImage(width, height);*/

            pointingHandMask = new Image<Gray, byte>(width, height, new Gray(0));

            // dst = (src > 0) ? 1 : 0;
            pointingHandMask = pointingHandMask.Or(edgeImage.Image.ThresholdBinary(new Gray(0), new Gray(1)));
            //i1.Image[0] = i1.Image[1] = i1.Image[2] = 255 * pointingHandMask;

            pointingHandMask = pointingHandMask.Dilate(4);
            //i2.Image[0] = i2.Image[1] = i2.Image[2] = 255 * pointingHandMask;
           
            fillFingerSlices(pointingHandMask, 1);
            //i3.Image[0] = i3.Image[1] = i3.Image[2] = 255 * pointingHandMask;

            //pointingHandMask = pointingHandMask.Dilate(1);
            //i4.Image[0] = i4.Image[1] = i4.Image[2] = 255 * pointingHandMask;

            MCvConnectedComp tmp = new MCvConnectedComp();
            // fill with value 2
            CvInvoke.cvFloodFill(pointingHandMask.Ptr, getPointInPointingHand(), new MCvScalar(2), new MCvScalar(0), new MCvScalar(0), out tmp, 0, IntPtr.Zero);

            // dst = (src > 1) ? 0 : 1    (src > 1 <-> src == 2 <-> src filled by flood fill)
            pointingHandMask = pointingHandMask.ThresholdBinaryInv(new Gray(1), new Gray(1));
            //i5.Image[0] = i5.Image[1] = i5.Image[2] = 255 * pointingHandMask;
            
            pointingHandMask = pointingHandMask.Erode(8);
            //i6.Image[0] = i6.Image[1] = i6.Image[2] = 255 * pointingHandMask;

            fillFingerSlices(pointingHandMask, 0);
            //i7.Image[0] = i7.Image[1] = i7.Image[2] = 255 * pointingHandMask;
            
            pointingHandMask = pointingHandMask.Erode(2);
            //i8.Image[0] = i8.Image[1] = i8.Image[2] = 255 * pointingHandMask;
            
            // only debug
            //i9.Image[0] = i9.Image[1] = i9.Image[2] = 255 * handImage.And(pointingHandMask);
        }

        private void findLongestPalmContour()
        {
            i1 = new OutputImage(width, height);
            i1.Image[0] = i1.Image[1] = i1.Image[2] = handImage * 255;

            i2 = new OutputImage(width, height);
            i2.Image[0] = i2.Image[1] = i2.Image[2] = pointingHandMask * 255;

            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 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)
            {
                float minFingerLineDist = float.MaxValue;
                foreach (Finger f in fingers)
                {
                    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)
                    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);

                if (angle <= Constants.PalmMaxThumbDefectAngle)
                    return d;
            }
            return null;
        }

        private void findHandPoints()
        {
            MCvConvexityDefect? thumbDefect = findThumbDefect();

            if (convexityDefects.Count > 0 && (thumbDefect != null || thumbDefectDepthFilter.Initialized))
            {
                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 if (thumbDefect == null)
                {
                    thumbDefectDepth = thumbDefectDepthFilter.getPrediction();
                    thumbDefectStart = thumbDefectStartFilter.getPrediction();
                    thumbDefectEnd = thumbDefectEndFilter.getPrediction();
                }
                else
                {
                    thumbDefectDepth = thumbDefectDepthFilter.getCorrectedPosition(thumbDefectDepth);
                    thumbDefectStart = thumbDefectStartFilter.getCorrectedPosition(thumbDefectStart);
                    thumbDefectEnd = thumbDefectEndFilter.getCorrectedPosition(thumbDefectEnd);
                }

                Vector2D handLength, handWidth;
                if (thumbDefectDepth.getDistanceTo(thumbDefectStart) > thumbDefectDepth.getDistanceTo(thumbDefectEnd))
                {
                    //right hand
                    handLength = thumbDefectStart - thumbDefectDepth;
                    handWidth = 0.8f * new Vector2D(-handLength.Y, handLength.X);
                    topLeft = thumbDefectStart;
                    bottomLeft = thumbDefectDepth - 0.4f * handLength;
                    bottomRight = bottomLeft + handWidth;
                    topRight = bottomRight + 1.2f * handLength - 0.3f * handWidth;
                }
                else
                {
                    //left hand
                    handLength = thumbDefectEnd - thumbDefectDepth;
                    handWidth = 0.8f * new Vector2D(handLength.Y, -handLength.X);
                    topRight = thumbDefectEnd;
                    bottomRight = thumbDefectDepth - 0.4f * handLength;
                    bottomLeft = bottomRight + handWidth;
                    topLeft = bottomLeft + 1.2f * handLength - 0.3f * handWidth;
                }

                PalmQuad = new Quadrangle(bottomLeft, topLeft, topRight, bottomRight);

                valid = true;
            }
            else
            {
                PalmQuad = null;
                valid = false;
            }
        }

        private void draw()
        {
            outputImage.drawContour(palmContour, Constants.PalmConturColor);
            outputImage.drawPoints(palmContour.GetConvexHull(Emgu.CV.CvEnum.ORIENTATION.CV_CLOCKWISE), Constants.PalmConvexHullColor);

            if (PalmQuad != null)
            {
                outputImage.fillCircle(thumbDefectDepth.IntX, thumbDefectDepth.IntY, 3, Color.Red);
                outputImage.fillCircle(thumbDefectStart.IntX, thumbDefectStart.IntY, 3, Color.Green);
                outputImage.fillCircle(thumbDefectEnd.IntX, thumbDefectEnd.IntY, 3, Color.Blue);

                //outputImage.drawLineSegment(new Utility.LineSegment2D(thumbDefectDepth, (thumbDefectStart + thumbDefectEnd) / 2.0f), Constants.PalmThumbDefectColor, 1);
                
                Vector2D[] vertices = PalmQuad.Vertices;
                for (int i = 0; i < 4; ++i)
                    outputImage.drawLineSegment(new bbiwarg.Utility.LineSegment2D(vertices[i], vertices[(i + 1) % 4]), Constants.PalmQuadColor);

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

        private void drawGrid(Vector2D a, Vector2D b, Vector2D c, Vector2D d)
        {
            int numRows = 4;
            int numColumns = 3;

            Vector2D relAB = (b - a) / numRows;
            Vector2D relDC = (c - d) / numRows;
            Vector2D relBC = (c - b) / numColumns;
            Vector2D relAD = (d - a) / numColumns;

            for (int i = 1; i < numRows; i++)
            {
                outputImage.drawLineSegment(new bbiwarg.Utility.LineSegment2D(a + i * relAB, d + i * relDC), Constants.PalmGridColor);
            }

            for (int i = 1; i < numColumns; i++)
            {
                outputImage.drawLineSegment(new bbiwarg.Utility.LineSegment2D(a + i * relAD, b + i * relBC), Constants.PalmGridColor);
            }
        }
    }
}