#include <math.h>
#include <stdint.h>
#include <string.h>

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

enum {
    CODUO_CM_VECTOR_AXIS_COUNT = 3,
    CODUO_CM_FAST_RSQRT_MAGIC = 0x5f3759df,
};

#define CODUO_CM_FAST_RSQRT_HALF 0.5f
#define CODUO_CM_FAST_RSQRT_THREE_HALVES 1.5f

void CM_StoreLeafBrushes(cmLeafQueryWork_t *work, int32_t nodeNum)
{
    const coduo_collision_leaf_t *leaf = &cm_leafs[-1 - nodeNum];
    coduo_collision_brush_t **brushes = work->list;

    for (int32_t leafBrushIndex = 0;
         leafBrushIndex < (int32_t)leaf->numLeafBrushes; ++leafBrushIndex) {
        int32_t brushIndex =
            cm_leafbrushes[leaf->firstLeafBrush + leafBrushIndex];
        coduo_collision_brush_t *brush = &cm_brushes[brushIndex];

        if (brush->checkcount == cm_checkcount) {
            continue;
        }
        brush->checkcount = cm_checkcount;

        int32_t axis;
        for (axis = 0;
             axis < CODUO_CM_VECTOR_AXIS_COUNT &&
             !(brush->mins[axis] >= work->maxs[axis]) &&
             !(work->mins[axis] >= brush->maxs[axis]);
             ++axis) {
        }

        if (axis == CODUO_CM_VECTOR_AXIS_COUNT) {
            if (work->maxcount <= work->count) {
                work->overflowed = qtrue;
                return;
            }
            brushes[work->count] = brush;
            work->count++;
        }
    }
}

int32_t CM_BoxBrushes(const vec3_t mins, const vec3_t maxs,
                     coduo_collision_brush_t **brushes, int32_t maxcount)
{
    cm_checkcount++;

    cmLeafQueryWork_t work;
    work.count = 0;
    work.maxcount = maxcount;
    work.overflowed = qfalse;
    work.list = brushes;
    work.mins[0] = mins[0];
    work.mins[1] = mins[1];
    work.mins[2] = mins[2];
    work.maxs[0] = maxs[0];
    work.maxs[1] = maxs[1];
    work.maxs[2] = maxs[2];
    work.lastLeaf = 0;
    work.storeLeafs = CM_StoreLeafBrushes;

    CM_BoxLeafnums_r(&work, 0);
    return work.count;
}

void CM_ProjectPointOntoLine(const vec3_t point, const vec3_t linePoint,
                  const vec3_t lineDirection, vec3_t projected)
{
    vec3_t delta = {
        point[0] - linePoint[0],
        point[1] - linePoint[1],
        point[2] - linePoint[2],
    };

    projected[0] =
        linePoint[0] + (((delta[0] * lineDirection[0]) +
                         (delta[1] * lineDirection[1])) +
                        (delta[2] * lineDirection[2])) *
                           lineDirection[0];
    projected[1] =
        linePoint[1] + (((delta[0] * lineDirection[0]) +
                         (delta[1] * lineDirection[1])) +
                        (delta[2] * lineDirection[2])) *
                           lineDirection[1];
    projected[2] =
        linePoint[2] + (((delta[0] * lineDirection[0]) +
                         (delta[1] * lineDirection[1])) +
                        (delta[2] * lineDirection[2])) *
                           lineDirection[2];
}

