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

GAME_MOVEMENT* gmove;
CVAR* mv_gravity = var_new( "mv_gravity", 800.f );
CVAR* mv_friction = var_new( "mv_friction", 4.f );
CVAR* mv_accelerate = var_new( "mv_accelerate", 5.f );
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 );
CVAR* mv_stopspeed = var_new( "mv_stopspeed", 80.f );
CVAR* mv_jump_impulse = var_new( "mv_jump_impulse", 301.993377f ); // csgo - sqrt(2 * 800 * 57 - jump height)

const U32 NON_JUMP_VELOCITY = 140.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->ground = p->ground_ent;
  gmove->bsp = gmove->game->state.map->bsp;
}

void gmove_end_tick() {
  PLAYER* p = gmove->pl;
  p->velocity = gmove->velocity;
  p->pos = gmove->pos;
  p->ground_ent = gmove->ground;
  if( p->ground_ent )
    p->flags |= PFL_ONGROUND;
  else
    p->flags &= ~PFL_ONGROUND;
}


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() {
  F32 gravity = gmove->pl->gravity * var_getf( mv_gravity );

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

U8 gmove_touch_ground( BSP_TRACE* tr, VEC3 start, VEC3 end ) {
  return bsp_trace( tr, gmove->bsp, gmove->aabb, start, end );
}

U8 gmove_touch_ground_quadrants( BSP_TRACE* tr, VEC3 start, VEC3 end ) {
  F32 frac = tr->frac;
  VEC3 point = tr->point;
  U8 hit = tr->hit;

  VEC3 mins, maxs;
  AABB aabb = gmove->aabb;

  defer( {
    tr->frac = frac;
    tr->point = point;
    tr->hit = hit;
  } );

  mins = aabb.min;
  maxs = { min( 0.f, aabb.max.x ), min( 0.f, aabb.max.y ), aabb.max.z };
  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
  if( tr->hit && tr->normal.z > 0.7f )
    return 1;

  mins = { max( 0.f, aabb.min.x ), max( 0.f, aabb.min.y ), aabb.min.z };
  maxs = aabb.max;
  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
  if( tr->hit && tr->normal.z > 0.7f )
    return 1;

  mins = { aabb.min.x, max( 0.f, aabb.min.y ), aabb.min.z };
  maxs = { min( 0.f, aabb.max.x ), aabb.max.y, aabb.max.z };
  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
  if( tr->hit && tr->normal.z > 0.7f )
    return 1;

  mins = { max( 0.f, aabb.min.x ), aabb.min.y, aabb.min.z };
  maxs = { aabb.max.x, min( 0.f, aabb.max.y ), aabb.max.z };
  bsp_trace( tr, gmove->bsp, AABB{ mins, maxs }, start, end );
  if( tr->hit && tr->normal.z > 0.7f )
    return 1;

  return 0;
}

void gmove_categorize_pos() {
  gmove->pl->surf_friction = 1.f;
  F32 offset = 2.f;
  BSP_TRACE tr{};

  VEC3 point = gmove->pos;
  point.z -= offset;

  VEC3 bump = gmove->pos;

  F32 zvel = gmove->velocity.z;
  U8 moveup_rapid = zvel > NON_JUMP_VELOCITY;

  // todo : ladders

  if( moveup_rapid ) {
    gmove->ground = 0;
  } else {
    gmove_touch_ground( &tr, bump, point );
    if( !tr.hit || tr.normal.z < 0.7f ) {
      gmove_touch_ground_quadrants( &tr, bump, point );
      if( !tr.hit || tr.normal.z < 0.7f ) {
        gmove->ground = 0;

        if( gmove->velocity.z > 0.f ) {
          gmove->pl->surf_friction = 0.25f;
        }
      } else {
        // todo : set actual ground entity when we get entity traces
        gmove->ground = objl->world;
      }
    } else {
      gmove->ground = objl->world;
    }
  }
}

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

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;
          F32 d = vec_dot( vel, dir );
          vel = dir * d;
        } else {
          vel = {};
        }

        break;
      }
    }
  }

  return vel;
}

