#include <math.h>

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

#include "cm_trace_core_private.h"

enum {
    CM_TEMP_CAPSULE_MODEL_HANDLE = 0x1fe
};

static const vec3_t cm_trace_zero_vec = {0.0f, 0.0f, 0.0f};

void CM_Trace(trace_t *trace,
              const vec3_t start,
              const vec3_t end,
              const vec3_t mins,
              const vec3_t maxs,
              int32_t model,
              int32_t brushMask,
              qboolean capsule,
              const cmTraceSphereRecord_t *sphere)
{
    const coduo_collision_model_t *collisionModel =
        CM_ClipHandleToModel(model);
    traceWork_t traceWork;
    vec3_t offset;

    cm_checkcount++;
    Com_Memset(&traceWork, 0, sizeof(traceWork));
    traceWork.trace.fraction = trace->fraction;

    const float *traceMins = mins;
    if (traceMins == NULL) {
        traceMins = cm_trace_zero_vec;
    }

    const float *traceMaxs = maxs;
    if (traceMaxs == NULL) {
        traceMaxs = cm_trace_zero_vec;
    }

#if EMULATE_X87
    /* x87 (DLL 0x0805a9d5): each setup value is one 80-bit chain rounded to
     * its float slot (offset = (mins+maxs)*0.5f via fld;fadd;fld 0.5f;fmulp;
     * fstp, then the mins/maxs/start/end/delta fld;fsub|fadd;fstp pairs), and
     * deltaLengthSquared is an 80-bit 3-term dot rounded once. */
    for (int32_t axis = 0; axis < 3; ++axis) {
        offset[axis] = x87f_store_f32(
            x87f_mul(x87f_add(x87f_load_f32(traceMins[axis]),
                              x87f_load_f32(traceMaxs[axis])),
                     x87f_load_f32(0.5f)));
        traceWork.mins[axis] = x87f_store_f32(
            x87f_sub(x87f_load_f32(traceMins[axis]),
                     x87f_load_f32(offset[axis])));
        traceWork.maxs[axis] = x87f_store_f32(
            x87f_sub(x87f_load_f32(traceMaxs[axis]),
                     x87f_load_f32(offset[axis])));
        traceWork.start[axis] = x87f_store_f32(
            x87f_add(x87f_load_f32(start[axis]),
                     x87f_load_f32(offset[axis])));
        traceWork.end[axis] = x87f_store_f32(
            x87f_add(x87f_load_f32(end[axis]), x87f_load_f32(offset[axis])));
        traceWork.delta[axis] = x87f_store_f32(
            x87f_sub(x87f_load_f32(traceWork.end[axis]),
                     x87f_load_f32(traceWork.start[axis])));
    }

    traceWork.deltaLengthSquared = x87f_store_f32(x87f_add(
        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]))),
        x87f_mul(x87f_load_f32(traceWork.delta[2]),
                 x87f_load_f32(traceWork.delta[2]))));
#else
    for (int32_t axis = 0; axis < 3; ++axis) {
        offset[axis] = (traceMins[axis] + traceMaxs[axis]) * 0.5f;
        traceWork.mins[axis] = traceMins[axis] - offset[axis];
        traceWork.maxs[axis] = traceMaxs[axis] - offset[axis];
        traceWork.start[axis] = start[axis] + offset[axis];
        traceWork.end[axis] = end[axis] + offset[axis];
        traceWork.delta[axis] =
            traceWork.end[axis] - traceWork.start[axis];
    }

    traceWork.deltaLengthSquared =
        (traceWork.delta[0] * traceWork.delta[0]) +
        (traceWork.delta[1] * traceWork.delta[1]) +
        (traceWork.delta[2] * traceWork.delta[2]);
#endif

    if (sphere != NULL) {
        traceWork.sphere = sphere->sphere;
        traceWork.sphereExtents[0] = sphere->extents[0];
        traceWork.sphereExtents[1] = sphere->extents[1];
        traceWork.sphereExtents[2] = sphere->extents[2];
    } else {
        traceWork.sphere.use = capsule;
        if (traceWork.maxs[2] < traceWork.maxs[0]) {
            traceWork.sphere.radius = traceWork.maxs[2];
        } else {
            traceWork.sphere.radius = traceWork.maxs[0];
        }
        traceWork.sphere.halfheight = traceWork.maxs[2];
        traceWork.sphere.offset[0] = 0.0f;
        traceWork.sphere.offset[1] = 0.0f;
#if EMULATE_X87
        traceWork.sphere.offset[2] =
            x87f_store_f32(x87f_sub(x87f_load_f32(traceWork.maxs[2]),
                                    x87f_load_f32(traceWork.sphere.radius)));
#else
        traceWork.sphere.offset[2] =
            traceWork.maxs[2] - traceWork.sphere.radius;
#endif
    }

    /* 0x805ac11..0x805ac23: stock sums maxs into the (never-read) work
     * field at +0xa0 with one float rounding. */