long double CM_DistanceSquaredPointToSegment(const vec3_t point,
                                             const vec3_t start,
                                             const vec3_t end,
                                             const vec3_t direction)
{
    vec3_t projected;
    CM_ProjectPointOntoLine(point, start, direction, projected);

    int32_t axis;
    for (axis = 0;
         axis < CODUO_CM_VECTOR_AXIS_COUNT &&
         !(projected[axis] > start[axis] &&
           projected[axis] > end[axis]) &&
         !(start[axis] > projected[axis] &&
           end[axis] > projected[axis]);
         ++axis) {
    }

    float distanceSquared;
    if (axis < CODUO_CM_VECTOR_AXIS_COUNT) {
        /* 0x8057e2d: both magnitudes are fld;fsub;fabs chains compared in
         * 80-bit with no store (fabsf would round each operand) -> full-width. */
#if EMULATE_X87
        const float *endpoint =
            x87f_lt(x87f_abs(x87f_sub(x87f_load_f32(projected[axis]),
                                      x87f_load_f32(start[axis]))),
                    x87f_abs(x87f_sub(x87f_load_f32(projected[axis]),
                                      x87f_load_f32(end[axis]))))
                ? start
                : end;
#else
        const float *endpoint =
            fabsl((long double)projected[axis] - (long double)start[axis]) <
                    fabsl((long double)projected[axis] -
                          (long double)end[axis])
                ? start
                : end;
#endif
        vec3_t delta = {
            point[0] - endpoint[0],
            point[1] - endpoint[1],
            point[2] - endpoint[2],
        };
        /* delta.delta 3-mul/2-add dot kept 80-bit, one store -> shim. */
#if EMULATE_X87
        distanceSquared = x87f_store_f32(x87f_add(x87f_add(
            x87f_mul(x87f_load_f32(delta[0]), x87f_load_f32(delta[0])),
            x87f_mul(x87f_load_f32(delta[1]), x87f_load_f32(delta[1]))),
            x87f_mul(x87f_load_f32(delta[2]), x87f_load_f32(delta[2]))));
#else
        distanceSquared =
            (delta[0] * delta[0]) + (delta[1] * delta[1]) +
            (delta[2] * delta[2]);
#endif
    } else {
        vec3_t delta = {
            point[0] - projected[0],
            point[1] - projected[1],
            point[2] - projected[2],
        };
#if EMULATE_X87
        distanceSquared = x87f_store_f32(x87f_add(x87f_add(
            x87f_mul(x87f_load_f32(delta[0]), x87f_load_f32(delta[0])),
            x87f_mul(x87f_load_f32(delta[1]), x87f_load_f32(delta[1]))),
            x87f_mul(x87f_load_f32(delta[2]), x87f_load_f32(delta[2]))));
#else
        distanceSquared =
            (delta[0] * delta[0]) + (delta[1] * delta[1]) +
            (delta[2] * delta[2]);
#endif
    }

    return distanceSquared;
}

/* NOTE: long double return can't carry 80 bits on arm64; no engine callers keep
 * it in an 80-bit chain (none in-tree), so left un-emulated.  A caller that does
 * would need a CM_DistanceSquaredX87 -> x87f variant (cf. ItemRandom*X87). */
long double CM_DistanceSquared(const vec3_t start, const vec3_t end)
{
    float dx = end[0] - start[0];
    float dy = end[1] - start[1];
    float dz = end[2] - start[2];

    return (((long double)dx * (long double)dx) +
            ((long double)dy * (long double)dy)) +
           ((long double)dz * (long double)dz);
}

long double CM_FastSqrt(float value)
{
    float half = value * CODUO_CM_FAST_RSQRT_HALF;
    uint32_t bits;
    float inverseRoot;

    memcpy(&bits, &value, sizeof(bits));
    bits = CODUO_CM_FAST_RSQRT_MAGIC -
           ((bits >> 1) | (bits & 0x80000000U));
    memcpy(&inverseRoot, &bits, sizeof(inverseRoot));

    inverseRoot = inverseRoot * (CODUO_CM_FAST_RSQRT_THREE_HALVES -
                                half * inverseRoot * inverseRoot);
    inverseRoot = inverseRoot * (CODUO_CM_FAST_RSQRT_THREE_HALVES -
                                half * inverseRoot * inverseRoot);

    return (long double)value * (long double)inverseRoot;
}
