﻿using Nemesis.Mathematics.Geometry;
using OpenTK;
using System;
using System.Collections.Generic;

namespace Nemesis.Physics.Collision.Response.Contact
{
    internal sealed class ContactsCache
    {
        const float contactDisplacementThreshold = 0.1f;
        const float contactDisplacementThreshold2 = contactDisplacementThreshold * contactDisplacementThreshold;

        private readonly List<CachedContact> contacts = new List<CachedContact>();

        public void AddContact(ParticlesContact newContact)
        {
            if (!ReplaceCachedContact(newContact))
            {
                contacts.Add(new CachedContact(newContact));
            }
        }

        private bool ReplaceCachedContact(ParticlesContact newContact)
        {
            var cachedContactIndex = GetCloseCachedContactIndex(newContact.ContactPoint);

            if (!cachedContactIndex.HasValue)
            {
                return false;
            }

            contacts[cachedContactIndex.Value] = new CachedContact(newContact);

            return true;
        }

        private int? GetCloseCachedContactIndex(Vector3 position)
        {
            var closeDistance2 = contactDisplacementThreshold2;
            int? closeContactIndex = null;

            for (var i = 0; i < contacts.Count; ++i)
            {
                var distance2 = position.DistanceSquared(contacts[i].ContactPoint);

                if (distance2 < closeDistance2)
                {
                    closeDistance2 = distance2;
                    closeContactIndex = i;
                }
            }

            return closeContactIndex;
        }

        public void ForEachContact(Action<ParticlesContact> action)
        {
            foreach (var c in contacts)
            {
                c.DoAction(action);
            }
        }

        public void RemoveInvalidContacts()
        {
            contacts.RemoveAll((CachedContact c) =>
            {
                return c.GetContactDisplacement() > contactDisplacementThreshold2;
            });
        }
    }
}
