using BBIWARG.Utility;
using System;
using System.Collections.Generic;

namespace BBIWARG.Recognition.Tracking
{
    /// <summary>
    /// Tracks multiple trackableObjects
    /// </summary>
    /// <typeparam name="T">The type of the TrackableObjects</typeparam>
    /// <typeparam name="TrackedT">The type of the TrackedObject</typeparam>
    public abstract class Tracker<T, TrackedT>
        where T : TrackableObject
        where TrackedT : TrackedObject<T>
    {
        /// <summary>
        /// the trackedObjects
        /// </summary>
        public List<TrackedT> TrackedObjects;

        /// <summary>
        /// the unique ID generator
        /// </summary>
        protected TrackIDPool idPool;

        /// <summary>
        /// the size of the input image (used to calculate position similarities)
        /// </summary>
        private ImageSize imageSize;

        /// <summary>
        /// list of similarities between trackableObjects and trackedObjects
        /// </summary>
        private List<Similarity<T, TrackedT>> similarities;

        /// <summary>
        /// Initializes a new instance of the Tracker class.
        /// </summary>
        /// <param name="imageSize">Size of the input image.</param>
        public Tracker(ImageSize imageSize)
        {
            this.imageSize = imageSize;
            reset();
        }

        /// <summary>
        /// Calculates the similarity [0-1] between a TrackedObject and a TrackableObject.
        /// </summary>
        /// <param name="trackedObject">the tracked object</param>
        /// <param name="detectedObject">the detected trackable object</param>
        /// <returns>the similarity [0-1]</returns>
        public abstract float calculateSimilarity(TrackedT trackedObject, T detectedObject);

        /// <summary>
        /// Resets the idPool and the trackedObjects.
        /// </summary>
        public void reset()
        {
            idPool = new TrackIDPool();
            TrackedObjects = new List<TrackedT>();
        }

        /// <summary>
        /// Creates a new TrackedObject with the given TrackableObject as initial object.
        /// </summary>
        /// <param name="detectedObject">the initial trackableObject</param>
        /// <returns>the TrackedObject</returns>
        protected abstract TrackedT createTrackedObject(T detectedObject);

        /// <summary>
        /// Gets a list of TrackableObjects from all TrackedObjects with the given state.
        /// </summary>
        /// <param name="state">the desired tracking state</param>
        /// <returns>the list of trackableObjects with the given state</returns>
        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;
        }

        /// <summary>
        /// Calculates a similarity [0-1] between two positions with the given maximum relative distance.
        /// </summary>
        /// <param name="p1">the first position</param>
        /// <param name="p2">the second position</param>
        /// <param name="maxRelativeDistance">the maximum distance [0-1] relative to the image size (maxAbsoluteDistance = maxRelativeDistance*imageSize.DiagonalLength)</param>
        /// <returns>the calculated similarity between the two positions</returns>
        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;
        }

        /// <summary>
        /// adds new TrackedObjects or updates and removes TrackedObjects with the new trackableObjects
        /// </summary>
        /// <param name="detectedObjects">the trackableObjects in the current frame</param>
        protected void trackObjects(List<T> detectedObjects)
        {
            if (TrackedObjects.Count == 0)
            {
                addNewTrackedObjects(detectedObjects);
            }
            else
            {
                updateTrackedObjects(detectedObjects);
                removeDeletableTrackedObjects();
            }
        }

        /// <summary>
        /// Updates the trackedObjects with the detectedObjects in the current frame. Each TrackedObject is assigned the best fitting detectedObject. Each unassigned detectedObject gets a new instance of a TrackedObject, each unassigned TrackedObject gets updated with null.
        /// </summary>
        /// <param name="detectedObjects">the trackableObjects in the current frame</param>
        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);
            }
        }

        /// <summary>
        /// creates a new TrackedObject for each trackableObject
        /// </summary>
        /// <param name="detectedObjects">the unassigned trackableObjects</param>
        private void addNewTrackedObjects(List<T> detectedObjects)
        {
            foreach (T detectedObject in detectedObjects)
            {
                TrackedT trackedObject = createTrackedObject(detectedObject);
                TrackedObjects.Add(trackedObject);
            }
        }

        /// <summary>
        /// Creates the list of similarities by creating a similarity for each TrackedObject with each detected TrackableObject.
        /// </summary>
        /// <param name="detectedObjects">the trackableObjects in the current frame</param>
        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);
                    if (similarityValue > 0)
                        similarities.Add(new Similarity<T, TrackedT>(trackedObject, detectedObject, similarityValue));
                }
            }

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

        /// <summary>
        /// Removes all similarities with the trackedObject or the same trackableObject as the given similarity.
        /// </summary>
        /// <param name="similarity">the similarity of two assigned objects</param>
        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);
            }
        }

        /// <summary>
        /// Removes all TrackedObjects, which current state is "delete".
        /// </summary>
        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);
                }
            }
        }
    }
}