/*
===========================================================================
Copyright (C) 2023 the OpenMoHAA team

This file is part of OpenMoHAA source code.

OpenMoHAA source code is free software; you can redistribute it
and/or modify it under the terms of the GNU General Public License as
published by the Free Software Foundation; either version 2 of the License,
or (at your option) any later version.

OpenMoHAA source code is distributed in the hope that it will be
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with OpenMoHAA source code; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
===========================================================================
*/

// g_mmove.cpp : AI/Path movement code.
//

#include "g_local.h"
#include "entity.h"
#include "game.h"

typedef struct {
    qboolean validGroundTrace;
    trace_t  groundTrace;
    float    previous_origin[3];
    float    previous_velocity[3];
} mml_t;

mmove_t *mm;
mml_t    mml;

void MM_ClipVelocity(float *in, float *normal, float *out, float overbounce)
{
    float backoff;
    float dir_z;
    float normal2[3];

    if (normal[2] >= MIN_WALK_NORMAL) {
        if (in[0] == 0.0f && in[1] == 0.0f) {
            VectorClear(out);
            return;
        }

        normal2[0] = in[0] * DotProduct2D(in, normal);
        normal2[1] = in[1] * DotProduct2D(in, normal);
        normal2[2] = normal[2] * DotProduct2D(in, in);

        VectorNormalize(normal2);

        dir_z = -normal2[2];

        out[0] = in[0];
        out[1] = in[1];
        out[2] = DotProduct2D(in, normal2) / dir_z;
    } else {
        backoff = DotProduct(in, normal);

        if (backoff < 0) {
            backoff *= overbounce;
        } else {
            backoff /= overbounce;
        }

        out[0] = in[0] - normal[0] * backoff;
        out[1] = in[1] - normal[1] * backoff;
        out[2] = in[2] - normal[2] * backoff;
    }
}

qboolean MM_AddTouchEnt(int entityNum)
{
    int      i;
    qboolean blockEnt;
    Entity  *ent;

    if (entityNum == ENTITYNUM_NONE || entityNum == ENTITYNUM_WORLD) {
        return qtrue;
    }

    ent = G_GetEntity(entityNum);

    blockEnt = ent->BlocksAIMovement();

    if (!blockEnt) {
        if (ent->IsSubclassOfPlayer()) {
            mm->hit_temp_obstacle |= 1;
        } else if (ent->IsSubclassOfDoor()) {
            mm->hit_temp_obstacle |= 2;
        }
    }

    if (mm->numtouch == MAXTOUCH) {
        return blockEnt;
    }

    // see if it is already added
    for (i = 0; i < mm->numtouch; i++) {
        if (mm->touchents[i] == entityNum) {
            return blockEnt;
        }
    }

    // add it
    mm->touchents[mm->numtouch] = entityNum;
    mm->numtouch++;

    return blockEnt;
}

