#pragma once


namespace M3D_Internal {
    inline M3D_VECTOR M3D_PTransform(M3D_VECTOR Plane, M3D_VECTOR Rotation, M3D_VECTOR Translation) noexcept {
        M3D_VECTOR vNormal = M3D_V3Rotate(Plane, Rotation);
        M3D_VECTOR vD = M3D_V4Subtract(M3D_V4SplatW(Plane), M3D_V3Dot(vNormal, Translation));
        vNormal = M3D_V4SetW(vNormal, M3D_V4GetW(vD));

        return vNormal;
    }
}

inline void FastIntersectAxisAlignedBoxPlane(M3D_VECTOR Center, M3D_VECTOR Extents, M3D_VECTOR Plane, M3D_VECTOR& Outside, M3D_VECTOR& Inside) noexcept {
    // Compute the distance to the center of the box.
    M3D_VECTOR Dist = M3D_V4Dot(Center, Plane);

    // Project the axes of the box onto the normal of the plane.  Half the
    // length of the projection (sometime called the "radius") is equal to
    // h(u) * abs(n dot b(u))) + h(v) * abs(n dot b(v)) + h(w) * abs(n dot b(w))
    // where h(i) are extents of the box, n is the plane normal, and b(i) are the
    // axes of the box. In this case b(i) = [(1,0,0), (0,1,0), (0,0,1)].
    M3D_VECTOR Radius = M3D_V3Dot(Extents, M3D_V4Abs(Plane));

    // Outside the plane?
    Outside = M3D_V4Greater(Dist, Radius);

    // Fully inside the plane?
    Inside = M3D_V4Less(Dist, M3D_V4Negate(Radius));
}

inline void FastIntersectTrianglePlane(M3D_VECTOR V0, M3D_VECTOR V1, M3D_VECTOR V2,
        M3D_VECTOR& Plane, M3D_VECTOR& Outside, M3D_VECTOR& Inside) noexcept {
    // Plane0
    M3D_VECTOR Dist0 = M3D_V4Dot(V0, Plane);
    M3D_VECTOR Dist1 = M3D_V4Dot(V1, Plane);
    M3D_VECTOR Dist2 = M3D_V4Dot(V2, Plane);

    M3D_VECTOR MinDist = M3D_V4Min(Dist0, Dist1);
    MinDist = M3D_V4Min(MinDist, Dist2);

    M3D_VECTOR MaxDist = M3D_V4Max(Dist0, Dist1);
    MaxDist = M3D_V4Max(MaxDist, Dist2);

    M3D_VECTOR Zero = M3D_V4Zero();

    // Outside the plane?
    Outside = M3D_V4Greater(MinDist, Zero);

    // Fully inside the plane?
    Inside = M3D_V4Less(MaxDist, Zero);
}

inline M3D_ContainmentType M3D_BoundingBox::ContainedBy(M3D_VECTOR Plane0, M3D_VECTOR Plane1, M3D_VECTOR Plane2,
        M3D_VECTOR& Plane3, M3D_VECTOR& Plane4, M3D_VECTOR& Plane5) const noexcept {
    // Load the box.
    M3D_VECTOR vCenter = M3D_V4LoadF3(&Center);
    M3D_VECTOR vExtents = M3D_V4LoadF3(&Extents);

    // Set w of the center to one so we can dot4 with a plane.
    vCenter = M3D_V4SetW(vCenter, 1.0f);

    M3D_VECTOR Outside, Inside;

    // Test against each plane.
    FastIntersectAxisAlignedBoxPlane(vCenter, vExtents, Plane0, Outside, Inside);

    M3D_VECTOR AnyOutside = Outside;
    M3D_VECTOR AllInside = Inside;

    FastIntersectAxisAlignedBoxPlane(vCenter, vExtents, Plane1, Outside, Inside);
    AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
    AllInside = M3D_V4AndInt(AllInside, Inside);

    FastIntersectAxisAlignedBoxPlane(vCenter, vExtents, Plane2, Outside, Inside);
    AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
    AllInside = M3D_V4AndInt(AllInside, Inside);

    FastIntersectAxisAlignedBoxPlane(vCenter, vExtents, Plane3, Outside, Inside);
    AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
    AllInside = M3D_V4AndInt(AllInside, Inside);

    FastIntersectAxisAlignedBoxPlane(vCenter, vExtents, Plane4, Outside, Inside);
    AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
    AllInside = M3D_V4AndInt(AllInside, Inside);

    FastIntersectAxisAlignedBoxPlane(vCenter, vExtents, Plane5, Outside, Inside);
    AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
    AllInside = M3D_V4AndInt(AllInside, Inside);

    // If the box is outside any plane it is outside.
    if (M3D_V4EqualInt(AnyOutside, M3D_V4TrueInt()))
        return DISJOINT;

    // If the box is inside all planes it is inside.
    if (M3D_V4EqualInt(AllInside, M3D_V4TrueInt()))
        return CONTAINS;

    // The box is not inside all planes or outside a plane, it may intersect.
    return INTERSECTS;
}

