#include "cm_trace_core_private.h"

void CM_TraceCapsuleThroughCapsule(traceWork_t *traceWork)
{
    vec3_t startOffsetMax;
    vec3_t startOffsetMin;
    vec3_t endOffsetMax;
    vec3_t endOffsetMin;
    vec3_t capsuleCenter;
    vec3_t capsuleMinsFromCenter;
    vec3_t capsuleMaxsFromCenter;
    float capsuleRadius;
    float capsuleHalfHeight;
    vec3_t topSphereOrigin;
    vec3_t bottomSphereOrigin;

    if (traceWork->bounds[0][0] > cm_boxModel.maxs[0] + 1.0f) {
        return;
    }
    if (traceWork->bounds[0][1] > cm_boxModel.maxs[1] + 1.0f) {
        return;
    }
    if (traceWork->bounds[0][2] > cm_boxModel.maxs[2] + 1.0f) {
        return;
    }
    if (cm_boxModel.mins[0] - 1.0f > traceWork->bounds[1][0]) {
        return;
    }
    if (cm_boxModel.mins[1] - 1.0f > traceWork->bounds[1][1]) {
        return;
    }
    if (cm_boxModel.mins[2] - 1.0f > traceWork->bounds[1][2]) {
        return;
    }

    startOffsetMax[0] = traceWork->start[0] + traceWork->sphere.offset[0];
    startOffsetMax[1] = traceWork->start[1] + traceWork->sphere.offset[1];
    startOffsetMax[2] = traceWork->start[2] + traceWork->sphere.offset[2];

    startOffsetMin[0] = traceWork->start[0] - traceWork->sphere.offset[0];
    startOffsetMin[1] = traceWork->start[1] - traceWork->sphere.offset[1];
    startOffsetMin[2] = traceWork->start[2] - traceWork->sphere.offset[2];

    endOffsetMax[0] = traceWork->end[0] + traceWork->sphere.offset[0];
    endOffsetMax[1] = traceWork->end[1] + traceWork->sphere.offset[1];
    endOffsetMax[2] = traceWork->end[2] + traceWork->sphere.offset[2];

    endOffsetMin[0] = traceWork->end[0] - traceWork->sphere.offset[0];
    endOffsetMin[1] = traceWork->end[1] - traceWork->sphere.offset[1];
    endOffsetMin[2] = traceWork->end[2] - traceWork->sphere.offset[2];

    for (int axis = 0; axis < 3; ++axis) {
        capsuleCenter[axis] =
            (cm_boxModel.mins[axis] + cm_boxModel.maxs[axis]) * 0.5f;
        capsuleMinsFromCenter[axis] =
            cm_boxModel.mins[axis] - capsuleCenter[axis];
        capsuleMaxsFromCenter[axis] =
            cm_boxModel.maxs[axis] - capsuleCenter[axis];
    }
    (void)capsuleMinsFromCenter;

    if (capsuleMaxsFromCenter[2] < capsuleMaxsFromCenter[0]) {
        capsuleRadius = capsuleMaxsFromCenter[2];
    } else {
        capsuleRadius = capsuleMaxsFromCenter[0];
    }

    capsuleHalfHeight = capsuleMaxsFromCenter[2] - capsuleRadius;

    topSphereOrigin[0] = capsuleCenter[0];
    topSphereOrigin[1] = capsuleCenter[1];
    topSphereOrigin[2] = capsuleCenter[2] + capsuleHalfHeight;

    bottomSphereOrigin[0] = capsuleCenter[0];
    bottomSphereOrigin[1] = capsuleCenter[1];
    bottomSphereOrigin[2] = capsuleCenter[2] - capsuleHalfHeight;

    if (topSphereOrigin[2] < startOffsetMin[2]) {
        if (CM_TraceSphereThroughSphere(traceWork, startOffsetMin, endOffsetMin,
                                       topSphereOrigin,
                                       capsuleRadius) == 0) {
            return;
        }
        if (0.0f <= traceWork->delta[2]) {
            return;
        }
    } else if (startOffsetMax[2] < bottomSphereOrigin[2]) {
        if (CM_TraceSphereThroughSphere(traceWork, startOffsetMax, endOffsetMax,
                                       bottomSphereOrigin,
                                       capsuleRadius) == 0) {
            return;
        }
        if (traceWork->delta[2] <= 0.0f) {
            return;
        }
    }

    if (CM_TraceCylinderThroughCylinder(traceWork, capsuleCenter,
                                     capsuleHalfHeight,
                                     capsuleRadius) != 0) {
        if (topSphereOrigin[2] < endOffsetMin[2]) {
            if (startOffsetMin[2] <= topSphereOrigin[2]) {
                CM_TraceSphereThroughSphere(traceWork, startOffsetMin,
                                           endOffsetMin, topSphereOrigin,
                                           capsuleRadius);
            }
        } else if ((endOffsetMax[2] < bottomSphereOrigin[2]) &&
                   (bottomSphereOrigin[2] <= startOffsetMax[2])) {
            CM_TraceSphereThroughSphere(traceWork, startOffsetMax,
                                       endOffsetMax, bottomSphereOrigin,
                                       capsuleRadius);
        }
    }
}
