#include "cm_patch_private.h"

enum {
    CODUO_PATCH_OUTSIDE_SUM = 1,
    CODUO_PATCH_OUTSIDE_EDGE0 = 2,
    CODUO_PATCH_OUTSIDE_EDGE1 = 4,
    CODUO_PATCH_EDGE_COUNT = 3
};

qboolean
CM_TestCapsuleInPatchCollide(traceWork_t *traceWork,
                             const coduo_patch_collide_t *patchCollide)
{
    const float radius = traceWork->sphere.radius;
    const float negativeRadius = -radius;
    const float capOffset = traceWork->sphere.halfheight - radius;
    float capDelta;

    if (patchCollide->capsuleOffsetSign == 0) {
        capDelta = capOffset + capOffset;
        traceWork->start[2] -= capOffset;
        traceWork->end[2] -= capOffset;
    } else {
        capDelta = capOffset * -2.0f;
        traceWork->start[2] += capOffset;
        traceWork->end[2] += capOffset;
    }

    for (int32_t facetIndex = 0;
         facetIndex < (int32_t)patchCollide->facetCount; ++facetIndex) {
        const coduo_patch_collision_facet_t *facet =
            &patchCollide->facets[facetIndex];
        const coduo_patch_collision_plane_t *surfacePlane =
            &facet->surfacePlane;

        const float surfaceDistance =
            ((traceWork->start[0] * surfacePlane->normal[0]) +
             (traceWork->start[1] * surfacePlane->normal[1])) +
            (traceWork->start[2] * surfacePlane->normal[2]) -
            surfacePlane->dist;

        if (!(surfaceDistance >= radius)) {
            if (surfaceDistance <= negativeRadius) {
                const float capDistance =
                    surfaceDistance + (capDelta * surfacePlane->normal[2]);

                if (!(negativeRadius >= capDistance)) {
                    const float enterOffset =
                        (negativeRadius - surfaceDistance) /
                        surfacePlane->normal[2];
                    const coduo_patch_collision_plane_t *edgePlane =
                        &facet->edgePlanes[0];
                    const float baseEdge0Distance =
                        ((traceWork->start[0] * edgePlane->normal[0]) +
                         (traceWork->start[1] * edgePlane->normal[1])) +
                        (traceWork->start[2] * edgePlane->normal[2]) -
                        edgePlane->dist;

                    edgePlane = &facet->edgePlanes[1];
                    const float baseEdge1Distance =
                        ((traceWork->start[0] * edgePlane->normal[0]) +
                         (traceWork->start[1] * edgePlane->normal[1])) +
                        (traceWork->start[2] * edgePlane->normal[2]) -
                        edgePlane->dist;

                    edgePlane = &facet->edgePlanes[0];
                    const float enterEdge0Distance =
                        baseEdge0Distance +
                        (enterOffset * edgePlane->normal[2]);
                    if (enterEdge0Distance >= 0.0f) {
                        edgePlane = &facet->edgePlanes[1];
                        const float enterEdge1Distance =
                            baseEdge1Distance +
                            (enterOffset * edgePlane->normal[2]);

                        if (enterEdge1Distance >= 0.0f &&
                            enterEdge0Distance + enterEdge1Distance <= 1.0f) {
                            return 1;
                        }
                    }

                    float leaveOffset;
                    if (radius > capDistance) {
                        leaveOffset = capDelta;
                    } else {
                        leaveOffset = (radius - surfaceDistance) /
                                      surfacePlane->normal[2];
                    }

                    edgePlane = &facet->edgePlanes[0];
                    const float leaveEdge0Distance =
                        baseEdge0Distance +
                        (leaveOffset * edgePlane->normal[2]);

                    if (!(0.0f > leaveEdge0Distance)) {
                        edgePlane = &facet->edgePlanes[1];
                        const float leaveEdge1Distance =
                            baseEdge1Distance +
                            (leaveOffset * edgePlane->normal[2]);

                        if (!(0.0f > leaveEdge1Distance) &&
                            !(leaveEdge0Distance + leaveEdge1Distance >
                              1.0f)) {
                            return 1;
                        }
                    }
                }
            } else {
                const float projectedX =
                    traceWork->start[0] -
                    (surfaceDistance * surfacePlane->normal[0]);
                const float projectedY =
                    traceWork->start[1] -
                    (surfaceDistance * surfacePlane->normal[1]);
                const float projectedZ =
                    traceWork->start[2] -
                    (surfaceDistance * surfacePlane->normal[2]);

                const coduo_patch_collision_plane_t *edgePlane =
                    &facet->edgePlanes[0];
                const float edge0Distance =
                    ((projectedX * edgePlane->normal[0]) +
                     (projectedY * edgePlane->normal[1])) +
                    (projectedZ * edgePlane->normal[2]) - edgePlane->dist;

                edgePlane = &facet->edgePlanes[1];
                const float edge1Distance =
                    ((projectedX * edgePlane->normal[0]) +
                     (projectedY * edgePlane->normal[1])) +
                    (projectedZ * edgePlane->normal[2]) - edgePlane->dist;

                uint32_t outsideFlags = 0;
                if (edge0Distance + edge1Distance > 1.0f) {
                    outsideFlags |= CODUO_PATCH_OUTSIDE_SUM;
                }
                if (edge0Distance < 0.0f) {
                    outsideFlags |= CODUO_PATCH_OUTSIDE_EDGE0;
                }
                if (edge1Distance < 0.0f) {
                    outsideFlags |= CODUO_PATCH_OUTSIDE_EDGE1;
                }

                if (outsideFlags == 0) {
                    return 1;
                }

                for (int32_t edgeIndex = 0; edgeIndex < CODUO_PATCH_EDGE_COUNT;
                     ++edgeIndex) {
                    if (((outsideFlags >> edgeIndex) & 1U) != 0) {
                        const coduo_patch_edge_cylinder_t *edgeCylinder =
                            facet->edgeCylinders[edgeIndex];

                        if (edgeCylinder != NULL) {
                            float deltaX =
                                traceWork->start[0] - edgeCylinder->origin[0];
                            float deltaY =
                                traceWork->start[1] - edgeCylinder->origin[1];
                            float deltaZ =
                                traceWork->start[2] - edgeCylinder->origin[2];
                            const float axisDistance =
                                ((deltaX * edgeCylinder->axis[0]) +
                                 (deltaY * edgeCylinder->axis[1])) +
                                (deltaZ * edgeCylinder->axis[2]);

                            if (axisDistance >= 0.0f &&
                                axisDistance <= edgeCylinder->length) {
                                deltaX -=
                                    axisDistance * edgeCylinder->axis[0];
                                deltaY -=
                                    axisDistance * edgeCylinder->axis[1];
                                deltaZ -=
                                    axisDistance * edgeCylinder->axis[2];

                                if (((deltaX * deltaX) + (deltaY * deltaY)) +
                                        (deltaZ * deltaZ) <
                                    radius * radius) {
                                    return 1;
                                }
                            }
                        }
                    } else {
                        const coduo_patch_vertex_sphere_t *vertexSphere =
                            facet->vertexSpheres[edgeIndex];

                        if (vertexSphere != NULL) {
                            const float deltaX =
                                traceWork->start[0] - vertexSphere->origin[0];
                            const float deltaY =
                                traceWork->start[1] - vertexSphere->origin[1];
                            const float deltaZ =
                                traceWork->start[2] - vertexSphere->origin[2];

                            if (((deltaX * deltaX) + (deltaY * deltaY)) +
                                    (deltaZ * deltaZ) <
                                radius * radius) {
                                return 1;
                            }
                        }
                    }
                }
            }
        }
    }

    return 0;
}