/* -------------------------------------------------------------------------------------------------------------------------- */

inline void M3D_BoundingBox::CreateFromPoints(M3D_BoundingBox& Out, M3D_VECTOR pt1, M3D_VECTOR pt2) noexcept {
    M3D_VECTOR Min = M3D_V4Min(pt1, pt2);
    M3D_VECTOR Max = M3D_V4Max(pt1, pt2);

    M3D_V4StoreF3(&Out.Center, M3D_V4Scale(M3D_V4Add(Min, Max), 0.5f));
    M3D_V4StoreF3(&Out.Extents, M3D_V4Scale(M3D_V4Subtract(Max, Min), 0.5f));
}

inline void M3D_BoundingBox::CreateFromPoints(M3D_BoundingBox& Out, size_t Count, const M3D_F3* pPoints, size_t Stride) noexcept {
    // Find the minimum and maximum x, y, and z
    M3D_VECTOR vMin, vMax;

    vMin = vMax = M3D_V4LoadF3(pPoints);

    for (size_t i = 1; i < Count; ++i) {
        M3D_VECTOR Point = M3D_V4LoadF3(reinterpret_cast<const M3D_F3*>(reinterpret_cast<const uint8_t*>(pPoints) + i * Stride));

        vMin = M3D_V4Min(vMin, Point);
        vMax = M3D_V4Max(vMax, Point);
    }

    // Store center and extents.
    M3D_V4StoreF3(&Out.Center, M3D_V4Scale(M3D_V4Add(vMin, vMax), 0.5f));
    M3D_V4StoreF3(&Out.Extents, M3D_V4Scale(M3D_V4Subtract(vMax, vMin), 0.5f));
}

inline void M3D_BoundingBox::Transform(M3D_BoundingBox& Out, M3D_MATRIX M) const noexcept {
    // Load center and extents.
    M3D_VECTOR vCenter = M3D_V4LoadF3(&Center);
    M3D_VECTOR vExtents = M3D_V4LoadF3(&Extents);

    // Compute and transform the corners and find new min/max bounds.
    M3D_VECTOR Corner = M3D_V4MultiplyAdd(vExtents, M3D_BBoxOffset[0], vCenter);
    Corner = M3D_V3Transform(Corner, M);

    M3D_VECTOR Min, Max;
    Min = Max = Corner;

    for (size_t i = 1; i < CORNER_COUNT; ++i) {
        Corner = M3D_V4MultiplyAdd(vExtents, M3D_BBoxOffset[i], vCenter);
        Corner = M3D_V3Transform(Corner, M);

        Min = M3D_V4Min(Min, Corner);
        Max = M3D_V4Max(Max, Corner);
    }

    // Store center and extents.
    M3D_V4StoreF3(&Out.Center, M3D_V4Scale(M3D_V4Add(Min, Max), 0.5f));
    M3D_V4StoreF3(&Out.Extents, M3D_V4Scale(M3D_V4Subtract(Max, Min), 0.5f));
}

inline void M3D_BoundingBox::GetCorners(M3D_F3* Corners) const noexcept {
    M3D_VECTOR vCenter = M3D_V4LoadF3(&Center);
    M3D_VECTOR vExtents = M3D_V4LoadF3(&Extents);

    for (size_t i = 0; i < CORNER_COUNT; ++i) {
        M3D_VECTOR C = M3D_V4MultiplyAdd(vExtents, M3D_BBoxOffset[i], vCenter);
        M3D_V4StoreF3(&Corners[i], C);
    }
}

inline M3D_BoundingFrustum::M3D_BoundingFrustum(M3D_MATRIX Projection, bool rhcoords) noexcept {
    CreateFromMatrix(*this, Projection, rhcoords);
}

