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

GAME_MOVEMENT* gmove;
CVAR* mv_gravity = var_new( "mv_gravity", 5.5f );
CVAR* mv_friction = var_new( "mv_friction", 5.f );
CVAR* mv_accelerate = var_new( "mv_accelerate", 5.5f );
CVAR* mv_airaccelerate = var_new( "mv_airaccelerate", 12.f );
CVAR* mv_wallboost = var_new( "mv_wallboost", 0 );
CVAR* mv_maxspeed = var_new( "mv_maxspeed", 3500.f );

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;
}

void gmove_start_tick() {
  PLAYER* p = gmove->pl;
  gmove->velocity = p->velocity;
  gmove->pos = p->pos;
  gmove->angle = p->rot;
  gmove->maxspeed = p->maxspeed;
  gmove->walkspeed = p->walkspeed;
  gmove->aabb.min = p->mins;
  gmove->aabb.max = p->maxs;
  gmove->bsp = gmove->game->state.map->bsp;
}

void gmove_end_tick() {
  PLAYER* p = gmove->pl;
  p->velocity = gmove->velocity;
  p->pos = gmove->pos;
}

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);

  if( var_geti( mv_wallboost ) ) {
    F32 len = vec_len( out );
    if( len > 0.f )
      out *= ( -1.f * blocked + len ) / len;
  }

  return out;
}

void gmove_check_velocity() {
  F32 maxspeed = var_getf( mv_maxspeed );

  for( U32 i = 0; i < 3; ++i ) {
    F32* v = &gmove->pl->velocity[i];
    if( isnan( *v ) ) *v = 0.f; continue;
    if( *v > maxspeed ) *v = maxspeed;
    if( *v < -maxspeed ) *v = -maxspeed;
  }
}

void gmove_start_gravity() {
  F32 gravity = gmove->pl->gravity * var_getf( mv_gravity );

  gmove->velocity.z -= gravity * 0.5f * TICK_INTERVAL;
  gmove_check_velocity();
}

void gmove_end_gravity() {
  
}

void gmove_accelerate( VEC3& wishdir, F32 wishspeed, F32 accel ) {
  F32 addspeed, accelspeed, currentspeed;
  F32 surf_friction = 1.f;

  currentspeed = vec_dot( gmove->pl->velocity, wishdir );
  addspeed = wishspeed - currentspeed;
  if( addspeed <= 0 )
    return;

  accelspeed = accel * TICK_INTERVAL * wishspeed * surf_friction;
  if( accelspeed > addspeed )
    accelspeed = addspeed;

  gmove->velocity += wishdir * accelspeed;
}

void gmove_stay_on_ground() {
  BSP_TRACE tr{};
  VEC3 start = gmove->pos;
  VEC3 end = gmove->pos;

  start.z += 2.f;
  end.z -= gmove->pl->stepsize;

  bsp_trace( &tr, gmove->bsp, gmove->aabb, start, end );
  start = tr.point;

  bsp_trace( &tr, gmove->bsp, gmove->aabb, start, end );
  if( tr.frac > 0.f && tr.frac < 1.f && !tr.startsolid && tr.normal.z >= 0.7f ) {
    F32 delta = fabs( gmove->pos.z - tr.point.z );

    if( delta > 0.5f * BSP_TRACE_EPSILON )
      gmove->pos = tr.point;
  }
}

void gmove_airaccelerate() {
  
}

void gmove_categorize_pos() {
  
}

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

  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;
}

void gmove_check_stuck( VEC3* pos ) {
  BSP_TRACE t;
  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
  if( !t.hit )
    return;

  VEC3 p1 = *pos;

  // try nudge away from wall
  *pos = p1 + t.normal * BSP_TRACE_EPSILON * 2.f;
  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
  if( !t.hit )
    return;

  // nudge +90deg away horizontal
  *pos = p1 + VEC3{ -t.normal.y, t.normal.x, t.normal.z } * BSP_TRACE_EPSILON * 2.f;
  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
  if( !t.hit )
    return;

  // nudge -90deg away horizontal
  *pos = p1 + VEC3{ t.normal.y, -t.normal.x, t.normal.z } * BSP_TRACE_EPSILON * 2.f;
  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
  if( !t.hit )
    return;

  // nudge -180deg away horizontal
  *pos = p1 + VEC3{ t.normal.y, t.normal.x, -t.normal.z } * BSP_TRACE_EPSILON * 2.f;
  bsp_trace( &t, gmove->bsp, gmove->aabb, *pos, *pos );
  if( !t.hit )
    return;

  // todo : loop over every dir from current pos and incrementally increase distance.
  // kinda like source
  *pos = p1; // restore original pos - failed to unstuck
}

F32 gmove_try_move( BSP_TRACE* t, VEC3* pos, VEC3* vel ) {
  F32        dt = TICK_INTERVAL;
  AABB       aabb = gmove->aabb;
  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
    if( vec_dot( *vel, t->normal ) < 0.f )
      *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, pos, &planes, 1.f /* todo : surface friction */ );
  }

  return dt < 0 ? 0 : dt;
}

void gmove_step_move( VEC3 dest, BSP_TRACE* tr ) {
  VEC3 endpos = dest;
  VEC3 pos = gmove->pos;
  VEC3 vel = gmove->velocity;

  VEC3 up = pos, down = pos;
  VEC3 upvel = vel, downvel = vel;

  gmove_try_move( tr, &down, &downvel );

  endpos.z += gmove->pl->stepsize + BSP_DIST_EPSILON;
  bsp_trace( tr, gmove->bsp, gmove->aabb, pos, endpos );
  if( !tr->startsolid && tr->frac > BSP_TRACE_EPSILON ) {
    up = tr->point;
  }

  gmove_try_move( tr, &up, &upvel );
  endpos = up;
  endpos.z -= (gmove->pl->stepsize + BSP_DIST_EPSILON);

  bsp_trace( tr, gmove->bsp, gmove->aabb, up, endpos );
  if( tr->normal.z < 0.7 ) { // not ground, slope
    gmove->pos = down;
    gmove->velocity = downvel;
    F32 step_dist = gmove->pos.z - pos.z;
    gmove->out_step = step_dist;
    return;
  }

  if( !tr->startsolid && tr->frac > BSP_TRACE_EPSILON ) {
    up = tr->point;
  }

  F32 updist = vec_dist( pos, up );
  F32 downdist = vec_dist( pos, down );

  gmove->out_step = gmove->pos.z - pos.z;
  if( downdist > updist ) {
    gmove->pos = down;
    gmove->velocity = downvel;
  }
  else {
    gmove->pos = up;
    gmove->velocity = upvel;
    return;
  }
}

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

  VEC3 vel = wishdir * gmove->walkspeed;
  F32 speed = vec_len( vel );
  if( speed > gmove->maxspeed ) {
    vel *= gmove->maxspeed / speed;
    speed = gmove->maxspeed;
  }

  gmove_accelerate( wishdir, speed, var_getf( mv_accelerate ) );
  speed = vec_len( vel );

  defer( gmove_check_stuck( &gmove->pos ) );
  if( speed < 1.f ) {
    gmove->velocity = {};
    return;
  }

  VEC3 dest = gmove->pos + gmove->velocity * TICK_INTERVAL;
  BSP_TRACE tr{};
  bsp_trace( &tr, gmove->bsp, gmove->aabb, gmove->pos, dest );

  defer( gmove_stay_on_ground() );
  if( !tr.hit ) {
    gmove->pos = dest;
    return;
  }

  gmove_step_move( dest, &tr );
}

void gmove_full_walk_move() {
  
}

void gmove_process_move() {
  gmove_walk_move();
}

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