#include "CMetroidAreaCollider.hpp"
#include "CMaterialFilter.hpp"
#include "CollisionUtil.hpp"
#include "CCollisionInfoList.hpp"

namespace urde
{

u32 CMetroidAreaCollider::g_CalledClip = 0;
u32 CMetroidAreaCollider::g_RejectedByClip = 0;
u32 CMetroidAreaCollider::g_TrianglesProcessed = 0;
u32 CMetroidAreaCollider::g_DupTrianglesProcessed = 0;
u16 CMetroidAreaCollider::g_DupPrimitiveCheckCount = 0;
u16 CMetroidAreaCollider::g_DupVertexList[0x5000] = {};
u16 CMetroidAreaCollider::g_DupEdgeList[0xC000] = {};
u16 CMetroidAreaCollider::g_DupTriangleList[0x4000] = {};

CAABoxAreaCache::CAABoxAreaCache(const zeus::CAABox& aabb, const zeus::CPlane* pl, const CMaterialFilter& filter,
                                 const CMaterialList& material, CCollisionInfoList& collisionList)
: x0_aabb(aabb), x4_planes(pl), x8_filter(filter), xc_material(material), x10_collisionList(collisionList),
  x14_center(aabb.center()), x20_halfExtent(aabb.extents())
{}

CBooleanAABoxAreaCache::CBooleanAABoxAreaCache(const zeus::CAABox& aabb, const CMaterialFilter& filter)
: x0_aabb(aabb), x4_filter(filter), x8_center(aabb.center()), x14_halfExtent(aabb.extents())
{}

CSphereAreaCache::CSphereAreaCache(const zeus::CAABox& aabb, const zeus::CSphere& sphere, const CMaterialFilter& filter,
                                   const CMaterialList& material, CCollisionInfoList& collisionList)
: x0_aabb(aabb), x4_sphere(sphere), x8_filter(filter), xc_material(material), x10_collisionList(collisionList)
{}

CBooleanSphereAreaCache::CBooleanSphereAreaCache(const zeus::CAABox& aabb, const zeus::CSphere& sphere,
                                                 const CMaterialFilter& filter)
: x0_aabb(aabb), x4_sphere(sphere), x8_filter(filter)
{}

SBoxEdge::SBoxEdge(const zeus::CAABox& aabb, int idx, const zeus::CVector3f& dir)
: x0_seg(aabb.getEdge(zeus::CAABox::EBoxEdgeId(idx))), x28_dir(x0_seg.dir), x40_end(x0_seg.end),
  x58_start(x40_end - x28_dir), x70_coDir(x58_start.cross(dir).asNormalized()),
  x88_dirCoDirDot(x28_dir.dot(x70_coDir))
{}

static void FlagEdgeIndicesForFace(int face, bool edgeFlags[12])
{
    switch (face)
    {
    case 0:
        edgeFlags[10] = true;
        edgeFlags[11] = true;
        edgeFlags[2] = true;
        edgeFlags[4] = true;
        return;
    case 1:
        edgeFlags[8] = true;
        edgeFlags[9] = true;
        edgeFlags[0] = true;
        edgeFlags[6] = true;
        return;
    case 2:
        edgeFlags[4] = true;
        edgeFlags[5] = true;
        edgeFlags[6] = true;
        edgeFlags[7] = true;
        return;
    case 3:
        edgeFlags[0] = true;
        edgeFlags[1] = true;
        edgeFlags[2] = true;
        edgeFlags[3] = true;
        return;
    case 4:
        edgeFlags[7] = true;
        edgeFlags[8] = true;
        edgeFlags[3] = true;
        edgeFlags[11] = true;
        return;
    case 5:
        edgeFlags[1] = true;
        edgeFlags[5] = true;
        edgeFlags[9] = true;
        edgeFlags[10] = true;
        return;
    default: break;
    }
}

static void FlagVertexIndicesForFace(int face, bool vertFlags[8])
{
    switch (face)
    {
    case 0:
        vertFlags[1] = true;
        vertFlags[3] = true;
        vertFlags[5] = true;
        vertFlags[7] = true;
        return;
    case 1:
        vertFlags[0] = true;
        vertFlags[2] = true;
        vertFlags[4] = true;
        vertFlags[6] = true;
        return;
    case 2:
        vertFlags[2] = true;
        vertFlags[3] = true;
        vertFlags[6] = true;
        vertFlags[7] = true;
        return;
    case 3:
        vertFlags[0] = true;
        vertFlags[1] = true;
        vertFlags[4] = true;
        vertFlags[5] = true;
        return;
    case 4:
        vertFlags[4] = true;
        vertFlags[5] = true;
        vertFlags[6] = true;
        vertFlags[7] = true;
        return;
    case 5:
        vertFlags[0] = true;
        vertFlags[1] = true;
        vertFlags[2] = true;
        vertFlags[3] = true;
        return;
    default: break;
    }
}

CMovingAABoxComponents::CMovingAABoxComponents(const zeus::CAABox& aabb, const zeus::CVector3f& dir)
: x6e8_aabb(aabb)
{
    bool edgeFlags[12] = {};
    bool vertFlags[8] = {};
    int useFaces = 0;

    for (int i=0 ; i<3 ; ++i)
    {
        if (dir[i] != 0.f)
        {
            int face = i * 2 + (dir[i] < 0.f);
            FlagEdgeIndicesForFace(face, edgeFlags);
            FlagVertexIndicesForFace(face, vertFlags);
            useFaces += 1;
        }
    }

    for (int i=0 ; i<12 ; ++i)
        x0_edges.push_back(SBoxEdge(aabb, i, dir));

    for (int i=0 ; i<8 ; ++i)
        if (vertFlags[i])
            x6c4_vertIdxs.push_back(i);

    if (useFaces == 1)
    {
        x6e8_aabb = zeus::CAABox::skInvertedBox;
        x6e8_aabb.accumulateBounds(aabb.getPoint(x6c4_vertIdxs[0]));
        x6e8_aabb.accumulateBounds(aabb.getPoint(x6c4_vertIdxs[1]));
        x6e8_aabb.accumulateBounds(aabb.getPoint(x6c4_vertIdxs[2]));
        x6e8_aabb.accumulateBounds(aabb.getPoint(x6c4_vertIdxs[3]));
    }
}

CMetroidAreaCollider::COctreeLeafCache::COctreeLeafCache(const CAreaOctTree& octTree)
: x0_octTree(octTree)
{
    x908_24_overflow = false;
}

void CMetroidAreaCollider::COctreeLeafCache::AddLeaf(const CAreaOctTree::Node& node)
{
    if (x4_nodeCache.size() == 64)
    {
        x908_24_overflow = true;
        return;
    }

    x4_nodeCache.push_back(node);
}

void CMetroidAreaCollider::BuildOctreeLeafCache(const CAreaOctTree::Node& node, const zeus::CAABox& aabb,
                                                CMetroidAreaCollider::COctreeLeafCache& cache)
{
    for (int i=0 ; i<8 ; ++i)
    {
        u16 flags = (node.GetChildFlags() >> (i * 2)) & 0x3;
        if (flags)
        {
            CAreaOctTree::Node ch = node.GetChild(i);
            if (aabb.intersects(ch.GetBoundingBox()))
            {
                if (flags == 0x2)
                    cache.AddLeaf(ch);
                else
                    BuildOctreeLeafCache(ch, aabb, cache);
            }
        }
    }
}

static zeus::CVector3f ClipRayToPlane(const zeus::CVector3f& a, const zeus::CVector3f& b,
                                      const zeus::CPlane& plane)
{
    return (1.f - -plane.pointToPlaneDist(a) / (b - a).dot(plane.vec)) * (a - b) + b;
}

bool CMetroidAreaCollider::ConvexPolyCollision(const zeus::CPlane* planes, const zeus::CVector3f* verts,
                                               zeus::CAABox& aabb)
{
    rstl::reserved_vector<zeus::CVector3f, 20> vecs[2];

    g_CalledClip += 1;
    g_RejectedByClip -= 1;

    vecs[0].push_back(verts[0]);
    vecs[0].push_back(verts[1]);
    vecs[0].push_back(verts[2]);

    int vecIdx = 0;
    int otherVecIdx = 1;
    for (int i=0 ; i<6 ; ++i)
    {
        rstl::reserved_vector<zeus::CVector3f, 20>& vec = vecs[vecIdx];
        rstl::reserved_vector<zeus::CVector3f, 20>& otherVec = vecs[otherVecIdx];
        otherVec.clear();

        bool inFrontOf = planes[i].pointToPlaneDist(vec.front()) >= 0.f;
        for (int j=0 ; j<vec.size() ; ++j)
        {
            const zeus::CVector3f& b = vec[(j + 1) % vec.size()];
            if (inFrontOf)
                otherVec.push_back(vec[j]);
            if ((planes[i].pointToPlaneDist(b) >= 0.f) ^ inFrontOf)
                otherVec.push_back(ClipRayToPlane(vec[j], b, planes[i]));
        }

        if (otherVec.empty())
            return false;

        vecIdx ^= 1;
        otherVecIdx ^= 1;
    }

    rstl::reserved_vector<zeus::CVector3f, 20>& accumVec = vecs[otherVecIdx ^ 1];
    for (const zeus::CVector3f& point : accumVec)
        aabb.accumulateBounds(point);

    g_RejectedByClip -= 1;
    return true;
}

bool CMetroidAreaCollider::AABoxCollisionCheckBoolean_Cached(const COctreeLeafCache& leafCache, const zeus::CAABox& aabb,
                                                             const CMaterialFilter& filter)
{
    CBooleanAABoxAreaCache cache(aabb, filter);

    for (const CAreaOctTree::Node& node : leafCache.x4_nodeCache)
    {
        if (cache.x0_aabb.intersects(node.GetBoundingBox()))
        {
            CAreaOctTree::TriListReference list = node.GetTriangleArray();
            for (int j=0 ; j<list.GetSize() ; ++j)
            {
                ++g_TrianglesProcessed;
                CCollisionSurface surf = node.GetOwner().GetMasterListTriangle(list.GetAt(j));
                if (cache.x4_filter.Passes(CMaterialList(surf.GetSurfaceFlags())))
                {
                    if (CollisionUtil::TriBoxOverlap(cache.x8_center, cache.x14_halfExtent,
                                                     surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                        return true;
                }
            }
        }
    }

    return false;
}

bool CMetroidAreaCollider::AABoxCollisionCheckBoolean_Internal(const CAreaOctTree::Node& node,
                                                               const CBooleanAABoxAreaCache& cache)
{
    for (int i=0 ; i<8 ; ++i)
    {
        CAreaOctTree::Node::ETreeType type = node.GetChildType(i);
        if (type != CAreaOctTree::Node::ETreeType::Invalid)
        {
            CAreaOctTree::Node ch = node.GetChild(i);
            if (cache.x0_aabb.intersects(ch.GetBoundingBox()))
            {
                if (type == CAreaOctTree::Node::ETreeType::Leaf)
                {
                    CAreaOctTree::TriListReference list = ch.GetTriangleArray();
                    for (int j=0 ; j<list.GetSize() ; ++j)
                    {
                        ++g_TrianglesProcessed;
                        CCollisionSurface surf = ch.GetOwner().GetMasterListTriangle(list.GetAt(j));
                        if (cache.x4_filter.Passes(CMaterialList(surf.GetSurfaceFlags())))
                        {
                            if (CollisionUtil::TriBoxOverlap(cache.x8_center, cache.x14_halfExtent,
                                                             surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                                return true;
                        }
                    }
                }
                else
                {
                    if (AABoxCollisionCheckBoolean_Internal(ch, cache))
                        return true;
                }
            }
        }
    }
    return false;
}

bool CMetroidAreaCollider::AABoxCollisionCheckBoolean(const CAreaOctTree& octTree, const zeus::CAABox& aabb,
                                                      const CMaterialFilter& filter)
{
    CBooleanAABoxAreaCache cache(aabb, filter);
    return AABoxCollisionCheckBoolean_Internal(octTree.GetRootNode(), cache);
}

bool CMetroidAreaCollider::SphereCollisionCheckBoolean_Cached(const COctreeLeafCache& leafCache, const zeus::CAABox& aabb,
                                                              const zeus::CSphere& sphere, const CMaterialFilter& filter)
{
    CBooleanSphereAreaCache cache(aabb, sphere, filter);

    for (const CAreaOctTree::Node& node : leafCache.x4_nodeCache)
    {
        if (cache.x0_aabb.intersects(node.GetBoundingBox()))
        {
            CAreaOctTree::TriListReference list = node.GetTriangleArray();
            for (int j=0 ; j<list.GetSize() ; ++j)
            {
                ++g_TrianglesProcessed;
                CCollisionSurface surf = node.GetOwner().GetMasterListTriangle(list.GetAt(j));
                if (cache.x8_filter.Passes(CMaterialList(surf.GetSurfaceFlags())))
                {
                    if (CollisionUtil::TriSphereOverlap(cache.x4_sphere,
                                                        surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                        return true;
                }
            }
        }
    }

    return false;
}

bool CMetroidAreaCollider::SphereCollisionCheckBoolean_Internal(const CAreaOctTree::Node& node,
                                                                const CBooleanSphereAreaCache& cache)
{
    for (int i=0 ; i<8 ; ++i)
    {
        CAreaOctTree::Node::ETreeType type = node.GetChildType(i);
        if (type != CAreaOctTree::Node::ETreeType::Invalid)
        {
            CAreaOctTree::Node ch = node.GetChild(i);
            if (cache.x0_aabb.intersects(ch.GetBoundingBox()))
            {
                if (type == CAreaOctTree::Node::ETreeType::Leaf)
                {
                    CAreaOctTree::TriListReference list = ch.GetTriangleArray();
                    for (int j=0 ; j<list.GetSize() ; ++j)
                    {
                        ++g_TrianglesProcessed;
                        CCollisionSurface surf = ch.GetOwner().GetMasterListTriangle(list.GetAt(j));
                        if (cache.x8_filter.Passes(CMaterialList(surf.GetSurfaceFlags())))
                        {
                            if (CollisionUtil::TriSphereOverlap(cache.x4_sphere,
                                                                surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                                return true;
                        }
                    }
                }
                else
                {
                    if (SphereCollisionCheckBoolean_Internal(ch, cache))
                        return true;
                }
            }
        }
    }
    return false;
}

bool CMetroidAreaCollider::SphereCollisionCheckBoolean(const CAreaOctTree& octTree, const zeus::CAABox& aabb,
                                                       const zeus::CSphere& sphere, const CMaterialFilter& filter)
{
    CAreaOctTree::Node node = octTree.GetRootNode();
    CBooleanSphereAreaCache cache(aabb, sphere, filter);
    return SphereCollisionCheckBoolean_Internal(node, cache);
}

bool CMetroidAreaCollider::AABoxCollisionCheck_Cached(const COctreeLeafCache& leafCache, const zeus::CAABox& aabb,
                                                      const CMaterialFilter& filter, const CMaterialList& matList,
                                                      CCollisionInfoList& list)
{
    bool ret = false;
    zeus::CPlane planes[] =
    {
        {zeus::CVector3f::skRight, aabb.min.dot(zeus::CVector3f::skRight)},
        {zeus::CVector3f::skLeft, aabb.max.dot(zeus::CVector3f::skLeft)},
        {zeus::CVector3f::skForward, aabb.min.dot(zeus::CVector3f::skForward)},
        {zeus::CVector3f::skBack, aabb.max.dot(zeus::CVector3f::skBack)},
        {zeus::CVector3f::skUp, aabb.min.dot(zeus::CVector3f::skUp)},
        {zeus::CVector3f::skDown, aabb.max.dot(zeus::CVector3f::skDown)}
    };
    CAABoxAreaCache cache(aabb, planes, filter, matList, list);

    ResetInternalCounters();

    for (const CAreaOctTree::Node& node : leafCache.x4_nodeCache)
    {
        if (aabb.intersects(node.GetBoundingBox()))
        {
            CAreaOctTree::TriListReference list = node.GetTriangleArray();
            for (int j=0 ; j<list.GetSize() ; ++j)
            {
                ++g_TrianglesProcessed;
                u16 triIdx = list.GetAt(j);
                if (g_DupPrimitiveCheckCount == g_DupTriangleList[triIdx])
                {
                    g_DupTrianglesProcessed += 1;
                }
                else
                {
                    g_DupTriangleList[triIdx] = g_DupPrimitiveCheckCount;
                    CCollisionSurface surf = node.GetOwner().GetMasterListTriangle(triIdx);
                    CMaterialList material(surf.GetSurfaceFlags());
                    if (cache.x8_filter.Passes(material))
                    {
                        if (CollisionUtil::TriBoxOverlap(cache.x14_center, cache.x20_halfExtent,
                                                         surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                        {
                            zeus::CAABox aabb = zeus::CAABox::skInvertedBox;
                            if (ConvexPolyCollision(cache.x4_planes, surf.GetVerts(), aabb))
                            {
                                zeus::CPlane plane = surf.GetPlane();
                                CCollisionInfo collision(aabb, cache.xc_material, material,
                                                         plane.normal(), -plane.normal());
                                cache.x10_collisionList.Add(collision, false);
                                ret = true;
                            }
                        }
                    }
                }
            }
        }
    }

    return ret;
}

bool CMetroidAreaCollider::AABoxCollisionCheck_Internal(const CAreaOctTree::Node& node,
                                                        const CAABoxAreaCache& cache)
{
    bool ret = false;

    switch (node.GetTreeType())
    {
    case CAreaOctTree::Node::ETreeType::Invalid:
        return false;
    case CAreaOctTree::Node::ETreeType::Branch:
    {
        for (int i=0 ; i<8 ; ++i)
        {
            CAreaOctTree::Node ch = node.GetChild(i);
            if (ch.GetBoundingBox().intersects(cache.x0_aabb))
                if (AABoxCollisionCheck_Internal(ch, cache))
                    ret = true;
        }
        break;
    }
    case CAreaOctTree::Node::ETreeType::Leaf:
    {
        CAreaOctTree::TriListReference list = node.GetTriangleArray();
        for (int j=0 ; j<list.GetSize() ; ++j)
        {
            ++g_TrianglesProcessed;
            u16 triIdx = list.GetAt(j);
            if (g_DupPrimitiveCheckCount == g_DupTriangleList[triIdx])
            {
                g_DupTrianglesProcessed += 1;
            }
            else
            {
                g_DupTriangleList[triIdx] = g_DupPrimitiveCheckCount;
                CCollisionSurface surf = node.GetOwner().GetMasterListTriangle(triIdx);
                CMaterialList material(surf.GetSurfaceFlags());
                if (cache.x8_filter.Passes(material))
                {
                    if (CollisionUtil::TriBoxOverlap(cache.x14_center, cache.x20_halfExtent,
                                                     surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                    {
                        zeus::CAABox aabb = zeus::CAABox::skInvertedBox;
                        if (ConvexPolyCollision(cache.x4_planes, surf.GetVerts(), aabb))
                        {
                            zeus::CPlane plane = surf.GetPlane();
                            CCollisionInfo collision(aabb, cache.xc_material, material,
                                                     plane.normal(), -plane.normal());
                            cache.x10_collisionList.Add(collision, false);
                            ret = true;
                        }
                    }
                }
            }
        }
        break;
    }
    default: break;
    }

    return ret;
}

bool CMetroidAreaCollider::AABoxCollisionCheck(const CAreaOctTree& octTree, const zeus::CAABox& aabb,
                                               const CMaterialFilter& filter, const CMaterialList& matList,
                                               CCollisionInfoList& list)
{
    zeus::CPlane planes[] =
    {
        {zeus::CVector3f::skRight, aabb.min.dot(zeus::CVector3f::skRight)},
        {zeus::CVector3f::skLeft, aabb.max.dot(zeus::CVector3f::skLeft)},
        {zeus::CVector3f::skForward, aabb.min.dot(zeus::CVector3f::skForward)},
        {zeus::CVector3f::skBack, aabb.max.dot(zeus::CVector3f::skBack)},
        {zeus::CVector3f::skUp, aabb.min.dot(zeus::CVector3f::skUp)},
        {zeus::CVector3f::skDown, aabb.max.dot(zeus::CVector3f::skDown)}
    };
    CAABoxAreaCache cache(aabb, planes, filter, matList, list);

    ResetInternalCounters();

    CAreaOctTree::Node node = octTree.GetRootNode();
    return AABoxCollisionCheck_Internal(node, cache);
}

bool CMetroidAreaCollider::SphereCollisionCheck_Cached(const COctreeLeafCache& leafCache, const zeus::CAABox& aabb,
                                                       const zeus::CSphere& sphere, const CMaterialList& matList,
                                                       const CMaterialFilter& filter, CCollisionInfoList& clist)
{
    ResetInternalCounters();

    bool ret = false;
    zeus::CVector3f point, normal;

    for (const CAreaOctTree::Node& node : leafCache.x4_nodeCache)
    {
        if (aabb.intersects(node.GetBoundingBox()))
        {
            CAreaOctTree::TriListReference list = node.GetTriangleArray();
            for (int j=0 ; j<list.GetSize() ; ++j)
            {
                ++g_TrianglesProcessed;
                u16 triIdx = list.GetAt(j);
                if (g_DupPrimitiveCheckCount == g_DupTriangleList[triIdx])
                {
                    g_DupTrianglesProcessed += 1;
                }
                else
                {
                    g_DupTriangleList[triIdx] = g_DupPrimitiveCheckCount;
                    CCollisionSurface surf = node.GetOwner().GetMasterListTriangle(triIdx);
                    CMaterialList material(surf.GetSurfaceFlags());
                    if (filter.Passes(material))
                    {
                        if (CollisionUtil::TriSphereIntersection(sphere,
                                                                 surf.GetVert(0), surf.GetVert(1), surf.GetVert(2),
                                                                 point, normal))
                        {
                            CCollisionInfo collision(point, matList, material, normal);
                            clist.Add(collision, false);
                            ret = true;
                        }
                    }
                }
            }
        }
    }

    return ret;
}

bool CMetroidAreaCollider::SphereCollisionCheck_Internal(const CAreaOctTree::Node& node,
                                                         const CSphereAreaCache& cache)
{
    bool ret = false;
    zeus::CVector3f point, normal;

    for (int i=0 ; i<8 ; ++i)
    {
        CAreaOctTree::Node::ETreeType chTp = node.GetChildType(i);
        if (chTp != CAreaOctTree::Node::ETreeType::Invalid)
        {
            CAreaOctTree::Node ch = node.GetChild(i);
            if (cache.x0_aabb.intersects(ch.GetBoundingBox()))
            {
                if (chTp == CAreaOctTree::Node::ETreeType::Leaf)
                {
                    CAreaOctTree::TriListReference list = ch.GetTriangleArray();
                    for (int j=0 ; j<list.GetSize() ; ++j)
                    {
                        ++g_TrianglesProcessed;
                        u16 triIdx = list.GetAt(j);
                        if (g_DupPrimitiveCheckCount == g_DupTriangleList[triIdx])
                        {
                            g_DupTrianglesProcessed += 1;
                        }
                        else
                        {
                            g_DupTriangleList[triIdx] = g_DupPrimitiveCheckCount;
                            CCollisionSurface surf = ch.GetOwner().GetMasterListTriangle(triIdx);
                            CMaterialList material(surf.GetSurfaceFlags());
                            if (cache.x8_filter.Passes(material))
                            {
                                if (CollisionUtil::TriSphereIntersection(cache.x4_sphere,
                                                                         surf.GetVert(0), surf.GetVert(1), surf.GetVert(2),
                                                                         point, normal))
                                {
                                    CCollisionInfo collision(point, cache.xc_material, material, normal);
                                    cache.x10_collisionList.Add(collision, false);
                                    ret = true;
                                }
                            }
                        }
                    }
                }
                else
                {
                    if (SphereCollisionCheck_Internal(ch, cache))
                        ret = true;
                }
            }
        }
    }

    return ret;
}

bool CMetroidAreaCollider::SphereCollisionCheck(const CAreaOctTree& octTree, const zeus::CAABox& aabb,
                                                const zeus::CSphere& sphere, const CMaterialList& matList,
                                                const CMaterialFilter& filter, CCollisionInfoList& list)
{
    CSphereAreaCache cache(aabb, sphere, filter, matList, list);
    ResetInternalCounters();
    CAreaOctTree::Node node = octTree.GetRootNode();
    return SphereCollisionCheck_Internal(node, cache);
}

bool CMetroidAreaCollider::MovingAABoxCollisionCheck_BoxVertexTri(const CCollisionSurface& surf, const zeus::CAABox& aabb,
                                                                  const rstl::reserved_vector<u32, 8>& vertIndices,
                                                                  const zeus::CVector3f& dir, double& d,
                                                                  zeus::CVector3f& normal, zeus::CVector3f& point)
{
    bool ret = false;
    for (u32 idx : vertIndices)
    {
        zeus::CVector3f point = aabb.getPoint(idx);
        if (CollisionUtil::RayTriangleIntersection_Double(point, dir, surf.GetVerts(), d))
        {
            point = float(d) * dir + point;
            normal = surf.GetNormal();
            ret = true;
        }
    }
    return ret;
}

bool CMetroidAreaCollider::MovingAABoxCollisionCheck_TriVertexBox(const zeus::CVector3f& vert, const zeus::CAABox& aabb,
                                                                  const zeus::CVector3f& dir, double& dOut,
                                                                  zeus::CVector3f& normal, zeus::CVector3f& point)
{
    zeus::CMRay ray(vert, -dir, dOut);
    zeus::CVector3f norm;
    double d;
    if (CollisionUtil::RayAABoxIntersection_Double(ray, aabb, norm, d) == 2)
    {
        d *= dOut;
        if (d < dOut)
        {
            normal = -norm;
            dOut = d;
            point = vert;
            return true;
        }
    }
    return false;
}

bool CMetroidAreaCollider::MovingAABoxCollisionCheck_Edge(const zeus::CVector3f& ev0, const zeus::CVector3f& ev1,
                                                          const rstl::reserved_vector<SBoxEdge, 12>& edges,
                                                          const zeus::CVector3f& dir, double& d,
                                                          zeus::CVector3f& normal, zeus::CVector3f& point)
{
    bool ret = false;

    for (const SBoxEdge& edge : edges)
    {
        zeus::CVector3d ev0d = ev0;
        zeus::CVector3d ev1d = ev1;
        if ((edge.x70_coDir.dot(ev0d) >= edge.x88_dirCoDirDot) !=
            (edge.x70_coDir.dot(ev1d) >= edge.x88_dirCoDirDot))
        {
            zeus::CVector3d delta = ev0d - ev1d;
            zeus::CVector3d cross0 = edge.x58_start.cross(delta);
            if (cross0.magSquared() > DBL_EPSILON)
            {
                zeus::CVector3d cross0Norm = cross0.asNormalized();
                if (cross0Norm.dot(dir) >= 0.0)
                {
                    ev1d = ev0;
                    ev0d = ev1;
                    delta = ev0d - ev1d;
                    cross0Norm = edge.x58_start.cross(delta).asNormalized();
                }

                zeus::CVector3d clipped = ev0d + -(ev0d.dot(edge.x70_coDir) - edge.x88_dirCoDirDot) /
                    delta.dot(edge.x70_coDir) * delta;
                int maxCompIdx = (std::fabs(edge.x70_coDir.x) > std::fabs(edge.x70_coDir.y)) ? 0 : 1;
                if (std::fabs(edge.x70_coDir[maxCompIdx]) < std::fabs(edge.x70_coDir.z))
                    maxCompIdx = 2;

                int ci0, ci1;
                switch (maxCompIdx)
                {
                case 0:
                    ci0 = 1;
                    ci1 = 2;
                    break;
                case 1:
                    ci0 = 0;
                    ci1 = 2;
                    break;
                default:
                    ci0 = 0;
                    ci1 = 1;
                    break;
                }

                double mag = edge.x58_start[ci0] * (clipped[ci1] - edge.x28_dir[ci1]) -
                    edge.x58_start[ci1] * (clipped[ci0] - edge.x28_dir[ci0]) /
                    edge.x58_start[ci0] * dir[ci1] - edge.x58_start[ci1] * dir[ci0];
                if (mag > 0.0 && mag < d)
                {
                    zeus::CVector3d clippedMag = clipped - mag * zeus::CVector3d(dir);
                    if ((edge.x28_dir.x - clippedMag).dot(edge.x40_end.x - clippedMag) < 0.0)
                    {
                        normal = cross0Norm.asCVector3f();
                        point = clipped.asCVector3f();
                        ret = true;
                    }
                }
            }
        }
    }

    return ret;
}

bool CMetroidAreaCollider::MovingAABoxCollisionCheck_Cached(const COctreeLeafCache& leafCache, const zeus::CAABox& aabb,
                                                            const CMaterialFilter& filter, const CMaterialList& matList,
                                                            const zeus::CVector3f& dir, float mag, CCollisionInfo& infoOut,
                                                            double& dOut)
{
    bool ret = false;
    ResetInternalCounters();
    dOut = mag;

    CMovingAABoxComponents components(aabb, dir);

    zeus::CAABox movedAABB = components.x6e8_aabb;
    zeus::CVector3f moveVec = mag * dir;
    movedAABB.accumulateBounds(aabb.min + moveVec);
    movedAABB.accumulateBounds(aabb.max + moveVec);

    zeus::CVector3f center = movedAABB.center();
    zeus::CVector3f extent = movedAABB.extents();

    zeus::CVector3f normal, point;

    for (const CAreaOctTree::Node& node : leafCache.x4_nodeCache)
    {
        if (movedAABB.intersects(node.GetBoundingBox()))
        {
            CAreaOctTree::TriListReference list = node.GetTriangleArray();
            for (int j=0 ; j<list.GetSize() ; ++j)
            {
                u16 triIdx = list.GetAt(j);
                if (g_DupPrimitiveCheckCount != g_DupTriangleList[triIdx])
                {
                    g_TrianglesProcessed += 1;
                    g_DupTriangleList[triIdx] = g_DupPrimitiveCheckCount;
                    CMaterialList triMat(node.GetOwner().GetTriangleMaterial(triIdx));
                    if (filter.Passes(triMat))
                    {
                        u16 vertIndices[3];
                        node.GetOwner().GetTriangleVertexIndices(triIdx, vertIndices);
                        CCollisionSurface surf(node.GetOwner().GetVert(vertIndices[0]),
                                               node.GetOwner().GetVert(vertIndices[1]),
                                               node.GetOwner().GetVert(vertIndices[2]),
                                               triMat.GetValue());

                        if (CollisionUtil::TriBoxOverlap(center, extent,
                                                         surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                        {
                            bool triRet = false;
                            double d = dOut;
                            if (MovingAABoxCollisionCheck_BoxVertexTri(surf, aabb, components.x6c4_vertIdxs,
                                                                       dir, d, normal, point) && d < dOut)
                            {
                                triRet = true;
                                ret = true;
                                infoOut = CCollisionInfo(point, matList, triMat, normal);
                                dOut = d;
                            }

                            for (int k=0 ; k<3 ; ++k)
                            {
                                u16 vertIdx = vertIndices[k];
                                zeus::CVector3f vtx = node.GetOwner().GetVert(vertIdx);
                                if (g_DupPrimitiveCheckCount != g_DupVertexList[vertIdx])
                                {
                                    g_DupVertexList[vertIdx] = g_DupPrimitiveCheckCount;
                                    if (movedAABB.pointInside(vtx))
                                    {
                                        d = dOut;
                                        if (MovingAABoxCollisionCheck_TriVertexBox(vtx, aabb, dir, d,
                                                                                   normal, point) && d < dOut)
                                        {
                                            CMaterialList vertMat(node.GetOwner().GetVertMaterial(vertIdx));
                                            triRet = true;
                                            ret = true;
                                            infoOut = CCollisionInfo(point, matList, vertMat, normal);
                                            dOut = d;
                                        }
                                    }
                                }
                            }

                            const u16* edgeIndices = node.GetOwner().GetTriangleEdgeIndices(triIdx);
                            for (int k=0 ; k<3 ; ++k)
                            {
                                u16 edgeIdx = edgeIndices[k];
                                if (g_DupPrimitiveCheckCount != g_DupEdgeList[edgeIdx])
                                {
                                    g_DupEdgeList[edgeIdx] = g_DupPrimitiveCheckCount;
                                    CMaterialList edgeMat(node.GetOwner().GetEdgeMaterial(edgeIdx));
                                    if (!edgeMat.HasMaterial(EMaterialTypes::TwentyFour))
                                    {
                                        d = dOut;
                                        const CCollisionEdge& edge = node.GetOwner().GetEdge(edgeIdx);
                                        if (MovingAABoxCollisionCheck_Edge(node.GetOwner().GetVert(edge.GetVertIndex1()),
                                                                           node.GetOwner().GetVert(edge.GetVertIndex2()),
                                                                           components.x0_edges, dir, d, normal, point) &&
                                            d < dOut)
                                        {
                                            triRet = true;
                                            ret = true;
                                            infoOut = CCollisionInfo(point, matList, edgeMat, normal);
                                            dOut = d;
                                        }
                                    }
                                }
                            }

                            if (triRet)
                            {
                                moveVec = float(dOut) * dir;
                                movedAABB = components.x6e8_aabb;
                                movedAABB.accumulateBounds(aabb.min + moveVec);
                                movedAABB.accumulateBounds(aabb.max + moveVec);
                                center = movedAABB.center();
                                extent = movedAABB.extents();
                            }
                        }
                        else
                        {
                            const u16* edgeIndices = node.GetOwner().GetTriangleEdgeIndices(triIdx);
                            g_DupEdgeList[edgeIndices[0]] = g_DupPrimitiveCheckCount;
                            g_DupEdgeList[edgeIndices[1]] = g_DupPrimitiveCheckCount;
                            g_DupEdgeList[edgeIndices[2]] = g_DupPrimitiveCheckCount;
                            g_DupVertexList[vertIndices[0]] = g_DupPrimitiveCheckCount;
                            g_DupVertexList[vertIndices[1]] = g_DupPrimitiveCheckCount;
                            g_DupVertexList[vertIndices[2]] = g_DupPrimitiveCheckCount;
                        }
                    }
                }
            }
        }
    }

    return ret;
}

bool CMetroidAreaCollider::MovingSphereCollisionCheck_Cached(const COctreeLeafCache& leafCache, const zeus::CAABox& aabb,
                                                             const zeus::CSphere& sphere,
                                                             const CMaterialFilter& filter, const CMaterialList& matList,
                                                             const zeus::CVector3f& dir, float mag, CCollisionInfo& infoOut,
                                                             double& dOut)
{
    bool ret = false;
    ResetInternalCounters();
    dOut = mag;

    zeus::CAABox movedAABB = aabb;
    zeus::CVector3f moveVec = mag * dir;
    movedAABB.accumulateBounds(aabb.min + moveVec);
    movedAABB.accumulateBounds(aabb.max + moveVec);

    zeus::CVector3f center = movedAABB.center();
    zeus::CVector3f extent = movedAABB.extents();

    for (const CAreaOctTree::Node& node : leafCache.x4_nodeCache)
    {
        if (movedAABB.intersects(node.GetBoundingBox()))
        {
            CAreaOctTree::TriListReference list = node.GetTriangleArray();
            for (int j=0 ; j<list.GetSize() ; ++j)
            {
                u16 triIdx = list.GetAt(j);
                if (g_DupPrimitiveCheckCount != g_DupTriangleList[triIdx])
                {
                    g_TrianglesProcessed += 1;
                    g_DupTriangleList[triIdx] = g_DupPrimitiveCheckCount;
                    CMaterialList triMat(node.GetOwner().GetTriangleMaterial(triIdx));
                    if (filter.Passes(triMat))
                    {
                        u16 vertIndices[3];
                        node.GetOwner().GetTriangleVertexIndices(triIdx, vertIndices);
                        CCollisionSurface surf(node.GetOwner().GetVert(vertIndices[0]),
                                               node.GetOwner().GetVert(vertIndices[1]),
                                               node.GetOwner().GetVert(vertIndices[2]),
                                               triMat.GetValue());

                        if (CollisionUtil::TriBoxOverlap(center, extent,
                                                         surf.GetVert(0), surf.GetVert(1), surf.GetVert(2)))
                        {
                            zeus::CVector3f surfNormal = surf.GetNormal();
                            if ((sphere.position + moveVec - surf.GetVert(0)).dot(surfNormal) <= sphere.radius)
                            {
                                bool triRet = false;

                                float mag = (sphere.radius - (sphere.position - surf.GetVert(0)).dot(surfNormal)) / dir.dot(surfNormal);
                                zeus::CVector3f intersectPoint = sphere.position + mag * dir;

                                bool outsideEdges[] =
                                {(intersectPoint - surf.GetVert(0)).dot((surf.GetVert(1) - surf.GetVert(0)).cross(surfNormal)) < 0.f,
                                 (intersectPoint - surf.GetVert(1)).dot((surf.GetVert(2) - surf.GetVert(1)).cross(surfNormal)) < 0.f,
                                 (intersectPoint - surf.GetVert(2)).dot((surf.GetVert(0) - surf.GetVert(2)).cross(surfNormal)) < 0.f};

                                if (mag >= 0.f && !outsideEdges[0] && !outsideEdges[1] && !outsideEdges[2] && mag < dOut)
                                {
                                    infoOut = CCollisionInfo(intersectPoint - sphere.radius * surfNormal, matList, triMat, surfNormal);
                                    dOut = mag;
                                    triRet = true;
                                    ret = true;
                                }

                                bool intersects = (sphere.position - surf.GetVert(0)).dot(surfNormal) <= sphere.radius;
                                bool testVert[] = {true, true, true};
                                const u16* edgeIndices = node.GetOwner().GetTriangleEdgeIndices(triIdx);
                                for (int k=0 ; k<3 ; ++k)
                                {
                                    if (intersects || outsideEdges[k])
                                    {
                                        u16 edgeIdx = edgeIndices[k];
                                        if (g_DupPrimitiveCheckCount != g_DupEdgeList[edgeIdx])
                                        {
                                            g_DupEdgeList[edgeIdx] = g_DupPrimitiveCheckCount;
                                            CMaterialList edgeMat(node.GetOwner().GetEdgeMaterial(edgeIdx));
                                            if (!edgeMat.HasMaterial(EMaterialTypes::TwentyFour))
                                            {
                                                int nextIdx = (k + 1) % 3;
                                                zeus::CVector3f edgeVec = surf.GetVert(nextIdx) - surf.GetVert(k);
                                                float edgeVecMag = edgeVec.magnitude();
                                                edgeVec *= (1.f / edgeVecMag);
                                                float dirDotEdge = dir.dot(edgeVec);
                                                zeus::CVector3f edgeRej = dir - dirDotEdge * edgeVec;
                                                float edgeRejMagSq = edgeRej.magSquared();
                                                zeus::CVector3f vertToSphere = sphere.position - surf.GetVert(k);
                                                float vtsDotEdge = vertToSphere.dot(edgeVec);
                                                zeus::CVector3f vtsRej = vertToSphere - vtsDotEdge * edgeVec;
                                                if (edgeRejMagSq > 0.f)
                                                {
                                                    float tmp = 2.f * vtsRej.dot(edgeRej);
                                                    float tmp2 = 4.f * edgeRejMagSq *
                                                        (vtsRej.magSquared() - sphere.radius * sphere.radius) - tmp * tmp;
                                                    if (tmp2 >= 0.f)
                                                    {
                                                        float mag = 0.5f / edgeRejMagSq * (-tmp - std::sqrt(tmp2));
                                                        if (mag >= 0.f)
                                                        {
                                                            float t = mag * dirDotEdge + vtsDotEdge;
                                                            if (t >= 0.f && t <= edgeVecMag && mag < dOut)
                                                            {
                                                                zeus::CVector3f point = surf.GetVert(k) + t * edgeVec;
                                                                infoOut = CCollisionInfo(point, matList, edgeMat,
                                                                                         (sphere.position + mag * dir - point).normalized());
                                                                dOut = mag;
                                                                triRet = true;
                                                                ret = true;
                                                                testVert[k] = false;
                                                                testVert[nextIdx] = false;
                                                            }
                                                            else if (t < -sphere.radius && dirDotEdge <= 0.f)
                                                            {
                                                                testVert[k] = false;
                                                            }
                                                            else if (t > edgeVecMag + sphere.radius && dirDotEdge >= 0.0)
                                                            {
                                                                testVert[nextIdx] = false;
                                                            }
                                                        }
                                                    }
                                                    else
                                                    {
                                                        testVert[k] = false;
                                                        testVert[nextIdx] = false;
                                                    }
                                                }
                                            }
                                        }
                                    }
                                }

                                for (int k=0 ; k<3 ; ++k)
                                {
                                    u16 vertIdx = vertIndices[k];
                                    if (testVert[k])
                                    {
                                        if (g_DupPrimitiveCheckCount != g_DupVertexList[vertIdx])
                                        {
                                            g_DupVertexList[vertIdx] = g_DupPrimitiveCheckCount;
                                            double d = dOut;
                                            if (CollisionUtil::RaySphereIntersection_Double(zeus::CSphere(surf.GetVert(k), sphere.radius),
                                                                                            sphere.position, dir, d) && d >= 0.0)
                                            {
                                                infoOut = CCollisionInfo(surf.GetVert(k), matList, node.GetOwner().GetVertMaterial(vertIdx),
                                                                         (sphere.position + dir * d - surf.GetVert(k)).normalized());
                                                dOut = d;
                                                triRet = true;
                                                ret = true;
                                            }
                                        }
                                    }
                                    else
                                    {
                                        g_DupVertexList[vertIdx] = g_DupPrimitiveCheckCount;
                                    }
                                }

                                if (triRet)
                                {
                                    moveVec = float(dOut) * dir;
                                    movedAABB = aabb;
                                    movedAABB.accumulateBounds(aabb.min + moveVec);
                                    movedAABB.accumulateBounds(aabb.max + moveVec);
                                    center = movedAABB.center();
                                    extent = movedAABB.extents();
                                }
                            }
                        }
                        else
                        {
                            const u16* edgeIndices = node.GetOwner().GetTriangleEdgeIndices(triIdx);
                            g_DupEdgeList[edgeIndices[0]] = g_DupPrimitiveCheckCount;
                            g_DupEdgeList[edgeIndices[1]] = g_DupPrimitiveCheckCount;
                            g_DupEdgeList[edgeIndices[2]] = g_DupPrimitiveCheckCount;
                            g_DupVertexList[vertIndices[0]] = g_DupPrimitiveCheckCount;
                            g_DupVertexList[vertIndices[1]] = g_DupPrimitiveCheckCount;
                            g_DupVertexList[vertIndices[2]] = g_DupPrimitiveCheckCount;
                        }
                    }
                }
            }
        }
    }

    return ret;
}

void CMetroidAreaCollider::ResetInternalCounters()
{
    g_CalledClip = 0;
    g_RejectedByClip = 0;
    g_TrianglesProcessed = 0;
    g_DupTrianglesProcessed = 0;
    if (g_DupPrimitiveCheckCount == 0xffff)
    {
        memset(g_DupVertexList, 0, 0x5000);
        memset(g_DupEdgeList, 0, 0xC000);
        memset(g_DupTriangleList, 0, 0x8000);
        g_DupPrimitiveCheckCount += 1;
    }
    g_DupPrimitiveCheckCount += 1;
}

void CAreaCollisionCache::ClearCache()
{
    x18_leafCaches.clear();
    x1b40_24_leafOverflow = false;
    x1b40_25_cacheOverflow = false;
}

void CAreaCollisionCache::AddOctreeLeafCache(const CMetroidAreaCollider::COctreeLeafCache& leafCache)
{
    if (!leafCache.GetNumLeaves())
        return;

    if (leafCache.HasCacheOverflowed())
        x1b40_24_leafOverflow = true;

    if (x18_leafCaches.size() < 3)
    {
        x18_leafCaches.push_back(leafCache);
    }
    else
    {
        x1b40_24_leafOverflow = true;
        x1b40_25_cacheOverflow = true;
    }
}

}