inline void M3D_BoundingFrustum::Transform(M3D_BoundingFrustum& Out, M3D_MATRIX M) const noexcept {
    // Load the frustum.
    M3D_VECTOR vOrigin = M3D_V4LoadF3(&Origin);
    M3D_VECTOR vOrientation = M3D_V4LoadF4(&Orientation);

    // Composite the frustum rotation and the transform rotation
    M3D_MATRIX nM;
    nM.rows[0] = M3D_V3Normalize(M.rows[0]);
    nM.rows[1] = M3D_V3Normalize(M.rows[1]);
    nM.rows[2] = M3D_V3Normalize(M.rows[2]);
    nM.rows[3] = M3D_MIdentityR3;
    M3D_VECTOR Rotation = M3D_QRotationMatrix(nM);
    vOrientation = M3D_QMultiply(vOrientation, Rotation);

    // Transform the center.
    vOrigin = M3D_V3Transform(vOrigin, M);

    // Store the frustum.
    M3D_V4StoreF3(&Out.Origin, vOrigin);
    M3D_V4StoreF4(&Out.Orientation, vOrientation);

    // Scale the near and far distances (the slopes remain the same).
    M3D_VECTOR dX = M3D_V3Dot(M.rows[0], M.rows[0]);
    M3D_VECTOR dY = M3D_V3Dot(M.rows[1], M.rows[1]);
    M3D_VECTOR dZ = M3D_V3Dot(M.rows[2], M.rows[2]);

    M3D_VECTOR d = M3D_V4Max(dX, M3D_V4Max(dY, dZ));
    float Scale = sqrtf(M3D_V4GetX(d));

    Out.Near = Near * Scale;
    Out.Far = Far * Scale;

    // Copy the slopes.
    Out.RightSlope = RightSlope;
    Out.LeftSlope = LeftSlope;
    Out.TopSlope = TopSlope;
    Out.BottomSlope = BottomSlope;
}

inline void M3D_BoundingFrustum::GetCorners(M3D_F3* Corners) const noexcept {
    // Load origin and orientation of the frustum.
    M3D_VECTOR vOrigin = M3D_V4LoadF3(&Origin);
    M3D_VECTOR vOrientation = M3D_V4LoadF4(&Orientation);

    // Build the corners of the frustum.
    M3D_VECTOR vRightTop = M3D_V4Set(RightSlope, TopSlope, 1.0f, 0.0f);
    M3D_VECTOR vRightBottom = M3D_V4Set(RightSlope, BottomSlope, 1.0f, 0.0f);
    M3D_VECTOR vLeftTop = M3D_V4Set(LeftSlope, TopSlope, 1.0f, 0.0f);
    M3D_VECTOR vLeftBottom = M3D_V4Set(LeftSlope, BottomSlope, 1.0f, 0.0f);
    M3D_VECTOR vNear = M3D_V4ReplicatePtr(&Near);
    M3D_VECTOR vFar = M3D_V4ReplicatePtr(&Far);

    // Returns 8 corners position of bounding frustum.
    //     Near    Far
    //    0----1  4----5
    //    |    |  |    |
    //    |    |  |    |
    //    3----2  7----6

    M3D_VECTOR vCorners[CORNER_COUNT];
    vCorners[0] = M3D_V4Multiply(vLeftTop, vNear);
    vCorners[1] = M3D_V4Multiply(vRightTop, vNear);
    vCorners[2] = M3D_V4Multiply(vRightBottom, vNear);
    vCorners[3] = M3D_V4Multiply(vLeftBottom, vNear);
    vCorners[4] = M3D_V4Multiply(vLeftTop, vFar);
    vCorners[5] = M3D_V4Multiply(vRightTop, vFar);
    vCorners[6] = M3D_V4Multiply(vRightBottom, vFar);
    vCorners[7] = M3D_V4Multiply(vLeftBottom, vFar);

    for (size_t i = 0; i < CORNER_COUNT; ++i) {
        M3D_VECTOR C = M3D_V4Add(M3D_V3Rotate(vCorners[i], vOrientation), vOrigin);
        M3D_V4StoreF3(&Corners[i], C);
    }
}

