#include "cm_trace_core_private.h"
#include "coduo_x87emu.h" /* defines EMULATE_X87; x87 shim when it is 1 */

#include <math.h>

qboolean CM_TraceCylinderThroughCylinder(traceWork_t *traceWork,
                                         const vec3_t cylinderOrigin,
                                         float cylinderHalfHeight,
                                         float cylinderRadius)
{
    vec3_t startDelta;
    vec3_t hitNormal;

    startDelta[0] = traceWork->start[0] - cylinderOrigin[0];
    startDelta[1] = traceWork->start[1] - cylinderOrigin[1];
    startDelta[2] = traceWork->start[2] - cylinderOrigin[2];

    /* 0x8059cbe: the squared combined radius is rounded to float in its own
     * slot before the radial dot product subtracts it. */
    const float combinedRadiusSquared =
        (cylinderRadius + traceWork->sphere.radius) *
        (cylinderRadius + traceWork->sphere.radius);
    const float radialStartDistance =
        (startDelta[0] * startDelta[0] + startDelta[1] * startDelta[1]) -
        combinedRadiusSquared;

    if (radialStartDistance <= 0.0f) {
        const float expandedHalfHeight =
            traceWork->sphere.halfheight - traceWork->sphere.radius +
            cylinderHalfHeight;

        if (expandedHalfHeight < startDelta[2] ||
            startDelta[2] < -expandedHalfHeight) {
            return 1;
        }

        traceWork->trace.fraction = 0.0f;
        traceWork->trace.startsolid = 1;
        startDelta[2] = 0.0f;
        VectorNormalize2(startDelta, traceWork->trace.normal);
        traceWork->trace.contents = CM_TempBoxModelContents();

        startDelta[0] = traceWork->end[0] - cylinderOrigin[0];
        startDelta[1] = traceWork->end[1] - cylinderOrigin[1];
        startDelta[2] = traceWork->end[2] - cylinderOrigin[2];

        /*
         * ORIGINAL_BINARY_BUG: 0x8059d77..0x8059dcd tests only the end
         * point's Z span before setting allsolid.  An end point that exits the
         * expanded cylinder radially but remains within this span is therefore
         * still classified allsolid.  See
         * original-bugs/coduomp-capsule-cylinder-allsolid-radial-exit.md.
         */
        if (startDelta[2] <= expandedHalfHeight &&
            -expandedHalfHeight <= startDelta[2]) {
            traceWork->trace.allsolid = 1;
        }

        return 0;
    }

    /* 2-component radial dots kept 80-bit, stored to float; discriminant,
     * epsilonFraction, baseFraction each 80-bit one store -> shim.  hitFraction
     * is a single add of two stored floats (native). */
#if EMULATE_X87
    const float radialDeltaDot = x87f_store_f32(x87f_add(
        x87f_mul(x87f_load_f32(traceWork->delta[0]), x87f_load_f32(startDelta[0])),
        x87f_mul(x87f_load_f32(traceWork->delta[1]), x87f_load_f32(startDelta[1]))));
    if (0.0f <= radialDeltaDot) {
        return 1;
    }

    const float radialDeltaLengthSquared = x87f_store_f32(x87f_add(
        x87f_mul(x87f_load_f32(traceWork->delta[0]),
                 x87f_load_f32(traceWork->delta[0])),
        x87f_mul(x87f_load_f32(traceWork->delta[1]),
                 x87f_load_f32(traceWork->delta[1]))));
    const float discriminant = x87f_store_f32(x87f_sub(
        x87f_mul(x87f_load_f32(radialDeltaDot), x87f_load_f32(radialDeltaDot)),
        x87f_mul(x87f_load_f32(radialDeltaLengthSquared),
                 x87f_load_f32(radialStartDistance))));
    if (discriminant < 0.0f) {
        return 1;
    }

    startDelta[2] = 0.0f;
    const float startLength = VectorNormalize2(startDelta, hitNormal);
    const float epsilonFraction = x87f_store_f32(x87f_div(
        x87f_mul(x87f_load_f32(startLength), x87f_load_f32(0.125f)),
        x87f_load_f32(radialDeltaDot)));
    const float baseFraction = x87f_store_f32(x87f_div(
        x87f_sub(x87f_neg(x87f_load_f32(radialDeltaDot)),
                 x87f_load_f64(sqrt((double)discriminant))),
        x87f_load_f32(radialDeltaLengthSquared)));
    const float hitFraction = baseFraction + epsilonFraction;
#else
    const float radialDeltaDot =
        traceWork->delta[0] * startDelta[0] +
        traceWork->delta[1] * startDelta[1];
    if (0.0f <= radialDeltaDot) {
        return 1;
    }

    const float radialDeltaLengthSquared =
        traceWork->delta[0] * traceWork->delta[0] +
        traceWork->delta[1] * traceWork->delta[1];
    const float discriminant =
        radialDeltaDot * radialDeltaDot -
        radialDeltaLengthSquared * radialStartDistance;
    if (discriminant < 0.0f) {
        return 1;
    }

    startDelta[2] = 0.0f;
    const float startLength = VectorNormalize2(startDelta, hitNormal);
    const float epsilonFraction = (startLength * 0.125f) / radialDeltaDot;
    const float baseFraction =
        (-radialDeltaDot - sqrt(discriminant)) / radialDeltaLengthSquared;
    const float hitFraction = baseFraction + epsilonFraction;
#endif

    if (!(traceWork->trace.fraction > hitFraction)) {
        return 1;
    }

    const float expandedHalfHeight =
        traceWork->sphere.halfheight - traceWork->sphere.radius +
        cylinderHalfHeight;
    const float zAtHit =
        ((hitFraction - epsilonFraction) * traceWork->delta[2] +
         traceWork->start[2]) -
        cylinderOrigin[2];

    if (expandedHalfHeight < zAtHit || zAtHit < -expandedHalfHeight) {
        return 1;
    }

    if (0.0f < hitFraction) {
        traceWork->trace.fraction = hitFraction;
    } else {
        traceWork->trace.fraction = 0.0f;
    }

    traceWork->trace.normal[0] = hitNormal[0];
    traceWork->trace.normal[1] = hitNormal[1];
    traceWork->trace.normal[2] = hitNormal[2];
    traceWork->trace.contents = CM_TempBoxModelContents();

    return 0;
}
