#include "movement.h"
#include "../world/trace.h"
#include "../../util/math.h"
#include "../../util/string.h"
#include "../../game.h"

GAME_MOVEMENT* gmove;

void gmove_init( GAME_DATA* game ) {
  gmove = new GAME_MOVEMENT;
  gmove->game = game;
}

U8 gmove_check_valid( STR<256> from ) {
  if( !gmove->game ) {
    dlog( "%s : game not init\n", from.data );
    return 0;
  }

  if( !gmove->pl ) {
    dlog( "%s : player null\n", from.data );
    return 0;
  }

  if( !gmove->input ) {
    dlog( "%s : input null\n", from.data );
    return 0;
  }

  if( !gmove->game->state.map ) {
    dlog( "%s : no map loaded\n", from.data );
    return 0;
  }

  if( !gmove->game->state.map->bsp ) {
    dlog( "%s : no bsp loaded\n", from.data );
    return 0;
  }

  return 1;
}

void gmove_set_player( PLAYER* player ) {
  gmove->pl = player;
  gmove->input = &player->input;
}

VEC3 gmove_clip_velocity( VEC3 in, VEC3 norm, F32 overbounce ) {
  F32 blocked = vec_dot( in, norm );
  F32 backoff = blocked * overbounce;
  VEC3 out = in - norm * backoff;

  F32 adjust = vec_dot( out, norm );
  if( adjust < 0.f )
    out -= (norm * adjust);

  // uncomment to enable quake/hl1-like wallboosting
  // F32 len = vec_len( out );
  // if( len > 0.f )
  //   out *= ( -1.f * blocked + len ) / len;

  return out;
}

VEC3 gmove_clip_planes( VEC3 vel, LIST<VEC3>* planes, F32 overbounce ) {
  if( planes->size > 2 )
    vel = {};

  planes->each( fn( VEC3* p ) {
    vel = gmove_clip_velocity( vel, *p, overbounce );
  } );

  if( planes->size > 1 ) {
    for( U32 i = 0; i < planes->size; ++i ) {
      if( vec_dot( vel, planes->data[i] ) < 0.f ) {
        VEC3 dir = vec_cross( planes->data[0], planes->data[1] );
        F32 len = vec_len( dir );
        if( len > 0.00001f ) {
          dir *= 1.0f / len;
          vel = dir * vec_dot( vel, dir );
        } else {
          vel = {};
        }

        break;
      }
    }
  }

  return vel;
}

F32 gmove_try_move( BSP_TRACE* t, AABB aabb, VEC3* pos, VEC3 vel ) {
  F32 dt = TICK_INTERVAL;
  LIST<VEC3> planes;

  for( U32 bump = 0; bump < 4; ++bump ) {
    VEC3 wishmove = vel * dt;
    bsp_trace( t, gmove->game->state.map->bsp, aabb, *pos, *pos + wishmove );
    if( !t->hit ) {
      *pos = t->point;
      break;
    }

    *pos += wishmove * t->frac;
    // nudge player away from wall
    *pos += t->normal * (BSP_TRACE_EPSILON * 2.f);

    planes.push( t->normal );

    dt *= (1.f - t->frac);
    if( dt <= 0.0001f )
      break;

    vel = gmove_clip_planes( vel, &planes, 1.f /* surface friction */ );
  }

  return dt < 0 ? 0 : dt;
}

void gmove_walk_move() {
  PLAYER* p = gmove->pl;
  VEC2    move = gmove->input->move;
  F32     yawrad = m_deg2rad( p->rot.y );
  VEC3    wishdir = {
    move.x * cosf( yawrad ) - move.y * sinf( yawrad ),
    move.y * cosf( yawrad ) + move.x * sinf( yawrad ),
    0
  };

  VEC3 vel = wishdir * p->maxspeed;
  VEC3 wishmove = vel * TICK_INTERVAL;
  p->velocity = vel;

  if( vec_len( wishmove ) < BSP_TRACE_EPSILON )
    return;

  BSP_TRACE tr{};
  AABB aabb{};
  aabb.min = p->mins;
  aabb.max = p->maxs;
  VEC3 npos = p->pos;

  F32 frac = gmove_try_move( &tr, aabb, &npos, vel );
  p->velocity *= frac / TICK_INTERVAL;
  p->pos = npos;
}

void gmove_process_move() {
  gmove_walk_move();
}

void gmove_tick() {
  if( !gmove_check_valid( "gmove_tick()" ) )
    return;
  gmove_process_move();
}