/*
=======================================================================
This file is part of Redman's RT.

Redman's RT 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 3 of the License, or
(at your option) any later version.

Redman's RT 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 Redman's RT.  If not, see <http://www.gnu.org/licenses/>.
=======================================================================
*/

#include "common.h"

W_blockdata_t G_RemoveBlock( W_world_t *world, vec3_t vieworg, vec3_t viewdir, size_t targetDepth )
{               
  ray_t ray;
  rtres_t rtres;
  W_otnode_t *target, *node;
  W_blockdata_t block;
  size_t i, depth;
  
  U_BuildRay( vieworg, viewdir, INFINITY, &ray, &rtres );
  
new_target:
  
  target = W_Trace( world, NULL, &ray, &rtres );
  
  if( !target )
    return BLOCK_NONE;

  for( depth = 0, node = target; node != world->otroot; node = node->p, depth++ );
  
  if( depth < targetDepth )
  {
    for( i = 0; i < 8; i++ )
    {
      target->n[ i ] = W_NewOTNode( target, i );
      memcpy( &target->n[ i ]->data, &target->data, sizeof( W_blockdata_t ) );
    }
    memset( &target->data, 0, sizeof( W_blockdata_t ) );
    goto new_target;
  }
  else if( depth == targetDepth )
    block = target->data;
  else
    block = BLOCK_NONE;

  W_DeleteOTNode( target );
  return block;
}

bool_t G_PlaceBlock( W_world_t *world, vec3_t vieworg, vec3_t viewdir, W_blockdata_t blockdata, size_t targetDepth )
{
  ray_t ray;
  rtres_t rtres;
  W_otnode_t *target, *node; 
  vec3_t point;
  size_t depth;
  
  U_BuildRay( vieworg, viewdir, INFINITY, &ray, &rtres );
              
  if( !W_Trace( world, NULL, &ray, &rtres ) )
    return false;

  V3MA2( point, ray.org, rtres.dist, ray.dir, 1.0e-2, rtres.normal );
  
  target = W_Point( world->otroot, point );
  
  if( !target )
    return false;
  
  if( target->data )
    return false;

  for( depth = 0, node = target; node != world->otroot; node = node->p, depth++ );

  while( depth < targetDepth )
  {
    int index, ix, iy, iz;
    vec3_t center;

    V3Lerp( center, 0.5, target->aabb[ 0 ], target->aabb[ 1 ] );
    
    ix = ( point[ 0 ] > center[ 0 ] );
    iy = ( point[ 1 ] >= center[ 1 ] );
    iz = ( point[ 2 ] >= center[ 2 ] );
    
    index = ix | ( iy << 1 ) | ( iz << 2 );
    if( target->n[ index ] )
      printf( "wtf???\n" );
    
    memset( &target->data, 0, sizeof( W_blockdata_t ) );
    target = target->n[ index ] = W_NewOTNode( target, index );

    depth++;
  }
  
  if( depth > targetDepth )
    return false; //no space

  target->data = blockdata;

  return true;
}

void G_RecolorBlock( W_world_t *world, vec3_t vieworg, vec3_t viewdir )
{
  W_otnode_t *node;
  ray_t ray;
  rtres_t rtres;
  
  U_BuildRay( vieworg, viewdir, INFINITY, &ray, &rtres );
  
  node = W_Trace( world, NULL, &ray, &rtres );
  
  if( node )
  {
    node->data++;
    node->data %= _Material_count;
    if( node->data == 0 )
      node->data = 1;
  }
}

