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

namespace BBIWARG.Recognition.PalmRecognition
{
    /// <summary>
    /// Represents a palm that is tracked for several frames.
    /// </summary>
    internal class TrackedPalm : TrackedObject<Palm>
    {
        /// <summary>
        /// the kalman filter for the fingers lower prediction
        /// </summary>
        private Kalman2DPositionFilter fingersLowerKalman;

        /// <summary>
        /// the kalman filter for the fingers upper prediction
        /// </summary>
        private Kalman2DPositionFilter fingersUpperKalman;

        /// <summary>
        /// the kalman filter for the wrist lower prediction
        /// </summary>
        private Kalman2DPositionFilter wristLowerKalman;

        /// <summary>
        /// the kalman filter for the wrist upper prediction
        /// </summary>
        private Kalman2DPositionFilter wristUpperKalman;

        /// <summary>
        /// the predicted position of the fingers lower
        /// </summary>
        public Vector2D FingersLowerPrediction { get { return fingersLowerKalman.getPrediction(); } }

        /// <summary>
        /// the predicted position of the fingers upper
        /// </summary>
        public Vector2D FingersUpperPrediction { get { return fingersUpperKalman.getPrediction(); } }

        /// <summary>
        /// the optimized palm (using predicted palm points)
        /// </summary>
        public Palm OptimizedPalm { get; private set; }

        /// <summary>
        /// the predicted position of the wrist lower
        /// </summary>
        public Vector2D WristLowerPrediction { get { return wristLowerKalman.getPrediction(); } }

        /// <summary>
        /// the predicted position of the wrist upper
        /// </summary>
        public Vector2D WristUpperPrediction { get { return wristUpperKalman.getPrediction(); } }

        /// <summary>
        /// Initializes a new instance of the TrackedPalm class.
        /// </summary>
        /// <param name="id">The track ID.</param>
        /// <param name="detectedPalm">The detected palm.</param>
        /// <param name="numFramesDetectedUntilTracked">The number of consecutive frames detected until the palm is considered tracked.</param>
        /// <param name="numFramesLostUntilDeleted">The number of consecutive frames lost until the palm is deleted.</param>
        public TrackedPalm(int id, Palm detectedPalm, int numFramesDetectedUntilTracked, int numFramesLostUntilDeleted)
            : base(id, detectedPalm, numFramesDetectedUntilTracked, numFramesLostUntilDeleted)
        {
            wristUpperKalman = new Kalman2DPositionFilter(Parameters.PalmmXX, Parameters.PalmmXY, Parameters.PalmmYY);
            wristLowerKalman = new Kalman2DPositionFilter(Parameters.PalmmXX, Parameters.PalmmXY, Parameters.PalmmYY);
            fingersUpperKalman = new Kalman2DPositionFilter(Parameters.PalmmXX, Parameters.PalmmXY, Parameters.PalmmYY);
            fingersLowerKalman = new Kalman2DPositionFilter(Parameters.PalmmXX, Parameters.PalmmXY, Parameters.PalmmYY);
            wristUpperKalman.setInitialPosition(detectedPalm.WristUpper);
            wristLowerKalman.setInitialPosition(detectedPalm.WristLower);
            fingersUpperKalman.setInitialPosition(detectedPalm.FingersUpper);
            fingersLowerKalman.setInitialPosition(detectedPalm.FingersLower);

            updateOptimizedPalm(detectedPalm);
            logStateChange();
        }

        /// <summary>
        /// Updates the tracked palm with the given palm, logs the state change, updates the palm point predictions and the optimized palm.
        /// </summary>
        /// <param name="detectedPalm">the detected palm</param>
        public override void updateFrame(Palm detectedPalm)
        {
            base.updateFrame(detectedPalm);

            if (NumFramesInCurrentState == 1)
                logStateChange();

            if (detectedPalm != null)
            {
                wristUpperKalman.getCorrectedPosition(detectedPalm.WristUpper);
                wristLowerKalman.getCorrectedPosition(detectedPalm.WristLower);
                fingersUpperKalman.getCorrectedPosition(detectedPalm.FingersUpper);
                fingersLowerKalman.getCorrectedPosition(detectedPalm.FingersLower);

                updateOptimizedPalm(detectedPalm);
            }
        }

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

        /// <summary>
        /// Updates the optimized palm by creating a new palm with the predicted palm points
        /// </summary>
        /// <param name="detectedPalm">the detected palm</param>
        private void updateOptimizedPalm(Palm detectedPalm)
        {
            OptimizedPalm = new Palm(detectedPalm.Hand, detectedPalm.ThumbDefect, detectedPalm.HandSide, WristUpperPrediction, FingersUpperPrediction, FingersLowerPrediction, WristLowerPrediction);
            OptimizedPalm.setTracked(ID);
        }
    }
}