using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using bbiwarg.Recognition.Tracking;
using bbiwarg.Utility;

namespace bbiwarg.Recognition.FingerRecognition
{
    class TrackedFinger : TrackedObject<Finger>
    {
        private Kalman2DPositionFilter tipPointKalman;
        private Kalman2DPositionFilter handPointKalman;
        private List<Vector2D> lastRawDirections;

        public Vector2D TipPointPrediction { get { return tipPointKalman.getPrediction(); } }
        public Vector2D HandPointPrediction { get { return handPointKalman.getPrediction(); } }
        public Vector2D MeanDirection { get { return Vector2D.mean(lastRawDirections); } }

        public TrackedFinger(int id, Finger detectedFinger, int numFramesDetectedUntilTracked, int numFramesLostUntilDeleted)
            : base(id, detectedFinger, numFramesDetectedUntilTracked, numFramesLostUntilDeleted)
        {
            tipPointKalman = new Kalman2DPositionFilter(Parameters.FingermXX, Parameters.FingermXY, Parameters.FingermYY);
            tipPointKalman.setInitialPosition(detectedFinger.TipPoint);
            handPointKalman = new Kalman2DPositionFilter(Parameters.FingermXX, Parameters.FingermXY, Parameters.FingermYY);
            handPointKalman.setInitialPosition(detectedFinger.HandPoint);
            lastRawDirections = new List<Vector2D>();

            logStateChange();
        }

        public override void updateFrame(Finger detectedFinger)
        {
            base.updateFrame(detectedFinger);

            if (NumFramesInCurrentState == 1)
                logStateChange();

            if (detectedFinger != null)
            {
                Vector2D rawDirection = detectedFinger.Direction;
                if (shouldFingerBeReversed(detectedFinger))
                    detectedFinger.reverse();

                tipPointKalman.getCorrectedPosition(detectedFinger.TipPoint);
                handPointKalman.getCorrectedPosition(detectedFinger.HandPoint);
                lastRawDirections.Add(rawDirection);
                if (lastRawDirections.Count == Parameters.FingerTrackerNumDirectionsForMeanDirection)
                    lastRawDirections.RemoveAt(0);
            }
        }

        public override float calculateSimilarity(Finger detectedFinger)
        {
            Vector2D tip, hand;
            if (shouldFingerBeReversed(detectedFinger))
            {
                tip = detectedFinger.HandPoint;
                hand = detectedFinger.TipPoint;
            }
            else
            {
                tip = detectedFinger.TipPoint;
                hand = detectedFinger.HandPoint;
            }

            float tipPointSimilarity = getPositionSimilarity(TipPointPrediction, tip, Parameters.FingerTrackerMaxTipPointMove);
            float handPointSimilarity = getPositionSimilarity(HandPointPrediction, hand, Parameters.FingerTrackerMaxHandPointMove);

            return tipPointSimilarity * handPointSimilarity;
        }

        private bool shouldFingerBeReversed(Finger detectedFinger)
        {
            Vector2D meanDirection = Vector2D.mean(lastRawDirections);
            return meanDirection.isInOppositeDirection(detectedFinger.Direction);
        }

        private void logStateChange()
        {
            String stateAsString = CurrentState.ToString().ToLower();
            Logger.log(String.Format("Finger #{0} {1}", this.ID, stateAsString), LogSubject.FingerTracker);
        }
    }
}