F32 gmove_try_move( BSP_TRACE* t, VEC3* pos, VEC3* vel ) {
  VEC3       origin = *pos;
  F32        dt = TICK_INTERVAL;
  AABB       aabb = gmove->aabb;
  LIST<VEC3> planes;
  I32        nudges = 0;

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

    F32 dot = vec_dot( *vel, t->normal );
    *pos += wishmove * t->frac;
    // nudge player away from wall
    *pos += t->normal * BSP_TRACE_EPSILON;

    // avoid clipping planes twice
    // in a rare edge case its possible to get stuck in the plane again after pushing away from it
    if( fabsf( dot ) > 0.0001f || planes.idx_of( t->normal ) == -1 )
      planes.push( t->normal );
    else {
      --bump;
      if( nudges++ > 3 ) {
        *vel = {};
        *pos = origin;
        break;
      }
      continue;
    }

    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_check_stuck( VEC3* in_pos ) {
  BSP_TRACE t;
  VEC3 pos = *in_pos;
  U32 i;
  if( vec_len( gmove->unstuck_vel ) < 0.f )
    gmove->unstuck_vel = { 0, 0, 1.f };

  bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
  if( !t.hit || !t.startsolid ) {
    if( vec_len( gmove->velocity ) > 1.f ) {
      gmove->unstuck_pos = pos;
      gmove->unstuck_vel = gmove->velocity;
    }
    return;
  }

  for( U32 i = 0; i < 3; ++i ) {
    VEC3 dir = vec3_axis[i];
    VEC3 nudge = pos + dir;
    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, nudge );
    if( !t.hit || !t.startsolid )
      return; // not actually stuck, just somewhat inside a solid

    nudge = pos + dir * -1.f;
    if( !t.hit || !t.startsolid )
      return;
  }

  VEC3 p1 = pos;
  defer( {
    if( !t.hit ) {
      *in_pos = pos;
      gmove_categorize_pos();
    }
  } );

  for( i = 1; i <= 32; i *= 2 ) {
    // try nudge away from wall
    pos = p1 + t.normal * BSP_TRACE_EPSILON * (F32)i;
    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
    if( !t.hit )
      return;

    // try moving up
    pos = p1 + VEC3{ 0, 0, 2.f } * (F32)i;
    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 } * (F32)i;
    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 } * (F32)i;
    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 } * (F32)i;
    bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
    if( !t.hit )
      return;

    // move back 1 tick
    if( vec_lensq( gmove->velocity ) > 10.f ) {
      pos = p1 + vec_normalize( gmove->unstuck_vel * -1.f ) * TICK_INTERVAL * (F32)i;
      bsp_trace( &t, gmove->bsp, gmove->aabb, pos, pos );
      if( !t.hit )
        return;
    }
  }
}

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

  currentspeed = vec_dot( gmove->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_airaccelerate( VEC3 wishdir, F32 wishspeed, F32 accel ) {
  F32 addspeed, accelspeed, currentspeed;
  F32 wishspd;

  wishspd = wishspeed;

  if( wishspd > 30.f )
    wishspd = 30.f;

  currentspeed = vec_dot( gmove->velocity, wishdir );
  addspeed = wishspd - currentspeed;

  if( addspeed <= 0.f )
    return;

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

  gmove->velocity += wishdir * accelspeed;
}

void gmove_friction() {
  F32 speed = vec_len2d( gmove->velocity );
  if( speed < 0.1f )
    return;

  F32 friction = var_getf( mv_friction ) /* * surface_friction */;
  F32 stopspeed = var_getf( mv_stopspeed );
  F32 control = (speed < stopspeed)? stopspeed : speed;

  F32 drop = control * friction * TICK_INTERVAL;
  F32 newspeed = speed - drop;
  if( newspeed < 0.f )
    newspeed = 0.f;

  if( newspeed != speed ) {
    newspeed /= speed;
    gmove->velocity *= newspeed;
  }
}

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, gmove->pos, start );
  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 > BSP_TRACE_EPSILON * 0.5f ) {
      gmove->pos = tr.point;
      gmove_check_stuck( &gmove->pos );
    }
  }
}

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;

  if( gmove->ground ) {
    VEC3 ground = pos;
    ground.z += 2.f;
    bsp_trace( tr, gmove->bsp, gmove->aabb, pos, ground );
  }

  // try moving forward first
  gmove_try_move( tr, &down, &downvel );
  VEC3 downn = tr->normal;

  // move up a step
  endpos = pos;
  endpos.z += (gmove->pl->stepsize + BSP_DIST_EPSILON);

  bsp_trace( tr, gmove->bsp, gmove->aabb, pos, endpos );
  if( !tr->startsolid )
    up = tr->point;

  gmove_try_move( tr, &up, &upvel );
  VEC3 upnorm = tr->normal;

  // move back down onto the ground
  endpos = up;
  endpos.z -= (gmove->pl->stepsize + BSP_DIST_EPSILON);

  bsp_trace( tr, gmove->bsp, gmove->aabb, up, endpos );

  defer( gmove_check_stuck( &gmove->pos ) );
  if( tr->normal.z < 0.7 ) { // not ground, slope
    gmove->pos = down;
    if( downn.z < 0.99f )
      gmove->pos += downn * BSP_TRACE_EPSILON * 2.f;
    gmove->velocity = downvel;
    F32 step_dist = gmove->pos.z - pos.z;
    gmove->out_step = step_dist;
    return;
  }

  if( !tr->startsolid )
    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;
    if( downn.z < 0.99f )
      gmove->pos += downn * BSP_TRACE_EPSILON;
    gmove->velocity = downvel;
  }
  else {
    // likely unnecessary but a sanity check
    // avoid moving more than velocity per tick horizontally
    VEC3 delta = up - pos;
    F32 dist = vec_len2d( delta );
    F32 speed = vec_len2d( gmove->velocity ) * TICK_INTERVAL;
    if( dist > speed ) {
      for( U32 i = 0; i < 2; ++i )
        delta[i] *= speed / dist;
    }

    gmove->pos += delta;
    if( tr->normal.z < 0.99f )
      gmove->pos += tr->normal * BSP_TRACE_EPSILON * 2.f;
    gmove->velocity = upvel;
    gmove->velocity.z = downvel.z;
  }
}

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

  if( vec_len( wishdir ) > 0.1f )
    vec_normalize( &wishdir );

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

  gmove_accelerate( wishdir, speed, var_getf( mv_accelerate ) );
  gmove->velocity[2] = 0;
  speed = vec_len( gmove->velocity );

  if( speed < 1.f ) {
    gmove->velocity = {};
    gmove_check_stuck( &gmove->pos );
    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;
    gmove_check_stuck( &gmove->pos );
    return;
  }

  gmove_step_move( dest, &tr );
}


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

  if( vec_len( wishdir ) > 0.1f )
    vec_normalize( &wishdir );

  VEC3 vel = wishdir * gmove->walkspeed;
  F32 speed = vec_len( vel );

  if( speed != 0 && speed > gmove->maxspeed ) {
    speed = gmove->maxspeed;
  }

  gmove_airaccelerate( wishdir, speed, var_getf( mv_airaccelerate ) );

  BSP_TRACE tr{};
  gmove_try_move( &tr, &gmove->pos, &gmove->velocity );
  gmove_check_stuck( &gmove->pos );
}

void gmove_check_falling() {

}

void gmove_check_jump() {
  if( !gmove->ground )
    return;

  gmove->ground = 0;
  // todo : play jump sound
  
  F32 mul = var_getf( mv_jump_impulse );
  if( gmove->pl->flags & PFL_DUCK ) {
    gmove->velocity.z = mul;
  }
  else {
    gmove->velocity.z += mul;
  }

  gmove_end_gravity();

  gmove->out_step += 0.15f;
}

void gmove_full_walk_move() {
  // todo: inwater, watermove
  gmove_start_gravity();

  // todo:
  // jump,
  // categorizepos
  // more

  if( gmove->input->jump )
    gmove_check_jump();

  if( gmove->ground ) {
    gmove_friction();
    gmove_check_velocity();
    gmove_walk_move();
  }
  else
    gmove_air_move();

  gmove_categorize_pos();

  gmove_end_gravity();
  if( gmove->ground )
    gmove->velocity.z = 0.f;

  gmove_check_falling();
}

void gmove_player_move() {
  if( gmove->velocity.z > 250.f )
    gmove->ground = 0;

  switch( gmove->pl->movetype ) {
    case PMT_WALK: return gmove_full_walk_move();
  }
}

void gmove_process_move() {
  gmove_player_move();
}

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