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

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

        public override float calculateSimilarity(Palm detectedPalm)
        {
            //hand
            float handSimilarity = (detectedPalm.Hand.TrackID == LastObject.Hand.TrackID) ? 1 : 0;

            //wristUpper
            float wristUpperDistance = WristUpperPrediction.getDistanceTo(detectedPalm.WristUpper);
            float wristUpperSimilarity = Math.Max(0, 1 - wristUpperDistance / Parameters.ImageDiagonalLength);

            //wristLower
            float wristLowerDistance = WristLowerPrediction.getDistanceTo(detectedPalm.WristLower);
            float wristLowerSimilarity = Math.Max(0, 1 - wristLowerDistance / Parameters.ImageDiagonalLength);

            //fingersUpper
            float fingersUpperDistance = FingersUpperPrediction.getDistanceTo(detectedPalm.FingersUpper);
            float fingersUpperSimilarity = Math.Max(0, 1 - fingersUpperDistance / Parameters.ImageDiagonalLength);

            //fingersLower
            float fingersLowerDistance = FingersLowerPrediction.getDistanceTo(detectedPalm.FingersLower);
            float fingersLowerSimilarity = Math.Max(0, 1 - fingersLowerDistance / Parameters.ImageDiagonalLength);

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

    }
}