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

namespace bbiwarg.Recognition.FingerRecognition
{
    class FingerDetector
    {
        private DepthImage depthImage;
        private EdgeImage edgeImageOriginal;
        private EdgeImage edgeImageAdapted;
        public List<Finger> Fingers { get; private set; }

        public FingerDetector(DepthImage depthImage, EdgeImage edgeImage)
        {
            this.depthImage = depthImage;
            this.edgeImageOriginal = edgeImage;
            this.edgeImageAdapted = edgeImage.copy();

            detectFingers();
        }

        private void detectFingers()
        {
            Vector2D maxPixel = Parameters.ImageMaxPixel;
            int maxX = maxPixel.IntX;
            int maxY = maxPixel.IntY;

            Fingers = new List<Finger>();

            for (int y = 1; y < maxY; y += 5)
            {
                for (int x = 1; x < maxX; x++)
                {
                    if (edgeImageAdapted.isEdgeAt(x, y))
                    {
                        Vector2D edgePoint = new Vector2D(x, y);
                        Vector2D edgeDirection = getEdgeDirection(edgePoint);
                        if (edgeDirection != null)
                        {
                            Vector2D dir = edgeDirection.getOrthogonal();
                            if (depthImage.getDepthAt(edgePoint - dir) < depthImage.getDepthAt(edgePoint + dir))
                                dir = dir.getInverse();

                            FingerSlice slice = findFingerSliceFromStartEdge(edgePoint, dir);
                            if (slice != null)
                            {
                                FingerSliceTrail trail = findFingerSliceTrail(slice, edgeDirection);
                                if (trail != null && trail.NumSlices > Parameters.FingerMinNumSlices)
                                {
                                    createFingerFromTrail(trail);
                                }
                            }
                        }
                    }
                }
            }
        }

        private Vector2D getEdgeDirection(Vector2D edgePoint)
        {
            int x = edgePoint.IntX;
            int y = edgePoint.IntY;

            if (edgeImageAdapted.isEdgeAt(x, y - 1) && edgeImageAdapted.isEdgeAt(x, y + 1)) return new Vector2D(0, 1);
            else if (edgeImageAdapted.isEdgeAt(x - 1, y) && edgeImageAdapted.isEdgeAt(x + 1, y)) return new Vector2D(1, 0);
            else if (edgeImageAdapted.isEdgeAt(x - 1, y - 1) && edgeImageAdapted.isEdgeAt(x + 1, y + 1)) return new Vector2D(1, 1).normalize();
            else if (edgeImageAdapted.isEdgeAt(x + 1, y - 1) && edgeImageAdapted.isEdgeAt(x - 1, y + 1)) return new Vector2D(1, -1).normalize();
            else return null;
        }

        private FingerSliceTrail findFingerSliceTrail(FingerSlice startSlice, Vector2D startDirection)
        {
            FingerSliceTrail trail = new FingerSliceTrail(startSlice);

            Vector2D direction = startDirection;
            Vector2D position = startSlice.Mid + Parameters.FingerStepSize * direction;

            if (position.isInBound())
            {
                FingerSlice nextSlice = findFingerSliceFromMid(position, direction);
                if (nextSlice != null)
                {
                    trail.addSlice(nextSlice);
                    trail = expandTrail(trail);

                    if (trail.NumSlices > Parameters.FingerMinNumSlices / 2)
                    {
                        trail.removeFirstSlices(Parameters.FingerRemoveNumSlicesForCorrection);
                        trail.reverse();
                        trail = expandTrail(trail, true);
                        trail.reverse();
                        return trail;
                    }
                }
            }

            return null;

        }

        private FingerSliceTrail expandTrail(FingerSliceTrail trail, bool reversed = false)
        {
            Vector2D currentDirection = trail.getEndDirection();
            Vector2D currentPosition = trail.EndSlice.Mid + Parameters.FingerStepSize * currentDirection;

            int gapCounter = 0;
            int numSlices = trail.NumSlices;

            FingerSlice lastSlice = trail.EndSlice;
            FingerSlice nextSlice;

            while (currentPosition.isInBound() && gapCounter <= Math.Min(numSlices, Parameters.FingerMaxGapCounter))
            {
                Vector2D direction = (reversed) ? currentDirection.getInverse() : currentDirection;
                nextSlice = findFingerSliceFromMid(currentPosition, direction);

                if (nextSlice != null && Math.Abs(nextSlice.Length - lastSlice.Length) <= Parameters.FingerMaxSliceDifferencePerStep)
                {
                    gapCounter = 0;
                    numSlices++;
                    trail.addSlice(nextSlice);
                    currentDirection = trail.getEndDirection();
                    currentPosition = nextSlice.Mid + Parameters.FingerStepSize * currentDirection;

                    lastSlice = nextSlice;
                }
                else
                {
                    gapCounter++;
                    currentPosition += currentDirection;
                }
            }

            return trail;
        }