bool_t G_ExactTraceAABB( const vec3_t const *aabb, const ray_t *ray, rtres_t *rt )
{
  vec_t tn, tp, tyn, typ, tzn, tzp;

  tn = ( aabb[ ray->dinvsign[ 0 ] ][ 0 ] - ray->org[ 0 ] ) * ray->dinv[ 0 ];
  tp = ( aabb[ 1 - ray->dinvsign[ 0 ] ][ 0 ] - ray->org[ 0 ] ) * ray->dinv[ 0 ];

  tyn = ( aabb[ ray->dinvsign[ 1 ] ][ 1 ] - ray->org[ 1 ] ) * ray->dinv[ 1 ];
  typ = ( aabb[ 1 - ray->dinvsign[ 1 ] ][ 1 ] - ray->org[ 1 ] ) * ray->dinv[ 1 ];
  
  V3Set( rt->normal, ray->dsign2[ 0 ], 0, 0 );
  
  V3MA( rt->hit, ray->org, tn, ray->dir );
  rt->hit[ 0 ] = aabb[ ray->dinvsign[ 0 ] ][ 0 ];
  
  if( ( tn > typ ) || ( tyn > tp ) )
    return false;

  if( tyn > tn )
  {
    tn = tyn;
    V3Set( rt->normal, 0, ray->dsign2[ 1 ], 0 );
    
    V3MA( rt->hit,  ray->org, tn, ray->dir );
    rt->hit[ 1 ] = aabb[ ray->dinvsign[ 1 ] ][ 1 ];
  }
  
  if( typ < tp )
    tp = typ;
  
  tzn = ( aabb[ ray->dinvsign[ 2 ] ][ 2 ] - ray->org[ 2 ] ) * ray->dinv[ 2 ];
  tzp = ( aabb[ 1 - ray->dinvsign[ 2 ] ][ 2 ] - ray->org[ 2 ] ) * ray->dinv[ 2 ];

  if( ( tn > tzp ) || ( tzn > tp ) )
    return false;
  
  if( tzn > tn )
  {
    tn = tzn;
    V3Set( rt->normal, 0, 0, ray->dsign2[ 2 ] );
    V3MA( rt->hit, ray->org, tn, ray->dir );
    rt->hit[ 2 ] = aabb[ ray->dinvsign[ 2 ] ][ 2 ];
  }
  
  if( tzp < tp )
    tp = tzp;

  rt->dist = tn; 

  return tn >= 0;
}

static void G_BoxTrace_R( W_otnode_t *node, W_otnode_t **node_best, rtres_t *rt_best, const ray_t *ray, const vec3_t *delta )
{
  size_t i;
  bool_t intersects;
  vec3_t aabb[ 2 ];
  rtres_t rt;
  
  V3Add( aabb[ 0 ], node->aabb[ 0 ], delta[ 0 ] );
  V3Add( aabb[ 1 ], node->aabb[ 1 ], delta[ 1 ] );
  
  if( U_PointInAABB2( ray->org, (const vec3_t *)aabb ) )
  {
    if( node->data )
      return;
    intersects = true;
  }
  else
    intersects = G_ExactTraceAABB( (const vec3_t*)aabb, ray, &rt );
    
  if( !intersects )
    return;

  if( node->data )
  {
    if( rt_best->dist >= rt.dist )
    {
      (*node_best) = node;
      memcpy( rt_best, &rt, sizeof( rtres_t ) );
      rt_best->data = node->data;
    }
  }
  else
    for( i = 0; i < 8; i++ )
      if( node->n[ i ] )
        G_BoxTrace_R( node->n[ i ], node_best, rt_best, ray, delta );
}

W_otnode_t *G_BoxTrace( W_world_t *world, rtres_t *rt_best, const ray_t *ray, const vec3_t *aabb )
{
  W_otnode_t *out_node = NULL;
  
  memset( rt_best, 0, sizeof( rtres_t ) );
  rt_best->dist = INFINITY;
  
  if( !world->otroot )
    return NULL;
    
  G_BoxTrace_R( world->otroot, &out_node, rt_best, ray, aabb );
  
  if( rt_best->dist > ray->limit )
    return NULL;
  
  return out_node;
}

void G_ClipVelocity( vec3_t out, vec3_t in, vec3_t normal )
{
  vec_t dot;
  
  dot = V3Dot( in, normal );
  V3Mul2( out, in, 1, normal, -dot );
}



