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

namespace bbiwarg.Detectors.Fingers
{
    class Finger
    {
        private List<Vector<int>> fingerPoints;
        private bool lineUpToDate = false;
        private Vector<float> direction;
        private Vector<float> pointOnLine;
        private Vector<float> lineEndPoint1;
        private Vector<float> lineEndPoint2;
        private Vector<int> tipPoint;
        private Vector<int> handPoint;
        private float length;
        
        public Finger(Vector<int> fingerPoint)
        {
            fingerPoints = new List<Vector<int>>();
            addFingerPoint(fingerPoint);
        }

        public Vector<float> getLineEndPoint1()
        {
            if (!lineUpToDate) updateLine();
            return lineEndPoint1;
        }

        public Vector<float> getLineEndPoint2()
        {
            if (!lineUpToDate) updateLine();
            return lineEndPoint2;
        }

        public Vector<float> getDirection()
        {
            if (!lineUpToDate) updateLine();
            return direction;
        }

        public Vector<int> getTipPoint()
        {
            if (!lineUpToDate) updateLine();
            return tipPoint;
        }

        public float getLength()
        {
            if (!lineUpToDate) updateLine();
            return length;
        }

        public void addFingerPoint(Vector<int> fingerPoint)
        {
            fingerPoints.Add(fingerPoint);
            lineUpToDate = false;
        }

        public float getMinDistance(Vector<int> fingerPoint)
        {
            float minDinstance = float.MaxValue;
            foreach (Vector<int> fp in fingerPoints)
            {
                float distance = fingerPoint.subDistance(fp, 2);
                if (distance < minDinstance)
                {
                    minDinstance = distance;
                }
            }

            return minDinstance;
        }

        public float getSimilarity(Finger compareFinger) {
            //startDistance
            float maxStartDistance = 100;
            float xDiffStart = lineEndPoint1.x - compareFinger.getLineEndPoint1().x;
            float yDiffStart = lineEndPoint1.y - compareFinger.getLineEndPoint1().y;
            float startDistance = (float)Math.Sqrt(xDiffStart * xDiffStart + yDiffStart * yDiffStart);
            float startSimilarity = Math.Max(1 - (startDistance / maxStartDistance), 0);

            //endDistance
            float maxEndDistance = 100;
            float xDiffEnd = lineEndPoint2.x - compareFinger.getLineEndPoint2().x;
            float yDiffEnd = lineEndPoint2.y - compareFinger.getLineEndPoint2().y;
            float endDistance = (float)Math.Sqrt(xDiffEnd * xDiffEnd + yDiffEnd * yDiffEnd);
            float endSimilarity = Math.Max(1 - (endDistance / maxEndDistance), 0);

            //direction
            float scalaProduct = direction * compareFinger.getDirection();
            float lengthDirection = direction.norm();
            float lengthCompareDirection = compareFinger.getDirection().norm();
            float directionSimilarity = Math.Abs(scalaProduct / (lengthDirection * lengthCompareDirection));

            //Console.WriteLine(Math.Round(directionSimilarity, 2) + "###" + Math.Round(startSimilarity, 2) + "###" + Math.Round(endSimilarity, 2));

            return (startSimilarity + endSimilarity + directionSimilarity) / 3;
            
        }

        private void updateLine() {
            //update direction+pointonline
            PointF[] pointArray = new PointF[fingerPoints.Count];
            int i = 0;
            foreach (Vector<int> fp in fingerPoints)
            {
                pointArray[i] = new PointF(fp.x, fp.y);
                ++i;
            }
            PointF tempDirection;
            PointF tempPointOnLine;
            PointCollection.Line2DFitting(pointArray, Emgu.CV.CvEnum.DIST_TYPE.CV_DIST_L2, out tempDirection, out tempPointOnLine);
            direction = new Vector<float>(new float[2]{tempDirection.X, tempDirection.Y});
            pointOnLine = new Vector<float>(new float[2] { tempPointOnLine.X, tempPointOnLine.Y });

            Vector<int> fp1 = fingerPoints[0];
            Vector<int> fp2 = fingerPoints[0];
            length = 0.0f;
            foreach (Vector<int> fp in fingerPoints) {
                float distanceToFP1 = fp.subDistance(fp1,2);
                float distanceToFP2 = fp.subDistance(fp2,2);
                if (length < distanceToFP1 && distanceToFP1 >= distanceToFP2) 
                {
                    fp2 = fp;
                    length = distanceToFP1;
                }
                else if (length < distanceToFP2 && distanceToFP2 > distanceToFP1)
                {
                    fp1 = fp;
                    length = distanceToFP2;
                }
            }

            if (fp1.y < fp2.y)
            {
                tipPoint = fp1;
                handPoint = fp2;
            }
            else 
            {
                tipPoint = fp2;
                handPoint = fp1;
            }

            //update start+end
            lineEndPoint1 = projectToLine(tipPoint);
            lineEndPoint2 = projectToLine(handPoint);

            lineUpToDate = true;
        }

        private Vector<float> projectToLine(Vector<int> p)
        {
            float px = p.x, py = p.y, dx = direction.x, dy = direction.y, ox = pointOnLine.x, oy = pointOnLine.y;
            float diffx = px - ox;
            float diffy = py - oy;

            float diff_d = (diffx * dx + diffy * dy);
            float d_d = (dx * dx + dy * dy);
            float q = diff_d / d_d;

            float newX = ox + q * dx;
            float newY = oy + q * dy;

            return new Vector<float>(new float[2]{newX, newY});
        }
    }
}