#include <math.h>

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

#include "cm_terrain_private.h"

void CM_TraceThroughTerrainCollide(
    traceWork_t *traceWork,
    const patchCollide_t *patchCollide)
{
    if (traceWork->isPoint != 0) {
        CM_TracePointThroughTerrainCollide(traceWork, patchCollide);
        return;
    }

    const facet_t *facet = patchCollide->facets;
    for (int32_t facetIndex = 0; facetIndex < patchCollide->numFacets;
         ++facetIndex, ++facet) {
        float enterFraction = -1.0f;
        float leaveFraction = traceWork->trace.fraction;
        int32_t hitChildIndex = -1;

        const patchPlane_t *plane =
            &patchCollide->planes[facet->surfacePlane];
        vec4_t clipPlane = {plane->normal[0], plane->normal[1],
                            plane->normal[2], plane->dist};
        vec3_t start;
        vec3_t end;

        if (traceWork->sphere.use == 0) {
            const float *offset = traceWork->offsets[plane->signbits];
#if EMULATE_X87
            /* planeOffset is a 3-term dot rounded to its float slot; the
             * clipPlane[3] adjust is then its own rounded store. */
            const float planeOffset = x87f_store_f32(x87f_add(
                x87f_add(x87f_mul(x87f_load_f32(offset[0]),
                                  x87f_load_f32(clipPlane[0])),
                         x87f_mul(x87f_load_f32(offset[1]),
                                  x87f_load_f32(clipPlane[1]))),
                x87f_mul(x87f_load_f32(offset[2]),
                         x87f_load_f32(clipPlane[2]))));
            clipPlane[3] = x87f_store_f32(x87f_sub(
                x87f_load_f32(clipPlane[3]), x87f_load_f32(planeOffset)));
#else
            const float planeOffset =
                ((offset[0] * clipPlane[0]) +
                 (offset[1] * clipPlane[1])) +
                (offset[2] * clipPlane[2]);
            clipPlane[3] -= planeOffset;
#endif
            start[0] = traceWork->start[0];
            start[1] = traceWork->start[1];
            start[2] = traceWork->start[2];
            end[0] = traceWork->end[0];
            end[1] = traceWork->end[1];
            end[2] = traceWork->end[2];
        } else {
#if EMULATE_X87
            clipPlane[3] = x87f_store_f32(
                x87f_add(x87f_load_f32(clipPlane[3]),
                         x87f_load_f32(traceWork->sphere.radius)));
            /* Stock sums the sphere dot ascending -- cp0*off0 + cp1*off1
             * + cp2*off2 (VA 0x8050475..0x805049a). */
            const float sphereOffset = x87f_store_f32(x87f_add(
                x87f_add(x87f_mul(x87f_load_f32(clipPlane[0]),
                                  x87f_load_f32(traceWork->sphere.offset[0])),
                         x87f_mul(x87f_load_f32(clipPlane[1]),
                                  x87f_load_f32(traceWork->sphere.offset[1]))),
                x87f_mul(x87f_load_f32(clipPlane[2]),
                         x87f_load_f32(traceWork->sphere.offset[2]))));
#else
            clipPlane[3] += traceWork->sphere.radius;
            /* Stock sums the sphere dot ascending -- cp0*off0 + cp1*off1
             * + cp2*off2 (VA 0x8050475..0x805049a). */
            const float sphereOffset =
                ((clipPlane[0] * traceWork->sphere.offset[0]) +
                 (clipPlane[1] * traceWork->sphere.offset[1])) +
                (clipPlane[2] * traceWork->sphere.offset[2]);
#endif

            if (sphereOffset > 0.0f) {
                start[0] = traceWork->start[0] - traceWork->sphere.offset[0];
                start[1] = traceWork->start[1] - traceWork->sphere.offset[1];
                start[2] = traceWork->start[2] - traceWork->sphere.offset[2];
                end[0] = traceWork->end[0] - traceWork->sphere.offset[0];
                end[1] = traceWork->end[1] - traceWork->sphere.offset[1];
                end[2] = traceWork->end[2] - traceWork->sphere.offset[2];
            } else {
                start[0] = traceWork->start[0] + traceWork->sphere.offset[0];
                start[1] = traceWork->start[1] + traceWork->sphere.offset[1];
                start[2] = traceWork->start[2] + traceWork->sphere.offset[2];
                end[0] = traceWork->end[0] + traceWork->sphere.offset[0];
                end[1] = traceWork->end[1] + traceWork->sphere.offset[1];
                end[2] = traceWork->end[2] + traceWork->sphere.offset[2];
            }
        }

        int32_t hitPlaneChanged;
        if (CM_TerrainTraceThroughPlane(clipPlane, start, end, &enterFraction,
                                        &leaveFraction,
                                        &hitPlaneChanged) == 0) {
            continue;
        }

        vec3_t hitNormal;
        if (hitPlaneChanged != 0) {
            hitNormal[0] = clipPlane[0];
            hitNormal[1] = clipPlane[1];
            hitNormal[2] = clipPlane[2];
        }

        int32_t childIndex;
        for (childIndex = 0; childIndex < facet->numBorders;
             ++childIndex) {
            plane = &patchCollide->planes[facet->borderPlanes[childIndex]];

            if (facet->borderInward[childIndex] == 0) {
                clipPlane[0] = plane->normal[0];
                clipPlane[1] = plane->normal[1];
                clipPlane[2] = plane->normal[2];
                clipPlane[3] = plane->dist;
            } else {
                clipPlane[0] = -plane->normal[0];
                clipPlane[1] = -plane->normal[1];
                clipPlane[2] = -plane->normal[2];
                clipPlane[3] = -plane->dist;
            }

            if (traceWork->sphere.use == 0) {
                const float *offset =
                    traceWork->offsets[plane->signbits];
#if EMULATE_X87
                const float planeOffset = x87f_store_f32(x87f_add(
                    x87f_add(x87f_mul(x87f_load_f32(offset[0]),
                                      x87f_load_f32(clipPlane[0])),
                             x87f_mul(x87f_load_f32(offset[1]),
                                      x87f_load_f32(clipPlane[1]))),
                    x87f_mul(x87f_load_f32(offset[2]),
                             x87f_load_f32(clipPlane[2]))));
                clipPlane[3] = x87f_store_f32(
                    x87f_add(x87f_load_f32(clipPlane[3]),
                             x87f_abs(x87f_load_f32(planeOffset))));
#else
                const float planeOffset =
                    ((offset[0] * clipPlane[0]) +
                     (offset[1] * clipPlane[1])) +
                    (offset[2] * clipPlane[2]);
                clipPlane[3] += fabsf(planeOffset);
#endif
                start[0] = traceWork->start[0];
                start[1] = traceWork->start[1];
                start[2] = traceWork->start[2];
                end[0] = traceWork->end[0];
                end[1] = traceWork->end[1];
                end[2] = traceWork->end[2];
            } else {
#if EMULATE_X87
                clipPlane[3] = x87f_store_f32(
                    x87f_add(x87f_load_f32(clipPlane[3]),
                             x87f_load_f32(traceWork->sphere.radius)));
                /* Stock sums the sphere dot ascending -- cp0*off0 +
                 * cp1*off1 + cp2*off2 (VA 0x8050734..0x8050759). */
                const float sphereOffset = x87f_store_f32(x87f_add(
                    x87f_add(
                        x87f_mul(x87f_load_f32(clipPlane[0]),
                                 x87f_load_f32(traceWork->sphere.offset[0])),
                        x87f_mul(x87f_load_f32(clipPlane[1]),
                                 x87f_load_f32(traceWork->sphere.offset[1]))),
                    x87f_mul(x87f_load_f32(clipPlane[2]),
                             x87f_load_f32(traceWork->sphere.offset[2]))));
#else
                clipPlane[3] += traceWork->sphere.radius;
                /* Stock sums the sphere dot ascending -- cp0*off0 +
                 * cp1*off1 + cp2*off2 (VA 0x8050734..0x8050759). */
                const float sphereOffset =
                    ((clipPlane[0] * traceWork->sphere.offset[0]) +
                     (clipPlane[1] * traceWork->sphere.offset[1])) +
                    (clipPlane[2] * traceWork->sphere.offset[2]);
#endif

                if (sphereOffset > 0.0f) {
                    start[0] =
                        traceWork->start[0] - traceWork->sphere.offset[0];
                    start[1] =
                        traceWork->start[1] - traceWork->sphere.offset[1];
                    start[2] =
                        traceWork->start[2] - traceWork->sphere.offset[2];
                    end[0] = traceWork->end[0] - traceWork->sphere.offset[0];
                    end[1] = traceWork->end[1] - traceWork->sphere.offset[1];
                    end[2] = traceWork->end[2] - traceWork->sphere.offset[2];
                } else {
                    start[0] =
                        traceWork->start[0] + traceWork->sphere.offset[0];
                    start[1] =
                        traceWork->start[1] + traceWork->sphere.offset[1];
                    start[2] =
                        traceWork->start[2] + traceWork->sphere.offset[2];
                    end[0] = traceWork->end[0] + traceWork->sphere.offset[0];
                    end[1] = traceWork->end[1] + traceWork->sphere.offset[1];
                    end[2] = traceWork->end[2] + traceWork->sphere.offset[2];
                }
            }

            if (CM_TerrainTraceThroughPlane(clipPlane, start, end,
                                            &enterFraction, &leaveFraction,
                                            &hitPlaneChanged) == 0) {
                break;
            }

            if (hitPlaneChanged != 0) {
                hitChildIndex = childIndex;
                hitNormal[0] = clipPlane[0];
                hitNormal[1] = clipPlane[1];
                hitNormal[2] = clipPlane[2];
            }
        }

        if (childIndex >= facet->numBorders &&
            hitChildIndex != facet->numBorders - 1 &&
            enterFraction < leaveFraction && enterFraction >= 0.0f &&
            enterFraction < traceWork->trace.fraction) {
            if (enterFraction < 0.0f) {
                enterFraction = 0.0f;
            }

            traceWork->trace.fraction = enterFraction;
            traceWork->trace.normal[0] = hitNormal[0];
            traceWork->trace.normal[1] = hitNormal[1];
            traceWork->trace.normal[2] = hitNormal[2];
        }
    }
}