//FIXME: G_BoxTrace gets stuck on edges
void G_MovePlayer( W_world_t *world, G_player_t *p, vec_t delta )
{
  ray_t ray;
  rtres_t rtres;
  vec_t disp, speed;
  bool_t clip;

  while( delta > 1e-3 )
  {
    V3Copy( ray.dir, p->velocity );
    speed = V3Normalize( ray.dir );
    disp = speed * delta;

    U_BuildRay( p->origin, p->velocity, disp, &ray, &rtres );

    clip = ( G_BoxTrace( world, &rtres, &ray, (const vec3_t*)p->aabb ) != NULL );

    if( clip )
    {
      V3Copy( p->origin, rtres.hit );
      delta -= rtres.dist / V3Length( p->velocity );
      
      V3Reflect( p->velocity, p->velocity, rtres.normal );
      
      //G_ClipVelocity( p->velocity, p->velocity, rtres.normal );
    }
    else
    {
      V3MA( p->origin, p->origin, disp, ray.dir );
      break;
    }
  }
}

/*
void G_RunPlayer( W_world_t *world, G_player_t *p )
{
  G_time_t frametime;
  vec_t delta;
  frametime = U_Clock( );

  V3AnglesToMatrix( p->matrix, p->angles );
  M3RotZ( p->flatMatrix, p->angles[ 2 ] );
  
  #define TIMESTEP 10
  
  while( p->lastPhysicsTime + TIMESTEP < frametime )
  {
    vec3_t wishvel;
    
    delta = 1.0e-3 * TIMESTEP;

    if( p->cmd_noclip )
    {
      vec_t mod;
      
      mod = delta * ( p->cmd_sprint ? 20 : 6 );
      
      V3MA2( p->origin, p->origin, p->cmd_move[ 0 ] * mod, p->matrix + 3, p->cmd_move[ 1 ] * mod, p->matrix );
      if( p->cmd_jump )
        V3MA( p->origin, p->origin, -mod, p->matrix + 6 );
        
      V3Set( p->velocity, 0, 0, 0 );
      
      p->lastPhysicsTime += TIMESTEP;
      continue;
    }

    {
      ray_t ray;
      rtres_t rtres;
      
      V3Copy( ray.org, p->origin );
      V3Set( ray.dir, 0, 0, 1 );
      U_BuildRay( NULL, NULL, 0.01, &ray, &rtres );
      p->onGround = ( G_BoxTrace( world, &rtres, &ray, (const vec3_t*)p->aabb ) != NULL );
    }
    
    if( p->onGround && p->cmd_jump )
        p->velocity[ 2 ] = -2.6;
    
    V3Mul2( wishvel, p->flatMatrix + 3, p->cmd_move[ 0 ],
                     p->flatMatrix, p->cmd_move[ 1 ] );
    wishvel[ 2 ] = 0.0;
    V2Normalize( wishvel );
    
    V2Mul( wishvel, wishvel, 2 );
    if( p->cmd_sprint )
      V2Mul( wishvel, wishvel, 2 );
    
    
    V2Lerp( p->velocity, 0.1, p->velocity, wishvel );
    p->velocity[ 2 ] += 5.81 * delta;

    G_MovePlayer( world, p, delta );

    p->lastPhysicsTime += TIMESTEP;
  }
}*/

void G_RunPlayer( W_world_t *world, G_player_t *p )
{
  G_time_t frametime;
  vec_t delta;
  frametime = U_Clock( );

  V3AnglesToMatrix( p->matrix, p->angles );
  
  #define TIMESTEP 10
  
  while( p->lastPhysicsTime + TIMESTEP < frametime )
  {
    size_t i;
    vec_t mod;
    vec3_t target;
    
    delta = 1.0e-3 * TIMESTEP;
    
    mod = ( p->cmd_sprint ? 20 : 6 );
    
    for( i = 0; i < 3; i++ )
      target[ i ] = p->cmd_move[ 0 ] * p->matrix[ i + 3 ] +
                    p->cmd_move[ 1 ] * p->matrix[ i ] +
                    ( p->cmd_jump ? -p->matrix[ i + 6 ] : 0 ) +
                    ( p->cmd_down ? p->matrix[ i + 6 ] : 0 );
    V3Mul( target, target, mod );
    
    V3Lerp( p->velocity, 0.1, p->velocity, target );
    
    V3MA( p->origin, p->origin, delta, p->velocity );

    p->lastPhysicsTime += TIMESTEP;
  }
}