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

namespace bbiwarg.Recognition.PalmRecognition
{
    class TrackedPalm : TrackedObject<Palm>
    {
        private Kalman2DPositionFilter wristUpperKalman;
        private Kalman2DPositionFilter wristLowerKalman;
        private Kalman2DPositionFilter fingersUpperKalman;
        private Kalman2DPositionFilter fingersLowerKalman;

        public Vector2D WristUpperPrediction { get { return wristUpperKalman.getPrediction(); } }
        public Vector2D WristLowerPrediction { get { return wristLowerKalman.getPrediction(); } }
        public Vector2D FingersUpperPrediction { get { return fingersUpperKalman.getPrediction(); } }
        public Vector2D FingersLowerPrediction { get { return fingersLowerKalman.getPrediction(); } }
        public Palm OptimizedPalm { get; private set; }

        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();
        }

        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);
            }
        }

        private void updateOptimizedPalm(Palm detectedPalm)
        {
            OptimizedPalm = new Palm(detectedPalm.Hand, detectedPalm.ThumbDefect, detectedPalm.HandSide, WristUpperPrediction, FingersUpperPrediction, FingersLowerPrediction, WristLowerPrediction);
        }

        public override float calculateSimilarity(Palm detectedPalm)
        {
            float handSimilarity = (detectedPalm.Hand.TrackID == LastObject.Hand.TrackID) ? 1 : 0;
            float wristUpperSimilarity = getPositionSimilarity(WristUpperPrediction, detectedPalm.WristUpper, Parameters.PalmTrackerMaxWristUpperMove);
            float wristLowerSimilarity = getPositionSimilarity(WristLowerPrediction, detectedPalm.WristLower, Parameters.PalmTrackerMaxWristLowerMove);
            float fingersUpperSimilarity = getPositionSimilarity(FingersUpperPrediction, detectedPalm.FingersUpper, Parameters.PalmTrackerMaxFingersUpperMove);
            float fingersLowerSimilarity = getPositionSimilarity(FingersLowerPrediction, detectedPalm.FingersLower, Parameters.PalmTrackerMaxFingersLowerMove);

            float similarity = handSimilarity * wristUpperSimilarity * wristLowerSimilarity * fingersUpperSimilarity * fingersLowerSimilarity;

            return similarity;
        }

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

    }
}