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;

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

        public TrackedFinger(int id, Finger detectedFinger, int numFramesDetectedUntilTracked, int numFramesLostUntilDeleted)
            : base(id, detectedFinger, numFramesDetectedUntilTracked, numFramesLostUntilDeleted)
        {
            tipPointKalman = new Kalman2DPositionFilter(Constants.FingermXX, Constants.FingermXY, Constants.FingermYY);
            tipPointKalman.setInitialPosition(detectedFinger.TipPoint);
            handPointKalman = new Kalman2DPositionFilter(Constants.FingermXX, Constants.FingermXY, Constants.FingermYY);
            handPointKalman.setInitialPosition(detectedFinger.HandPoint);

            logStateChange();
        }

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

            if (NumFramesInCurrentState == 1)
                logStateChange();

            if (detectedObject != null)
            {
                tipPointKalman.getCorrectedPosition(detectedObject.TipPoint);
                handPointKalman.getCorrectedPosition(detectedObject.HandPoint);
            }
        }

        public override float getSimilarity(Finger detectedFinger)
        {
            //tip position
            float tipPointDistance = detectedFinger.TipPoint.getDistanceTo(TipPointPrediction);
            float tipPointSimilarity = Math.Max(0, 1 - tipPointDistance / Constants.ImageDiagonalLength);

            //hand position
            float handPointDistance = detectedFinger.HandPoint.getDistanceTo(HandPointPrediction);
            float handPointSimilarity = Math.Max(0, 1 - handPointDistance / Constants.ImageDiagonalLength);

            return tipPointSimilarity * handPointSimilarity;
        }

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