qboolean MM_SlideMove(qboolean gravity)
{
    int      bumpcount;
    vec3_t   dir;
    float    d;
    int      numplanes;
    vec3_t   planes[5];
    vec3_t   clipVelocity;
    int      i;
    int      j;
    int      k;
    trace_t  trace;
    vec3_t   end;
    float    time_left;
    qboolean bBlockEnt;

    if (gravity) {
        mm->velocity[2] = mm->velocity[2] - mm->frametime * sv_gravity->integer;
        if (mm->groundPlane) {
            MM_ClipVelocity(mm->velocity, mm->groundPlaneNormal, mm->velocity, OVERCLIP);
        }
    }

    time_left = mm->frametime;

    if (mm->groundPlane) {
        numplanes = 1;
        VectorCopy(mm->groundPlaneNormal, planes[0]);
    } else {
        numplanes = 0;
    }

    // never turn against original velocity
    VectorNormalize2(mm->velocity, planes[numplanes]);
    numplanes++;

    for (bumpcount = 0; bumpcount < 4; bumpcount++) {
        // calculate position we are trying to move to
        VectorMA(mm->origin, time_left, mm->velocity, end);

        // see if we can make it there
        gi.trace(&trace, mm->origin, mm->mins, mm->maxs, end, mm->entityNum, mm->tracemask, qtrue, qfalse);

        if (trace.allsolid) {
            break;
        }

        if (trace.fraction > 0) {
            // actually covered some distance
            VectorCopy(trace.endpos, mm->origin);
        }

        if (trace.fraction == 1) {
            return bumpcount != 0;
        }

        // save entity for contact
        bBlockEnt = MM_AddTouchEnt(trace.entityNum);

        if (trace.plane.normal[2] < MIN_WALK_NORMAL) {
            if (trace.plane.normal[2] > -0.999f && bBlockEnt && mm->groundPlane) {
                if (!mm->hit_obstacle) {
                    mm->hit_obstacle = true;
                    VectorCopy(mm->origin, mm->hit_origin);
                }

                VectorAdd(mm->obstacle_normal, trace.plane.normal, mm->obstacle_normal);
            }
        } else {
            memcpy(&mml.groundTrace, &trace, sizeof(mml.groundTrace));
            mml.validGroundTrace = true;
        }

        time_left -= time_left * trace.fraction;

        if (numplanes >= MAX_CLIP_PLANES) {
            VectorClear(mm->velocity);
            return qtrue;
        }

        //
        // if this is the same plane we hit before, nudge velocity
        // out along it, which fixes some epsilon issues with
        // non-axial planes
        //
        for (i = 0; i < numplanes; i++) {
            if (DotProduct(trace.plane.normal, planes[i]) > 0.99) {
                VectorAdd(trace.plane.normal, mm->velocity, mm->velocity);
                break;
            }
        }

        if (i >= numplanes) {
            //
            // modify velocity so it parallels all of the clip planes
            //
            VectorCopy(trace.plane.normal, planes[numplanes]);
            numplanes++;

            // find a plane that it enters
            for (i = 0; i < numplanes; i++) {
                if (DotProduct(mm->velocity, planes[i]) >= 0.1) {
                    continue; // move doesn't interact with the plane
                }

                // slide along the plane
                MM_ClipVelocity(mm->velocity, planes[i], clipVelocity, OVERCLIP);

                // see if there is a second plane that the new move enters
                for (j = 0; j < numplanes; j++) {
                    if (j == i) {
                        continue;
                    }

                    if (DotProduct(clipVelocity, planes[j]) >= 0.1) { 
                        continue; // move doesn't interact with the plane
                    }

                    // slide along the plane
                    MM_ClipVelocity(clipVelocity, planes[j], clipVelocity, OVERCLIP);

                    if (DotProduct(clipVelocity, planes[i]) >= 0) {
                        continue; // move doesn't interact with the plane
                    }

                    // slide the original velocity along the crease
                    CrossProduct(planes[i], planes[j], dir);
                    VectorNormalize(dir);
                    d = DotProduct(dir, mm->velocity);
                    VectorScale(dir, d, clipVelocity);

                    // see if there is a third plane the the new move enters
                    for (k = 0; k < numplanes; k++) {
                        if (k == i || k == j) {
                            continue;
                        }

                        if (DotProduct(clipVelocity, planes[k]) >= 0.1) {
                            continue; // move doesn't interact with the plane
                        }

                        // stop dead at a tripple plane interaction
                        VectorClear(mm->velocity);
                        return qtrue;
                    }
                }

                // if we have fixed all interactions, try another move
                VectorCopy(clipVelocity, mm->velocity);
                break;
            }
        }
    }

    if (mm->velocity[0] || mm->velocity[1]) {
        if (mm->groundPlane) {
            VectorCopy(mm->velocity, dir);
            VectorNegate(dir, dir);
            VectorNormalize(dir);

            if (MM_AddTouchEnt(trace.entityNum)) {
                if (!mm->hit_obstacle) {
                    mm->hit_obstacle = true;
                    VectorCopy(mm->origin, mm->hit_origin);
                }

                VectorAdd(mm->obstacle_normal, dir, mm->obstacle_normal);
            }
        }

        VectorClear(mm->velocity);
        return true;
    }

    mm->velocity[2] = 0;
    return false;
}

void MM_GroundTraceInternal(void)
{
    if (mml.groundTrace.fraction == 1.0f) {
        mm->groundPlane = qfalse;
        mm->walking     = qfalse;
        return;
    }

    if (mm->velocity[2] > 0.0f) {
        if (DotProduct(mm->velocity, mml.groundTrace.plane.normal) > 10.0f) {
            mm->groundPlane = qfalse;
            mm->walking     = qfalse;
            return;
        }
    }

    // slopes that are too steep will not be considered onground
    if (mml.groundTrace.plane.normal[2] < MIN_WALK_NORMAL) {
        vec3_t oldvel;
        float  d;

        VectorCopy(mm->velocity, oldvel);
        VectorSet(mm->velocity, 0, 0, -1.0f / mm->frametime);
        MM_SlideMove(qfalse);

        d = VectorLength(mm->velocity);
        VectorCopy(oldvel, mm->velocity);

        if (d > (0.1f / mm->frametime)) {
            mm->groundPlane = qtrue;
            mm->walking     = qfalse;
            VectorCopy(mml.groundTrace.plane.normal, mm->groundPlaneNormal);
            return;
        }
    }

    mm->groundPlane = qtrue;
    mm->walking     = qtrue;
    VectorCopy(mml.groundTrace.plane.normal, mm->groundPlaneNormal);

    MM_AddTouchEnt(mml.groundTrace.entityNum);
}