#if EMULATE_X87
    traceWork.maxsSum = x87f_store_f32(
        x87f_add(x87f_add(x87f_load_f32(traceWork.maxs[0]),
                          x87f_load_f32(traceWork.maxs[1])),
                 x87f_load_f32(traceWork.maxs[2])));
#else
    traceWork.maxsSum =
        traceWork.maxs[0] + traceWork.maxs[1] + traceWork.maxs[2];
#endif

    traceWork.contents = brushMask;

    traceWork.offsets[0][0] = traceWork.mins[0];
    traceWork.offsets[0][1] = traceWork.mins[1];
    traceWork.offsets[0][2] = traceWork.mins[2];
    traceWork.offsets[1][0] = traceWork.maxs[0];
    traceWork.offsets[1][1] = traceWork.mins[1];
    traceWork.offsets[1][2] = traceWork.mins[2];
    traceWork.offsets[2][0] = traceWork.mins[0];
    traceWork.offsets[2][1] = traceWork.maxs[1];
    traceWork.offsets[2][2] = traceWork.mins[2];
    traceWork.offsets[3][0] = traceWork.maxs[0];
    traceWork.offsets[3][1] = traceWork.maxs[1];
    traceWork.offsets[3][2] = traceWork.mins[2];
    traceWork.offsets[4][0] = traceWork.mins[0];
    traceWork.offsets[4][1] = traceWork.mins[1];
    traceWork.offsets[4][2] = traceWork.maxs[2];
    traceWork.offsets[5][0] = traceWork.maxs[0];
    traceWork.offsets[5][1] = traceWork.mins[1];
    traceWork.offsets[5][2] = traceWork.maxs[2];
    traceWork.offsets[6][0] = traceWork.mins[0];
    traceWork.offsets[6][1] = traceWork.maxs[1];
    traceWork.offsets[6][2] = traceWork.maxs[2];
    traceWork.offsets[7][0] = traceWork.maxs[0];
    traceWork.offsets[7][1] = traceWork.maxs[1];
    traceWork.offsets[7][2] = traceWork.maxs[2];

    /*
     * These bounds drive the leaf/patch culling, so an ULP here changes which
     * brushes and patches get traced at all — not just the resulting fraction.
     * Each is one 80-bit chain rounded once to its float slot.
     */
    if (traceWork.sphere.use != 0) {
        for (int32_t axis = 0; axis < 3; ++axis) {
#if EMULATE_X87
            const float sphereOffset =
                x87f_store_f32(x87f_abs(
                    x87f_load_f32(traceWork.sphere.offset[axis])));

            if (traceWork.start[axis] < traceWork.end[axis]) {
                traceWork.bounds[0][axis] = x87f_store_f32(x87f_sub(
                    x87f_sub(x87f_load_f32(traceWork.start[axis]),
                             x87f_load_f32(sphereOffset)),
                    x87f_load_f32(traceWork.sphere.radius)));
                traceWork.bounds[1][axis] = x87f_store_f32(x87f_add(
                    x87f_add(x87f_load_f32(sphereOffset),
                             x87f_load_f32(traceWork.end[axis])),
                    x87f_load_f32(traceWork.sphere.radius)));
            } else {
                traceWork.bounds[0][axis] = x87f_store_f32(x87f_sub(
                    x87f_sub(x87f_load_f32(traceWork.end[axis]),
                             x87f_load_f32(sphereOffset)),
                    x87f_load_f32(traceWork.sphere.radius)));
                traceWork.bounds[1][axis] = x87f_store_f32(x87f_add(
                    x87f_add(x87f_load_f32(sphereOffset),
                             x87f_load_f32(traceWork.start[axis])),
                    x87f_load_f32(traceWork.sphere.radius)));
            }
#else
            /* 0x805ad8b: stock inlines the fabs here (no helper call). */
            const float sphereOffset =
                fabsf(traceWork.sphere.offset[axis]);

            if (traceWork.start[axis] < traceWork.end[axis]) {
                traceWork.bounds[0][axis] =
                    traceWork.start[axis] - sphereOffset -
                    traceWork.sphere.radius;
                traceWork.bounds[1][axis] =
                    sphereOffset + traceWork.end[axis] +
                    traceWork.sphere.radius;
            } else {
                traceWork.bounds[0][axis] =
                    traceWork.end[axis] - sphereOffset -
                    traceWork.sphere.radius;
                traceWork.bounds[1][axis] =
                    sphereOffset + traceWork.start[axis] +
                    traceWork.sphere.radius;
            }
#endif
        }
    } else {
        for (int32_t axis = 0; axis < 3; ++axis) {
#if EMULATE_X87
            if (traceWork.start[axis] < traceWork.end[axis]) {
                traceWork.bounds[0][axis] = x87f_store_f32(
                    x87f_add(x87f_load_f32(traceWork.start[axis]),
                             x87f_load_f32(traceWork.mins[axis])));
                traceWork.bounds[1][axis] = x87f_store_f32(
                    x87f_add(x87f_load_f32(traceWork.end[axis]),
                             x87f_load_f32(traceWork.maxs[axis])));
            } else {
                traceWork.bounds[0][axis] = x87f_store_f32(
                    x87f_add(x87f_load_f32(traceWork.end[axis]),
                             x87f_load_f32(traceWork.mins[axis])));
                traceWork.bounds[1][axis] = x87f_store_f32(
                    x87f_add(x87f_load_f32(traceWork.start[axis]),
                             x87f_load_f32(traceWork.maxs[axis])));
            }
#else
            if (traceWork.start[axis] < traceWork.end[axis]) {
                traceWork.bounds[0][axis] =
                    traceWork.start[axis] + traceWork.mins[axis];
                traceWork.bounds[1][axis] =
                    traceWork.end[axis] + traceWork.maxs[axis];
            } else {
                traceWork.bounds[0][axis] =
                    traceWork.end[axis] + traceWork.mins[axis];
                traceWork.bounds[1][axis] =
                    traceWork.start[axis] + traceWork.maxs[axis];
            }
#endif
        }
    }

    if (start[0] == end[0] && start[1] == end[1] && start[2] == end[2]) {
        if (model == 0) {
            CM_PositionTest(&traceWork);
        } else if (model == CM_TEMP_CAPSULE_MODEL_HANDLE) {
            if ((cm_boxBrush->contents & brushMask) != 0) {
                if (traceWork.sphere.use != 0) {
                    CM_TestCapsuleInCapsule(&traceWork);
                } else {
                    CM_TestBoundingBoxInCapsule(&traceWork);
                }
            }
        } else {
            CM_TestInLeaf(&traceWork, &collisionModel->leaf);
        }
    } else {
#if EMULATE_X87
        /* The maxs sum is formed and compared against zero in 80-bit (no float
         * store); sphereExtents = radius + |offset| is one chain per axis. */
        traceWork.isPoint =
            x87f_eq(x87f_add(x87f_add(x87f_load_f32(traceWork.maxs[0]),
                                      x87f_load_f32(traceWork.maxs[1])),
                             x87f_load_f32(traceWork.maxs[2])),
                    x87f_load_f32(0.0f));

        if (traceWork.sphere.use != 0) {
            for (int32_t axis = 0; axis < 3; ++axis) {
                traceWork.sphereExtents[axis] = x87f_store_f32(x87f_add(
                    x87f_load_f32(traceWork.sphere.radius),
                    x87f_abs(x87f_load_f32(traceWork.sphere.offset[axis]))));
            }
        }
#else
        traceWork.isPoint =
            (traceWork.maxs[0] + traceWork.maxs[1] + traceWork.maxs[2] ==
             0.0f);

        if (traceWork.sphere.use != 0) {
            for (int32_t axis = 0; axis < 3; ++axis) {
                /* 0x805b020: stock calls the CM_TraceFloatAbs helper here
                 * (exact value either way; form matches the call). */
                traceWork.sphereExtents[axis] =
                    traceWork.sphere.radius +
                    CM_TraceFloatAbs(traceWork.sphere.offset[axis]);
            }
        }
#endif

        if (model == 0) {
            CM_TraceThroughTree(&traceWork, 0, 0.0f,
                                traceWork.trace.fraction, traceWork.start,
                                traceWork.end);
        } else if (model == CM_TEMP_CAPSULE_MODEL_HANDLE) {
            if ((cm_boxBrush->contents & traceWork.contents) != 0) {
                if (traceWork.sphere.use != 0) {
                    CM_TraceCapsuleThroughCapsule(&traceWork);
                } else {
                    CM_TraceBoundingBoxThroughCapsule(&traceWork);
                }
            }
        } else {
            CM_TraceThroughLeaf(&traceWork, &collisionModel->leaf);
        }
    }

#if EMULATE_X87
    /* endpos = start + delta*fraction: one 80-bit chain per axis, rounded once. */
    for (int32_t axis = 0; axis < 3; ++axis) {
        traceWork.trace.endpos[axis] = x87f_store_f32(
            x87f_add(x87f_load_f32(start[axis]),
                     x87f_mul(x87f_load_f32(traceWork.delta[axis]),
                              x87f_load_f32(traceWork.trace.fraction))));
    }
#else
    traceWork.trace.endpos[0] =
        start[0] + traceWork.delta[0] * traceWork.trace.fraction;
    traceWork.trace.endpos[1] =
        start[1] + traceWork.delta[1] * traceWork.trace.fraction;
    traceWork.trace.endpos[2] =
        start[2] + traceWork.delta[2] * traceWork.trace.fraction;
#endif

    *trace = traceWork.trace;
}