inline M3D_ContainmentType M3D_BoundingFrustum::Contains(const M3D_BoundingBox& box) const noexcept {
    // Load origin and orientation of the frustum.
    M3D_VECTOR vOrigin = M3D_V4LoadF3(&Origin);
    M3D_VECTOR vOrientation = M3D_V4LoadF4(&Orientation);

    // Create 6 planes (do it inline to encourage use of registers)
    M3D_VECTOR NearPlane = M3D_V4Set(0.0f, 0.0f, -1.0f, Near);
    NearPlane = M3D_Internal::M3D_PTransform(NearPlane, vOrientation, vOrigin);
    NearPlane = M3D_V3Normalize(NearPlane);

    M3D_VECTOR FarPlane = M3D_V4Set(0.0f, 0.0f, 1.0f, -Far);
    FarPlane = M3D_Internal::M3D_PTransform(FarPlane, vOrientation, vOrigin);
    FarPlane = M3D_V3Normalize(FarPlane);

    M3D_VECTOR RightPlane = M3D_V4Set(1.0f, 0.0f, -RightSlope, 0.0f);
    RightPlane = M3D_Internal::M3D_PTransform(RightPlane, vOrientation, vOrigin);
    RightPlane = M3D_V3Normalize(RightPlane);

    M3D_VECTOR LeftPlane = M3D_V4Set(-1.0f, 0.0f, LeftSlope, 0.0f);
    LeftPlane = M3D_Internal::M3D_PTransform(LeftPlane, vOrientation, vOrigin);
    LeftPlane = M3D_V3Normalize(LeftPlane);

    M3D_VECTOR TopPlane = M3D_V4Set(0.0f, 1.0f, -TopSlope, 0.0f);
    TopPlane = M3D_Internal::M3D_PTransform(TopPlane, vOrientation, vOrigin);
    TopPlane = M3D_V3Normalize(TopPlane);

    M3D_VECTOR BottomPlane = M3D_V4Set(0.0f, -1.0f, BottomSlope, 0.0f);
    BottomPlane = M3D_Internal::M3D_PTransform(BottomPlane, vOrientation, vOrigin);
    BottomPlane = M3D_V3Normalize(BottomPlane);

    return box.ContainedBy(NearPlane, FarPlane, RightPlane, LeftPlane, TopPlane, BottomPlane);
}

inline void M3D_BoundingFrustum::GetPlanes(M3D_VECTOR* NearPlane, M3D_VECTOR* FarPlane, M3D_VECTOR* RightPlane,
        M3D_VECTOR* LeftPlane, M3D_VECTOR* TopPlane, M3D_VECTOR* BottomPlane) const noexcept {
    // Load origin and orientation of the frustum.
    M3D_VECTOR vOrigin = M3D_V4LoadF3(&Origin);
    M3D_VECTOR vOrientation = M3D_V4LoadF4(&Orientation);

    if (NearPlane) {
        M3D_VECTOR vNearPlane = M3D_V4Set(0.0f, 0.0f, -1.0f, Near);
        vNearPlane = M3D_Internal::M3D_PTransform(vNearPlane, vOrientation, vOrigin);
        *NearPlane = M3D_V3Normalize(vNearPlane);
    }

    if (FarPlane) {
        M3D_VECTOR vFarPlane = M3D_V4Set(0.0f, 0.0f, 1.0f, -Far);
        vFarPlane = M3D_Internal::M3D_PTransform(vFarPlane, vOrientation, vOrigin);
        *FarPlane = M3D_V3Normalize(vFarPlane);
    }

    if (RightPlane) {
        M3D_VECTOR vRightPlane = M3D_V4Set(1.0f, 0.0f, -RightSlope, 0.0f);
        vRightPlane = M3D_Internal::M3D_PTransform(vRightPlane, vOrientation, vOrigin);
        *RightPlane = M3D_V3Normalize(vRightPlane);
    }

    if (LeftPlane) {
        M3D_VECTOR vLeftPlane = M3D_V4Set(-1.0f, 0.0f, LeftSlope, 0.0f);
        vLeftPlane = M3D_Internal::M3D_PTransform(vLeftPlane, vOrientation, vOrigin);
        *LeftPlane = M3D_V3Normalize(vLeftPlane);
    }

    if (TopPlane) {
        M3D_VECTOR vTopPlane = M3D_V4Set(0.0f, 1.0f, -TopSlope, 0.0f);
        vTopPlane = M3D_Internal::M3D_PTransform(vTopPlane, vOrientation, vOrigin);
        *TopPlane = M3D_V3Normalize(vTopPlane);
    }

    if (BottomPlane) {
        M3D_VECTOR vBottomPlane = M3D_V4Set(0.0f, -1.0f, BottomSlope, 0.0f);
        vBottomPlane = M3D_Internal::M3D_PTransform(vBottomPlane, vOrientation, vOrigin);
        *BottomPlane = M3D_V3Normalize(vBottomPlane);
    }
}

