#include "cm_trace_core_private.h"

void CM_TestCapsuleInCapsule(traceWork_t *traceWork)
{
    vec3_t startOffsetMax;
    vec3_t startOffsetMin;
    vec3_t boxCenter;
    vec3_t boxMinsFromCenter;
    vec3_t boxMaxsFromCenter;
    float lowerZFromCenter;
    float verticalSpan;
    float radiusSquared;
    vec3_t tempPoint;
    vec3_t delta;
    float startZDelta;
    float cylinderHalfHeight;

    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];

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

    if (boxMaxsFromCenter[0] > boxMaxsFromCenter[2]) {
        lowerZFromCenter = boxMaxsFromCenter[2];
    } else {
        lowerZFromCenter = boxMaxsFromCenter[0];
    }

    verticalSpan = boxMaxsFromCenter[2] - lowerZFromCenter;
    radiusSquared = (traceWork->sphere.radius + lowerZFromCenter) *
                    (traceWork->sphere.radius + lowerZFromCenter);

    tempPoint[0] = boxCenter[0];
    tempPoint[1] = boxCenter[1];
    tempPoint[2] = boxCenter[2] + verticalSpan;

    delta[0] = tempPoint[0] - startOffsetMax[0];
    delta[1] = tempPoint[1] - startOffsetMax[1];
    delta[2] = tempPoint[2] - startOffsetMax[2];
    if ((delta[0] * delta[0]) + (delta[1] * delta[1]) +
            (delta[2] * delta[2]) <
        radiusSquared) {
        traceWork->trace.allsolid = 1;
        traceWork->trace.startsolid = 1;
        traceWork->trace.fraction = 0.0f;
    }

    delta[0] = tempPoint[0] - startOffsetMin[0];
    delta[1] = tempPoint[1] - startOffsetMin[1];
    delta[2] = tempPoint[2] - startOffsetMin[2];
    if ((delta[0] * delta[0]) + (delta[1] * delta[1]) +
            (delta[2] * delta[2]) <
        radiusSquared) {
        traceWork->trace.allsolid = 1;
        traceWork->trace.startsolid = 1;
        traceWork->trace.fraction = 0.0f;
    }

    tempPoint[0] = boxCenter[0];
    tempPoint[1] = boxCenter[1];
    tempPoint[2] = boxCenter[2] - verticalSpan;

    delta[0] = tempPoint[0] - startOffsetMax[0];
    delta[1] = tempPoint[1] - startOffsetMax[1];
    delta[2] = tempPoint[2] - startOffsetMax[2];
    if ((delta[0] * delta[0]) + (delta[1] * delta[1]) +
            (delta[2] * delta[2]) <
        radiusSquared) {
        traceWork->trace.allsolid = 1;
        traceWork->trace.startsolid = 1;
        traceWork->trace.fraction = 0.0f;
    }

    delta[0] = tempPoint[0] - startOffsetMin[0];
    delta[1] = tempPoint[1] - startOffsetMin[1];
    delta[2] = tempPoint[2] - startOffsetMin[2];
    if ((delta[0] * delta[0]) + (delta[1] * delta[1]) +
            (delta[2] * delta[2]) <
        radiusSquared) {
        traceWork->trace.allsolid = 1;
        traceWork->trace.startsolid = 1;
        traceWork->trace.fraction = 0.0f;
    }

    startZDelta = traceWork->start[2] - boxCenter[2];
    cylinderHalfHeight =
        verticalSpan + traceWork->sphere.halfheight - traceWork->sphere.radius;
    if ((startZDelta <= cylinderHalfHeight) &&
        (-cylinderHalfHeight <= startZDelta)) {
        tempPoint[2] = 0.0f;
        startOffsetMax[2] = 0.0f;

        delta[0] = startOffsetMax[0] - tempPoint[0];
        delta[1] = startOffsetMax[1] - tempPoint[1];
        delta[2] = startOffsetMax[2] - tempPoint[2];
        if ((delta[0] * delta[0]) + (delta[1] * delta[1]) +
                (delta[2] * delta[2]) <
            radiusSquared) {
            traceWork->trace.allsolid = 1;
            traceWork->trace.startsolid = 1;
            traceWork->trace.fraction = 0.0f;
        }
    }
}
