using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using bbiwarg.Images;
using bbiwarg.Utility;
using Emgu.CV.Structure;
using Emgu.CV;

namespace bbiwarg.Detectors.Fingers
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImage;
        private FingerImage fingerImage;
        private List<FingerSliceTrail> horizontalFingerSliceTrails;
        private List<FingerSliceTrail> verticalFingerSliceTrails;
        private List<Finger> fingers;
        public List<Finger> Fingers { get { return fingers; } private set { fingers = value; } }


        public FingerDetector(DepthImage depthImage, EdgeImage edgeImage, FingerImage fingerImage)
        {
            this.depthImage = depthImage;
            this.edgeImage = edgeImage;
            this.fingerImage = fingerImage;

            findHorizontalFingerSliceTrails();
            findVerticalFingerSliceTrails();
            transformFingerSliceTrailsToFingers();
        }

        private void findHorizontalFingerSliceTrails()
        {

            horizontalFingerSliceTrails = new List<FingerSliceTrail>();

            int width = depthImage.getWidth();
            int height = depthImage.getHeight();

            int minFingerSize = 10;
            int maxFingerSize = 30;

            //search horizontal finger-slices
            for (int y = 0; y < height; y++)
            {
                int x = 0;
                while (x <= width - minFingerSize)
                {
                    if (edgeImage.isEdgeAt(x, y) && depthImage.getDepthAt(x, y) > depthImage.getDepthAt(x + 1, y))
                    {
                        int sliceX1 = x;
                        int maxSliceX2 = Math.Min(sliceX1 + maxFingerSize, width - 1);
                        x++;
                        while (x <= maxSliceX2)
                        {
                            if (edgeImage.isEdgeAt(x, y))
                            {
                                int sliceX2 = x;
                                Vector2D sliceStart = new Vector2D(Math.Max(sliceX1 - 1, 0), y);
                                Vector2D sliceEnd = new Vector2D(Math.Min(sliceX2 + 1, width - 1), y);
                                FingerSlice fingerSlice = new FingerSlice(sliceStart, sliceEnd);
                                if (fingerSlice.Size > minFingerSize && fingerSliceDepthTest(fingerSlice))
                                {
                                    addHorizontalFingerSlice(fingerSlice);
                                    fingerImage.drawLine(fingerSlice.Line, FingerImageState.possibleFingerSlice);
                                }
                                break;
                            }
                            else x++;
                        }
                    }
                    else
                        x++;

                }
            }
        }

        private void findVerticalFingerSliceTrails()
        {
            verticalFingerSliceTrails = new List<FingerSliceTrail>();

            int width = depthImage.getWidth();
            int height = depthImage.getHeight();

            int minFingerSize = 10;
            int maxFingerSize = 30;

            //search vertical finger-slices
            for (int x = 0; x < width; x++)
            {
                int y = 0;
                while (y <= height - minFingerSize)
                {
                    if (edgeImage.isEdgeAt(x, y) && depthImage.getDepthAt(x, y) > depthImage.getDepthAt(x, y + 1))
                    {
                        int sliceY1 = y;
                        int maxSliceY2 = Math.Min(sliceY1 + maxFingerSize, height - 1);
                        y++;
                        while (y <= maxSliceY2)
                        {
                            if (edgeImage.isEdgeAt(x, y))
                            {
                                int sliceY2 = y;
                                Vector2D sliceStart = new Vector2D(x, Math.Max(sliceY1 - 1, 0));
                                Vector2D sliceEnd = new Vector2D(x, Math.Min(sliceY2 + 1, height - 1));
                                FingerSlice fingerSlice = new FingerSlice(sliceStart, sliceEnd);
                                if (fingerSlice.Size > minFingerSize && fingerSliceDepthTest(fingerSlice))
                                {
                                    addVerticalFingerSlice(fingerSlice);
                                    fingerImage.drawLine(fingerSlice.Line, FingerImageState.possibleFingerSlice);
                                }
                                break;
                            }
                            else y++;
                        }
                    }
                    else
                        y++;

                }
            }
        }

        private void transformFingerSliceTrailsToFingers()
        {
            int minNumSlices = 15;

            Fingers = new List<Finger>();
            List<FingerSliceTrail> fingerSliceTrails = new List<FingerSliceTrail>();
            fingerSliceTrails.AddRange(horizontalFingerSliceTrails);
            fingerSliceTrails.AddRange(verticalFingerSliceTrails);

            foreach (FingerSliceTrail trail in fingerSliceTrails)
            {
                if (trail.NumSlices >= minNumSlices)
                {
                    //reorder finger slice so that it goes tip->hand
                    /*Int16 depthAtTip = depthImage.getDepthAt((int)trail.Start.Mid.X, (int)trail.Start.Mid.Y);
                    Int16 depthAtHand = depthImage.getDepthAt((int)trail.End.Mid.X, (int)trail.End.Mid.Y);
                    if (depthAtTip < depthAtHand)
                        trail.reverse();*/
                    
                    Finger finger = new Finger(trail);

                    //filter double hits (horizontal+vertical finger)
                    bool addFinger = true;
                    for (int i = fingers.Count - 1; i >= 0; i--)
                    {
                        Finger f = fingers[i];
                        Utility.LineSegment2D lineA = finger.LineSegment;
                        Utility.LineSegment2D lineB = f.LineSegment;

                        if(lineA.getVerticalDistanceTo(lineB) < 5 && lineA.getParallelDistanceTo(lineB) < 5)
                        {
                            if (finger.SliceTrail.NumSlices > f.SliceTrail.NumSlices)
                                fingers.RemoveAt(i);
                            else
                                addFinger = false;
                        }
                    }

                    if (addFinger)
                    {
                        fingers.Add(finger);
                        fingerImage.drawFinger(finger, FingerImageState.fingerDetected);
                    }

                }

            }

        }

        private bool fingerSliceDepthTest(FingerSlice fingerSlice)
        {
            Int16 depthStart = depthImage.getDepthAt(fingerSlice.Start.IntX, fingerSlice.Start.IntY);
            Int16 depthMid = depthImage.getDepthAt(fingerSlice.Mid.IntX, fingerSlice.Mid.IntY);
            Int16 depthEnd = depthImage.getDepthAt(fingerSlice.End.IntX, fingerSlice.End.IntY);
            return (depthStart > depthMid && depthMid < depthEnd);
        }

        private void addHorizontalFingerSlice(FingerSlice slice)
        {
            int maxYGap = 5;
            int maxXDifference = 5;

            bool assigned = false;
            foreach (FingerSliceTrail trail in horizontalFingerSliceTrails)
            {
                FingerSlice trailEnd = trail.End;
                if (slice.Mid.Y - trailEnd.Mid.Y <= maxYGap && Math.Abs(trailEnd.Mid.X - slice.Mid.X) <= maxXDifference)
                {
                    trail.addSlice(slice);
                    assigned = true;
                }
            }

            if (!assigned)
                horizontalFingerSliceTrails.Add(new FingerSliceTrail(slice));
        }

        private void addVerticalFingerSlice(FingerSlice slice)
        {
            int maxXGap = 5;
            int maxYDifference = 5;

            bool assigned = false;
            foreach (FingerSliceTrail trail in verticalFingerSliceTrails)
            {
                FingerSlice trailEnd = trail.End;
                if (slice.Mid.X - trailEnd.Mid.X <= maxXGap && Math.Abs(trailEnd.Mid.Y - slice.Mid.Y) <= maxYDifference)
                {
                    trail.addSlice(slice);
                    assigned = true;
                }
            }

            if (!assigned)
                verticalFingerSliceTrails.Add(new FingerSliceTrail(slice));
        }
    }
}