﻿using System.Collections.Generic;
using ShellUtil;

namespace EnergyShell.Physics
{
    /// <summary>
    /// Narrow phase collider for circles.
    /// </summary>
    public sealed class CircleNarrowPhase
    {
        /// <summary>
        /// The initial manifold pair cache size.
        /// </summary>
        private const int InitialCacheSize = 5000;

        /// <summary>
        /// The manifold pair cache which we use to grab reusable manifold pairs from.
        /// </summary>
        private readonly List<ManifoldPair> manifoldPairCache;

        /// <summary>
        /// Initializes a new instance of the <see cref="CircleNarrowPhase"/> class.
        /// </summary>
        public CircleNarrowPhase()
        {
            this.manifoldPairCache = new List<ManifoldPair>(InitialCacheSize);
            IncreaseCacheSize(InitialCacheSize);
        }

        /// <summary>
        /// Increases the size of the cache.
        /// </summary>
        /// <param name="newSize">The new size.</param>
        private void IncreaseCacheSize(int newSize)
        {
            this.manifoldPairCache.Clear();
            for (int i = 0; i < newSize; ++i)
            {
                this.manifoldPairCache.Add(new ManifoldPair());
            }
        }

        /// <summary>
        /// Finds all narrow phase collisions from broad phase contact pairs.
        /// </summary>
        /// <param name="broadPhasePairs">The broad phase pairs.</param>
        /// <returns>An enumerable containing narrow phase manifold pairs.</returns>
        public IEnumerable<ManifoldPair> FindCollisions(IList<ContactPair> broadPhasePairs)
        {
            // Increase the size of the cache if it is going to be potentially too small.
            if (broadPhasePairs.Count > this.manifoldPairCache.Count)
            {
                IncreaseCacheSize(broadPhasePairs.Count);
            }

            IList<ManifoldPair> collisionPairs = new List<ManifoldPair>();

            int manifoldCachePointer = 0;

            foreach (ContactPair contactPair in broadPhasePairs)
            {
                float shapeDistance = Vector2.Distance(contactPair.BodyOne.Position, contactPair.BodyTwo.Position);
                float cumulativeRadius = contactPair.BodyOne.Radius + contactPair.BodyTwo.Radius;
                // If they aren't colliding then we can continue to the next contact pair.
                if (!(shapeDistance < cumulativeRadius)) continue;
                // We need to generate a manifold pair for the two circles.
                ManifoldPair manifoldPair = this.manifoldPairCache[manifoldCachePointer++];
                manifoldPair.MovedBody = contactPair.BodyOne;
                manifoldPair.StaticBody = contactPair.BodyTwo;
                manifoldPair.Penetration = cumulativeRadius - shapeDistance;
                manifoldPair.ResolutionNormal = Vector2.Normalize(contactPair.BodyOne.Position - contactPair.BodyTwo.Position);
                collisionPairs.Add(manifoldPair);
            }

            return collisionPairs;
        }
    }
}