        private FingerSlice findFingerSliceFromMid(Vector2D position, Vector2D direction)
        {
            if (edgeImageAdapted.isRoughEdgeAt(position)) return null;

            Vector2D dirStart = direction.getOrthogonal(true);
            Vector2D dirEnd = direction.getOrthogonal(false);

            Vector2D start = edgeImageAdapted.findNextEdge(position, dirStart, Parameters.FingerMaxWidth);
            if (start == null) return null;

            Vector2D end = edgeImageAdapted.findNextEdge(position, dirEnd, Parameters.FingerMaxWidth);
            if (end == null) return null;

            return getFingerSlice(start, end);
        }
        private FingerSlice findFingerSliceFromStartEdge(Vector2D start, Vector2D direction)
        {
            Vector2D searchStart = start + Parameters.FingerContourMargin * direction;
            Vector2D end = edgeImageAdapted.findNextEdge(searchStart, direction, Parameters.FingerMaxWidth);
            if (end == null) 
                return null;

            return getFingerSlice(start, end);
        }

        private FingerSlice getFingerSlice(Vector2D start, Vector2D end)
        {
            FingerSlice slice = new FingerSlice(start, end);
            if (slice.Length >= Parameters.FingerMinWidth && slice.Length <= Parameters.FingerMaxWidth && fingerSliceDepthTest(slice))
                return slice;

            return null;
        }

        private bool fingerSliceDepthTest(FingerSlice fingerSlice)
        {
            Int16 depthStart = depthImage.getDepthAt(fingerSlice.ContourStart);
            Int16 depthMid = depthImage.getDepthAt(fingerSlice.Mid);
            Int16 depthEnd = depthImage.getDepthAt(fingerSlice.ContourEnd);
            return (depthStart > depthMid && depthMid < depthEnd);
        }

        private void createFingerFromTrail(FingerSliceTrail trail)
        {
            //bring finger in correct direction Tip->Hand
            trail = orderTrailTipToHand(trail);

            //create finger
            Finger finger = new Finger(trail);

            //add finger
            if (!isCrippleFinger(finger))
                Fingers.Add(finger);

            //remove edges around detected finger to improve performance
            edgeImageAdapted.removeEdgesInsidePolygon(finger.Contour.ToArray());
        }

        private bool isCrippleFinger(Finger finger)
        {
            Int16 depthAtFinger = depthImage.getDepthAt(finger.MidPoint);
            Int16 depthAtOut1 = depthImage.getDepthAt(finger.SliceTrail.MidSlice.OutStart);
            Int16 depthAtOut2 = depthImage.getDepthAt(finger.SliceTrail.MidSlice.OutEnd);
            int minDepthDifference = Math.Min(Math.Abs(depthAtFinger - depthAtOut1), Math.Abs(depthAtFinger - depthAtOut2));

            return (minDepthDifference < Parameters.FingerCrippleMinDifference);
        }


        private FingerSliceTrail orderTrailTipToHand(FingerSliceTrail trail)
        {
            int numSlicesForDirectionDetection = Parameters.FingerNumSlicesForDirectionDetection;
            int maxIndex = trail.NumSlices - 1;
            float sumStart = 0;
            float sumEnd = 0;

            for (int i = 0; i < numSlicesForDirectionDetection; i++)
            {
                sumStart += trail[i].Length;
                sumEnd += trail[maxIndex - i].Length;
            }


            float avergaeStart = sumStart / numSlicesForDirectionDetection;
            float averageEnd = sumEnd / numSlicesForDirectionDetection;

            //check direction
            if (avergaeStart > averageEnd)
                trail.reverse();

            return trail;
        }
    }
}