using System.Diagnostics.CodeAnalysis;
using OpenTK.Mathematics;

namespace DoomDeathmatch.Component.Util.Collision;

public class CollisionManager : Engine.Scene.Component.Component
{
  private List<AABBColliderComponent> _colliders = [];

  public override void PreUpdate()
  {
    _colliders = GameObject.Scene!.FindAllComponents<AABBColliderComponent>();
  }

  public override void Update(double parDeltaTime)
  {
    for (var i = 0; i < _colliders.Count; i++)
    {
      var colliderA = _colliders[i];

      for (var j = i + 1; j < _colliders.Count; j++)
      {
        var colliderB = _colliders[j];

        var canCollideAB = colliderA.ExcludeColliderCollideGroups.Count == 0 ||
                           !colliderA.ExcludeColliderCollideGroups.IsSubsetOf(colliderB.ColliderGroups);
        var canCollideBA = colliderB.ExcludeColliderCollideGroups.Count == 0 ||
                           !colliderB.ExcludeColliderCollideGroups.IsSubsetOf(colliderA.ColliderGroups);

        if (!canCollideAB && !canCollideBA)
          continue;

        if (!colliderA.Collider.Intersects(colliderB.Collider))
          continue;

        if (canCollideAB)
          colliderA.CollideWith(colliderB);

        if (canCollideBA)
          colliderB.CollideWith(colliderA);
      }
    }
  }

  public bool Raycast(Vector3 parStart, Vector3 parDirection, HashSet<string> parColliderGroups,
    [MaybeNullWhen(false)] out RaycastResult parResult)
  {
    var start = parStart;
    var direction = parDirection.Normalized();

    var closestDistance = float.MaxValue;

    parResult = null;
    foreach (var collider in _colliders)
    {
      if (!collider.ColliderGroups.Overlaps(parColliderGroups))
        continue;

      if (!RaycastAABB(start, direction, collider.Collider, out var hitPoint, out var normal))
        continue;

      var distance = (start - hitPoint).Length;
      if (distance > closestDistance)
        continue;

      closestDistance = distance;

      parResult = new RaycastResult
      {
        HitPoint = hitPoint, Normal = normal, Distance = distance, HitObject = collider.GameObject
      };
    }

    return parResult != null;
  }

  private static bool RaycastAABB(Vector3 parOrigin, Vector3 parDirection, AABBCollider parCollider,
    out Vector3 parHitPoint,
    out Vector3 parHitNormal)
  {
    const int RIGHT = 0;
    const int LEFT = 1;
    const int MIDDLE = 2;

    parHitPoint = Vector3.Zero;
    parHitNormal = Vector3.Zero;

    var minB = parCollider.Min;
    var maxB = parCollider.Max;

    // Initialize arrays to store the 3D components
    var inside = true;
    var quadrant = new int[3];
    var maxT = new float[3];
    var candidatePlane = new float[3];

    // Find candidate planes
    for (var i = 0; i < 3; i++)
    {
      if (parOrigin[i] < minB[i])
      {
        quadrant[i] = LEFT;
        candidatePlane[i] = minB[i];
        inside = false;
      }
      else if (parOrigin[i] > maxB[i])
      {
        quadrant[i] = RIGHT;
        candidatePlane[i] = maxB[i];
        inside = false;
      }
      else
      {
        quadrant[i] = MIDDLE;
      }
    }

    // Ray origin inside bounding box
    if (inside)
    {
      parHitPoint = parOrigin;
      return true;
    }

    // Calculate T distances to candidate planes
    for (var i = 0; i < 3; i++)
    {
      if (quadrant[i] != MIDDLE && parDirection[i] != 0.0f)
        maxT[i] = (candidatePlane[i] - parOrigin[i]) / parDirection[i];
      else
        maxT[i] = -1.0f;
    }

    // Get largest of the maxT's for final choice of intersection
    var whichPlane = 0;
    for (var i = 1; i < 3; i++)
    {
      if (maxT[whichPlane] < maxT[i])
        whichPlane = i;
    }

    // Check final candidate actually inside box
    if (maxT[whichPlane] < 0.0f)
      return false;

    for (var i = 0; i < 3; i++)
    {
      if (whichPlane != i)
      {
        parHitPoint[i] = parOrigin[i] + maxT[whichPlane] * parDirection[i];
        if (parHitPoint[i] < minB[i] || parHitPoint[i] > maxB[i])
          return false;
      }
      else
      {
        parHitPoint[i] = candidatePlane[i];
        // Calculate normal for the intersection plane
        parHitNormal[i] = (quadrant[i] == LEFT) ? -1.0f : 1.0f;
      }
    }

    return true;
  }
}