void MM_GroundTrace(void)
{
    float point[3];

    point[0] = mm->origin[0];
    point[1] = mm->origin[1];
    point[2] = mm->origin[2] - 0.25f;

    gi.trace(&mml.groundTrace, mm->origin, mm->mins, mm->maxs, point, mm->entityNum, mm->tracemask, qtrue, qfalse);
    MM_GroundTraceInternal();
}

void MM_StepSlideMove(void)
{
    vec3_t   start_o;
    vec3_t   start_v;
    vec3_t   nostep_o;
    vec3_t   nostep_v;
    trace_t  trace;
    qboolean bWasOnGoodGround;
    vec3_t   up;
    vec3_t   down;
    qboolean start_hit_wall;
    vec3_t   start_wall_normal;
    qboolean first_hit_wall;
    vec3_t   first_wall_normal;
    vec3_t   start_hit_origin;
    vec3_t   first_hit_origin;
    trace_t  nostep_groundTrace;
    qboolean nostep_validGroundTrace;

    VectorCopy(mm->origin, start_o);
    VectorCopy(mm->velocity, start_v);
    start_hit_wall = mm->hit_obstacle;
    VectorCopy(mm->hit_origin, start_hit_origin);
    VectorCopy(mm->obstacle_normal, start_wall_normal);

    if (MM_SlideMove(qtrue) == 0) {
        if (mml.validGroundTrace) {
            MM_GroundTraceInternal();
        } else {
            MM_GroundTrace();
        }

        return;
    }

    VectorCopy(start_o, down);
    down[2] -= STEPSIZE;
    gi.trace(&trace, start_o, mm->mins, mm->maxs, down, mm->entityNum, mm->tracemask, qtrue, qfalse);
    VectorSet(up, 0, 0, 1);

    // never step up when you still have up velocity
    if (mm->velocity[2] > 0 && (trace.fraction == 1.0f || DotProduct(trace.plane.normal, up) < MIN_WALK_NORMAL)) {
        if (mml.validGroundTrace) {
            MM_GroundTraceInternal();
        } else {
            MM_GroundTrace();
        }

        return;
    }

    if (mm->groundPlane && mm->groundPlaneNormal[2] >= MIN_WALK_NORMAL) {
        bWasOnGoodGround = qtrue;
    } else {
        bWasOnGoodGround = qfalse;
    }

    VectorCopy(start_o, up);
    up[2] += 18;

    VectorCopy(mm->origin, nostep_o);
    VectorCopy(mm->velocity, nostep_v);
    memcpy(&nostep_groundTrace, &mml.groundTrace, sizeof(trace_t));
    // Fixed in OPM
    //  Save the valid ground trace
    nostep_validGroundTrace = mml.validGroundTrace;

    VectorCopy(up, mm->origin);
    VectorCopy(start_v, mm->velocity);

    first_hit_wall = mm->hit_obstacle;
    VectorCopy(mm->hit_origin, first_hit_origin);
    VectorCopy(mm->obstacle_normal, first_wall_normal);

    mm->hit_obstacle = start_hit_wall;
    VectorCopy(start_hit_origin, mm->hit_origin);
    VectorCopy(start_wall_normal, mm->obstacle_normal);
    MM_SlideMove(qtrue);

    VectorCopy(mm->origin, down);
    down[2] -= STEPSIZE * 2;

    // test the player position if they were a stepheight higher
    gi.trace(&trace, mm->origin, mm->mins, mm->maxs, down, mm->entityNum, mm->tracemask, qtrue, qfalse);
    if (trace.entityNum != ENTITYNUM_WORLD && trace.entityNum != ENTITYNUM_NONE) {
        VectorCopy(nostep_o, mm->origin);
        VectorCopy(nostep_v, mm->velocity);
        memcpy(&mml.groundTrace, &nostep_groundTrace, sizeof(mml.groundTrace));
        // Fixed in OPM
        //  Do not use the ground trace as it's invalid and would have an invalid entity number
        mml.validGroundTrace = nostep_validGroundTrace;

        mm->hit_obstacle = first_hit_wall;
        VectorCopy(first_hit_origin, mm->hit_origin);
        VectorCopy(first_wall_normal, mm->obstacle_normal);

        if (mml.validGroundTrace) {
            MM_GroundTraceInternal();
        } else {
            MM_GroundTrace();
        }

        return;
    }

    if (!trace.allsolid) {
        memcpy(&mml.groundTrace, &trace, sizeof(mml.groundTrace));
        mml.validGroundTrace = qtrue;

        if (bWasOnGoodGround && trace.fraction < 1 && trace.plane.normal[2] < MIN_WALK_NORMAL) {
            VectorCopy(nostep_o, mm->origin);
            VectorCopy(nostep_v, mm->velocity);

            if (first_hit_wall) {
                mm->hit_obstacle = first_hit_wall;
                VectorCopy(first_hit_origin, mm->hit_origin);
                VectorCopy(first_wall_normal, mm->obstacle_normal);
            }

            MM_GroundTraceInternal();
            return;
        }

        VectorCopy(trace.endpos, mm->origin);
    }

    if (trace.fraction < 1) {
        MM_ClipVelocity(mm->velocity, trace.plane.normal, mm->velocity, OVERCLIP);
    }

    if (mml.validGroundTrace) {
        MM_GroundTraceInternal();
    } else {
        MM_GroundTrace();
    }
}

