using System.Collections.Generic;
using System;
using bbiwarg.Utility;

namespace bbiwarg.Recognition.Tracking
{
    internal abstract class Tracker<T, TrackedT>
        where T : TrackableObject
        where TrackedT : TrackedObject<T>
    {
        private ImageSize imageSize;
        private List<Similarity<T, TrackedT>> similarities;
        protected TrackIDPool idPool;

        public List<TrackedT> TrackedObjects;

        public Tracker(ImageSize imageSize)
        {
            this.imageSize = imageSize;
            reset();
        }

        public void reset()
        {
            idPool = new TrackIDPool();
            TrackedObjects = new List<TrackedT>();
        }

        protected void trackObjects(List<T> detectedObjects)
        {
            if (TrackedObjects.Count == 0)
            {
                addNewTrackedObjects(detectedObjects);
            }
            else
            {
                updateTrackedObjects(detectedObjects);
                removeDeletableTrackedObjects();
            }
        }

        protected void updateTrackedObjects(List<T> detectedObjects)
        {
            List<TrackedT> unassignedTrackedObjects = new List<TrackedT>(TrackedObjects);
            List<T> unassignedDetectedObjects = new List<T>(detectedObjects);

            createSimilarities(detectedObjects);
            while (similarities.Count > 0)
            {
                Similarity<T, TrackedT> maxSimilarity = similarities[0];
                maxSimilarity.TrackedObject.updateFrame(maxSimilarity.DetectedObject);

                unassignedDetectedObjects.Remove(maxSimilarity.DetectedObject);
                unassignedTrackedObjects.Remove(maxSimilarity.TrackedObject);
                removeConcurringSimilarities(maxSimilarity);
            }

            addNewTrackedObjects(unassignedDetectedObjects);

            foreach (TrackedT trackedObject in unassignedTrackedObjects)
            {
                trackedObject.updateFrame(null);
            }
        }

        private void addNewTrackedObjects(List<T> detectedObjects)
        {
            foreach (T detectedObject in detectedObjects)
            {
                TrackedT trackedObject = createTrackedObject(detectedObject);
                TrackedObjects.Add(trackedObject);
            }
        }

        private void createSimilarities(List<T> detectedObjects)
        {
            similarities = new List<Similarity<T, TrackedT>>();

            foreach (TrackedT trackedObject in TrackedObjects)
            {
                foreach (T detectedObject in detectedObjects)
                {
                    float similarityValue = calculateSimilarity(trackedObject, detectedObject);
                    Similarity<T, TrackedT> similarity = new Similarity<T, TrackedT>(trackedObject, detectedObject, similarityValue);
                    if (similarity.Value > 0)
                        similarities.Add(similarity);
                }
            }

            // sort depending on similarity-value
            similarities.Sort((s1, s2) => s2.Value.CompareTo(s1.Value));
        }

        public abstract float calculateSimilarity(TrackedT trackedObject, T detectedObject);


        private void removeConcurringSimilarities(Similarity<T, TrackedT> similarity)
        {
            for (int i = similarities.Count - 1; i >= 0; i--)
            {
                Similarity<T, TrackedT> s = similarities[i];
                if (s.TrackedObject == similarity.TrackedObject || s.DetectedObject == similarity.DetectedObject)
                    similarities.RemoveAt(i);
            }
        }

        private void removeDeletableTrackedObjects()
        {
            for (int i = TrackedObjects.Count - 1; i >= 0; i--)
            {
                TrackedT trackedObject = TrackedObjects[i];
                if (trackedObject.CurrentState == TrackingState.Deleted)
                {
                    idPool.setIDUnused(trackedObject.ID);
                    TrackedObjects.RemoveAt(i);
                }
            }
        }

        protected List<T> getCurrentObjectsWithState(TrackingState state)
        {
            List<T> objects = new List<T>();
            foreach (TrackedT trackedObject in TrackedObjects)
            {
                if (trackedObject.CurrentState == state)
                    objects.Add(trackedObject.CurrentObject);
            }
            return objects;
        }

        protected abstract TrackedT createTrackedObject(T detectedObject);

        protected float getPositionSimilarity(Vector2D p1, Vector2D p2, float maxRelativeDistance)
        {
            float distance = p1.getDistanceTo(p2);
            float maxDistance = maxRelativeDistance * imageSize.DiagonalLength;
            float similarity = Math.Max(1 - distance / maxDistance, 0);
            return similarity;
        }
    }
}