inline void M3D_BoundingFrustum::CreateFromMatrix(M3D_BoundingFrustum& Out, M3D_MATRIX Projection, bool rhcoords) noexcept {
    // Corners of the projection frustum in NDC space.
    static M3D_V4F32 NDCPoints[6] = {
        {{{1.0f, 0.0f, 1.0f, 1.0f}}},   // right (at far plane)
        {{{-1.0f, 0.0f, 1.0f, 1.0f}}},   // left
        {{{0.0f, 1.0f, 1.0f, 1.0f}}},   // top
        {{{0.0f, -1.0f, 1.0f, 1.0f}}},   // bottom

        {{{0.0f, 0.0f, 0.0f, 1.0f}}},     // near
        {{{0.0f, 0.0f, 1.0f, 1.0f}}}      // far
    };

    M3D_MATRIX matInverse = M3D_MInverse(Projection);

    // Compute the frustum corners in world space.
    M3D_VECTOR Points[6];

    for (size_t i = 0; i < 6; ++i) {
        // Transform point.
        Points[i] = M3D_V4Transform(NDCPoints[i], matInverse);
    }

    Out.Origin = M3D_F3(0.0f, 0.0f, 0.0f);
    Out.Orientation = M3D_F4(0.0f, 0.0f, 0.0f, 1.0f);

    // Compute the slopes.
    Points[0] = M3D_V4Multiply(Points[0], M3D_V4Reciprocal(M3D_V4SplatZ(Points[0])));
    Points[1] = M3D_V4Multiply(Points[1], M3D_V4Reciprocal(M3D_V4SplatZ(Points[1])));
    Points[2] = M3D_V4Multiply(Points[2], M3D_V4Reciprocal(M3D_V4SplatZ(Points[2])));
    Points[3] = M3D_V4Multiply(Points[3], M3D_V4Reciprocal(M3D_V4SplatZ(Points[3])));

    Out.RightSlope = M3D_V4GetX(Points[0]);
    Out.LeftSlope = M3D_V4GetX(Points[1]);
    Out.TopSlope = M3D_V4GetY(Points[2]);
    Out.BottomSlope = M3D_V4GetY(Points[3]);

    // Compute near and far.
    Points[4] = M3D_V4Multiply(Points[4], M3D_V4Reciprocal(M3D_V4SplatW(Points[4])));
    Points[5] = M3D_V4Multiply(Points[5], M3D_V4Reciprocal(M3D_V4SplatW(Points[5])));

    if (rhcoords) {
        Out.Near = M3D_V4GetZ(Points[5]);
        Out.Far = M3D_V4GetZ(Points[4]);
    } else {
        Out.Near = M3D_V4GetZ(Points[4]);
        Out.Far = M3D_V4GetZ(Points[5]);
    }
}

namespace M3D_TriangleTests {
    inline M3D_ContainmentType ContainedBy(
            M3D_VECTOR V0, M3D_VECTOR V1, M3D_VECTOR V2,
            M3D_VECTOR& Plane0,
            M3D_VECTOR& Plane1, M3D_VECTOR& Plane2,
            M3D_VECTOR& Plane3, M3D_VECTOR& Plane4, M3D_VECTOR& Plane5) noexcept {
        // Set w of the points to one so we can dot4 with a plane.
        M3D_VECTOR TV0 = M3D_V4SetW(V0, 1.0f);
        M3D_VECTOR TV1 = M3D_V4SetW(V1, 1.0f);
        M3D_VECTOR TV2 = M3D_V4SetW(V2, 1.0f);

        M3D_VECTOR Outside, Inside;

        // Test against each plane.
        FastIntersectTrianglePlane(TV0, TV1, TV2, Plane0, Outside, Inside);

        M3D_VECTOR AnyOutside = Outside;
        M3D_VECTOR AllInside = Inside;

        FastIntersectTrianglePlane(TV0, TV1, TV2, Plane1, Outside, Inside);
        AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
        AllInside = M3D_V4AndInt(AllInside, Inside);

        FastIntersectTrianglePlane(TV0, TV1, TV2, Plane2, Outside, Inside);
        AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
        AllInside = M3D_V4AndInt(AllInside, Inside);

        FastIntersectTrianglePlane(TV0, TV1, TV2, Plane3, Outside, Inside);
        AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
        AllInside = M3D_V4AndInt(AllInside, Inside);

        FastIntersectTrianglePlane(TV0, TV1, TV2, Plane4, Outside, Inside);
        AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
        AllInside = M3D_V4AndInt(AllInside, Inside);

        FastIntersectTrianglePlane(TV0, TV1, TV2, Plane5, Outside, Inside);
        AnyOutside = M3D_V4OrInt(AnyOutside, Outside);
        AllInside = M3D_V4AndInt(AllInside, Inside);

        // If the triangle is outside any plane it is outside.
        if (M3D_V4EqualInt(AnyOutside, M3D_V4TrueInt()))
            return DISJOINT;

        // If the triangle is inside all planes it is inside.
        if (M3D_V4EqualInt(AllInside, M3D_V4TrueInt()))
            return CONTAINS;

        // The triangle is not inside all planes or outside a plane, it may intersect.
        return INTERSECTS;
    }
}