void MM_ClipVelocity2D(float *in, float *normal, float *out, float overbounce)
{
    float backoff;
    float dir_z;
    float normal2[3];

    if (normal[2] >= MIN_WALK_NORMAL) {
        if (in[0] == 0.0f && in[1] == 0.0f) {
            VectorClear(out);
            return;
        }

        normal2[0] = in[0] * DotProduct2D(in, normal);
        normal2[1] = in[1] * DotProduct2D(in, normal);
        normal2[2] = normal[2] * DotProduct2D(in, in);

        VectorNormalize(normal2);

        dir_z = -normal2[2];

        out[0] = in[0];
        out[1] = in[1];
        out[2] = DotProduct2D(in, normal2) / dir_z;
    } else {
        backoff = DotProduct2D(in, normal);

        if (backoff < 0) {
            backoff *= overbounce;
        } else {
            backoff /= overbounce;
        }

        out[0] = in[0] - normal[0] * backoff;
        out[1] = in[1] - normal[1] * backoff;
        out[2] = -(backoff * normal[2]);
    }
}

void MmoveSingle(mmove_t *mmove)
{
    float   point[3];
    trace_t trace;

    mm = mmove;

    mmove->numtouch  = 0;
    mm->hit_obstacle = false;
    VectorCopy(vec3_origin, mm->obstacle_normal);
    mm->hit_temp_obstacle = false;

    memset(&mml, 0, sizeof(mml_t));

    VectorCopy(mm->origin, mml.previous_origin);
    VectorCopy(mm->velocity, mml.previous_velocity);

    if (mm->walking) {
        if (mm->desired_speed < 1.0f) {
            VectorClear2D(mm->velocity);
            MM_GroundTrace();
            return;
        }

        vec3_t wishdir;

        MM_ClipVelocity2D(mm->desired_dir, mm->groundPlaneNormal, wishdir, OVERCLIP);
        VectorNormalize(wishdir);

        mm->velocity[0] = mm->desired_speed * wishdir[0];
        mm->velocity[1] = mm->desired_speed * wishdir[1];
    } else if (mm->groundPlane) {
        MM_ClipVelocity(mm->velocity, mm->groundPlaneNormal, mm->velocity, OVERCLIP);
    }

    MM_StepSlideMove();

    if (!mm->walking && mml.previous_velocity[2] >= 0.0f && mm->velocity[2] <= 0.0f) {
        point[0] = mm->origin[0];
        point[1] = mm->origin[1];
        point[2] = mm->origin[2] - 18.0f;

        gi.trace(&trace, mm->origin, mm->mins, mm->maxs, point, mm->entityNum, mm->tracemask, qtrue, qfalse);

        if (trace.fraction < 1.0f && !trace.allsolid) {
            VectorCopy(trace.endpos, mm->origin);
            MM_GroundTrace();
        }
    }
}