using BBIWARG.Recognition.Tracking;
using BBIWARG.Utility;
using System;
using System.Collections.Generic;

namespace BBIWARG.Recognition.FingerRecognition
{
    /// <summary>
    /// Represents a finger that is tracked for several frames.
    /// </summary>
    internal class TrackedFinger : TrackedObject<Finger>
    {
        /// <summary>
        /// the kalman filter for the hand point prediction
        /// </summary>
        private Kalman2DPositionFilter handPointKalman;

        /// <summary>
        /// list of last directions (used to correct finger directions if they differ from average)
        /// </summary>
        private List<Vector2D> lastRawDirections;

        /// <summary>
        /// the kalman filter for the tip point prediction
        /// </summary>
        private Kalman2DPositionFilter tipPointKalman;

        /// <summary>
        /// predicted position of the hand point
        /// </summary>
        public Vector2D HandPointPrediction { get { return handPointKalman.getPrediction(); } }

        /// <summary>
        /// the average direction of the last fingers
        /// </summary>
        public Vector2D MeanDirection { get { return Vector2D.mean(lastRawDirections); } }

        /// <summary>
        /// predicted position of the tip point
        /// </summary>
        public Vector2D TipPointPrediction { get { return tipPointKalman.getPrediction(); } }

        /// <summary>
        /// Initializes a new instance of the TrackedFinger class.
        /// </summary>
        /// <param name="id">The track ID.</param>
        /// <param name="detectedFinger">The initial detected finger.</param>
        /// <param name="numFramesDetectedUntilTracked">The number of consecutive frames detected until the finger is considered to be tracked.</param>
        /// <param name="numFramesLostUntilDeleted">The number of consecutive frames lost until the finger is considered to be deleted.</param>
        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();
        }

        /// <summary>
        /// Indicates whether a newly detected finger should be reverse (direction differs from average direction of previous fingers).
        /// </summary>
        /// <param name="detectedFinger">the detected finger</param>
        /// <returns>whether the fingers direction matches to the direction of the previous fingers</returns>
        public bool shouldFingerBeReversed(Finger detectedFinger)
        {
            Vector2D meanDirection = Vector2D.mean(lastRawDirections);
            return meanDirection.isInOppositeDirection(detectedFinger.Direction);
        }

        /// <summary>
        /// Updates the tracked finger with the given finger, logs the state change, corrects the finger's direction if needed and updates the position predictions (kalman filters).
        /// </summary>
        /// <param name="detectedFinger">The detected finger.</param>
        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);
            }
        }

        /// <summary>
        /// logs the state change
        /// </summary>
        private void logStateChange()
        {
            String stateAsString = CurrentState.ToString().ToLower();
            Logger.log(String.Format("Finger #{0} {1}", this.ID, stateAsString), LogSubject.FingerTracker);
        }
    }
}