#include "trace.h"
#include "../../util/math.h"
#include "bsp.h"
#include "map.h"

U8 bsp_trace_should_hit_face( BSP_TRACE* tr, BSP_FACE* face ) {
  if( face->propid == MAPPROP_SKYBOX )
    return 0;
  if( !(face->hitmask & tr->hitmask) )
    return 0;

  return 1;
}

U8 bsp_trace_intersects_face(
  const VEC3& va, const VEC3& vb,
  const VEC3& v0, const VEC3& v1, const VEC3& v2,
  F32* fract,
  VEC2* out_uv = 0
) {
  VEC3 dir = vb - va;
  VEC3 e1 = v1 - v0,
       e2 = v2 - v0;

  VEC3 p = vec_cross( dir, e2 );
  F32 d = vec_dot( e1, p );
  if( d < 0 )
    d *= -1.0f;

  if( d <= BSP_NORM_EPSILON )
    return 0;

  F32 inv = 1.0f / d;
  VEC3 ivec = va - v0;
  F32 u = vec_dot( ivec, p ) * inv;
  if( u < -BSP_TRACE_EPSILON || u > 1.0f + BSP_TRACE_EPSILON ) {
    // outside
    return 0;
  }

  VEC3 q = vec_cross( ivec, e1 );
  F32 v = vec_dot( dir, q ) * inv;
  if( v < -BSP_TRACE_EPSILON || u + v > 1.0f + BSP_TRACE_EPSILON ) {
    // outside
    return 0;
  }

  F32 t = vec_dot( e2, q ) * inv;
  if( t < -BSP_TRACE_EPSILON || t > 1.0f + BSP_TRACE_EPSILON ) {
    // outside
    return 0;
  }

  if( fract ) *fract = m_clamp( t, 0.f, 1.f );
  if( out_uv ) {
    *out_uv = { u, v };
  }

  return 1;
}

U8 bsp_trace_leaf( BSP_TRACE* trace, BSP* bsp, I32 leafidx, const VEC3& start, const VEC3& end ) {
  BSP_LEAF* leaf = &bsp->leaves.data[leafidx];
  U8 didhit = 0;
  F32 best = FLT_MAX;

  for( U32 i = 0; i < leaf->count; ++i ) {
    BSP_FACE* face = &bsp->faces.data[leaf->first + i];

    if( !bsp_trace_should_hit_face( trace, face ) )
      continue;

    // shouldnt happen
    if( face->verts.size < 3 )
      continue;

    for( U32 t0 = 1; t0 + 1 < face->verts.size; ++t0 ) {
      VEC3 v0 = face->verts[0].pos;
      VEC3 v1 = face->verts[t0].pos;
      VEC3 v2 = face->verts[t0 + 1].pos;

      F32 t;
      VEC2 uv;

      if( !bsp_trace_intersects_face( start, end, v0, v1, v2, &t, &uv ) )
        continue;

      if( t < best ) {
        best = t;
        trace->hit = didhit = 1;
        trace->frac = t;
        trace->point = start + (end - start) * t;
        trace->normal = vec_normalize( vec_cross( v1 - v0, v2 - v0 ) );
        trace->propid = face->propid;
        trace->leafid = leafidx;
        trace->faceid = leaf->first + i;
      }
    }
  }

  return didhit;
}

U8 bsp_trace( BSP_TRACE* trace, BSP* bsp, I32 nodeidx, const VEC3& start, const VEC3& end ) {
  if( bsp_is_leaf( nodeidx ) ) {
    I32 i = bsp_leaf_index( nodeidx );
    return bsp_trace_leaf( trace, bsp, i, start, end );
  }

  BSP_NODE* node = &bsp->nodes.data[nodeidx];
  F32 da = bsp_plane_dist( node->plane, start );
  F32 db = bsp_plane_dist( node->plane, end );

  if( da > BSP_TRACE_EPSILON && db > BSP_TRACE_EPSILON )
    return bsp_trace( trace, bsp, node->front, start, end );
  if( da < -BSP_TRACE_EPSILON && db < -BSP_TRACE_EPSILON )
    return bsp_trace( trace, bsp, node->back, start, end );

  VEC3 dir = end - start;
  F32 denom = vec_dot( node->plane.normal, dir );
  I32 near = (da >= 0.f) ? node->front : node->back;
  I32 far  = (da >= 0.f) ? node->back : node->front;
  if( fabsf( denom ) < BSP_TRACE_EPSILON ) {
    if( bsp_trace( trace, bsp, near, start, end ) )
      return 1;
    return bsp_trace( trace, bsp, far, start, end );
  }

  F32 t = (node->plane.dist - vec_dot( node->plane.normal, start ) ) / denom;
  t = m_clamp( t, 0.f, 1.f );

  if( bsp_trace( trace, bsp, near, start, end ) )
    return 1;

  return bsp_trace( trace, bsp, far, start, end );
}

// nudge the trace ends slightly to prevent exact-coplanar infinite recursion.
// quake does this every recursion divided up by the length of the trace
// should not matter in the end
void bsp_trace_nudge( BSP_TRACE* trace ) {
  VEC3 dir = trace->in_end - trace->in_start;
  VEC3 nudge = dir * BSP_TRACE_EPSILON;
  trace->in_start += nudge;
  trace->in_end -= nudge;
}

U8 bsp_trace( BSP_TRACE* trace, BSP* bsp, VEC3 start, VEC3 end ) {
  if( !trace ) {
    dlog( "bsp_trace() : null trace\n" );
    abort();
  }

  trace->in_start = start;
  trace->in_end = end;

  U8 ret = bsp_trace( trace, bsp );
  if( !ret )
    trace->point = trace->in_end;
  return ret;
}

U8 bsp_trace( BSP_TRACE* trace, BSP* bsp ) {
  if( !trace ) {
    dlog( "bsp_trace() : null trace\n" );
    abort();
  }

  trace->hit = 0;
  trace->startsolid = 0;

  bsp_trace_nudge( trace );
  VEC3 start = trace->in_start;
  VEC3 end = trace->in_end;

  return bsp_trace( trace, bsp, bsp->root, start, end );
}

F32 hull_proj_radius( const AABB& hull, VEC3 dir ) {
  F32 expand = aabb_support_radius( hull, dir );
  VEC3 half   = (hull.max - hull.min) * 0.5f;
  F32 r;
  if( dir.z < BSP_NORM_EPSILON ) {
    r = (half.x + half.y) * 0.5f;
  } else
    r = half.z;

  F32 hypot = r * 1.414213f;
  F32 a = expand;
  F32 c = sqrtf( hypot * hypot - a * a );

  return c;
}

inline U8 point_in_inflated_poly(
  const VEC3& point,
  const BSP_FACE* face,
  const AABB& hull,
  const VEC3& norm
){
  U32 c = face->verts.size;
  VEC3 center = face->center;
  if( !c )
    return 0;

  VEC3 hullh = ( hull.max - hull.min ) * 0.5f;
  for( U32 i = 0; i < c; ++i ) {
    VEC3 a = face->verts.data[i].pos;
    VEC3 b = face->verts.data[(i+1)%c].pos;
    VEC3 e = b - a;
    VEC3 in = vec_cross( norm, e );
    F32 len = vec_len( in );
    if( len <= 0.0001f ) continue;
    in /= len;
    if( vec_dot( center - a, in ) < 0.0f )
      in = in * -1.0f;

    F32 dist   = vec_dot( point - a, in );
    F32 expand = fabsf( in.z ) > 0.001f? hullh.z : hullh.x;
    if( dist < -( expand - BSP_EDGE_TOLERANCE ) )
      return 0;

    U8 axis = 0;
    // check if plane is aligned to any axis
    for( U32 i = 0; i < 3; ++i ) {
      if( norm[i] < BSP_NORM_EPSILON )
        ++axis;
    }

    if( !axis ) { // project closest hull corner onto plane
      VEC3 diag = in;
      diag *= -1;
      for( U32 i = 0; i < 3; ++i ) {
        if( diag[i] )
          diag[i] = copysignf( 1.0f, diag[i] );
      }

      diag *= hullh;
      if( vec_dot( point - diag, in ) < 0.f )
        return 0;
    }

  }
  return 1;
}


// big fat ass
U8 bsp_trace_sweep_aabb_to_face(
  BSP_TRACE* trace,
  const AABB& hull,
  BSP_FACE* face,
  U32 face_idx
) {
  if( face->verts.size < 3 )
    return 0;

  VEC3 norm   = bsp_face_get_normal( face );
  F32  d      = vec_dot( norm, face->verts[0].pos );
  F32  radius = aabb_support_radius( hull, norm );

  VEC3 dir  = trace->in_end - trace->in_start;
  F32  s0   = vec_dot( norm, trace->in_start ) - d;
  F32  ndir = vec_dot( norm, dir );

  if( ndir > 0 ) // dont collide with rear side
    return 0;

  VEC3 hullv  = hull.max - hull.min;
  for( U32 i = 0; i < 3; ++i ) hullv[i] += BSP_EDGE_TOLERANCE;
  AABB fhullh = { face->mins - hullv * 0.5f, face->maxs + hullv * 0.5f };

  if( fabsf( s0 ) <= radius + BSP_TRACE_EPSILON ) {
    if( aabb_contains_point( fhullh, trace->in_start ) && point_in_inflated_poly( trace->in_start, face, hull, norm ) ) {
      trace->hit = 1;
      trace->frac = 0.f;
      trace->startsolid = 1;
      trace->point = trace->in_start;
      trace->normal = norm;
      trace->propid = face->propid;
      trace->faceid = face_idx;
      return 1;
    }
  }

  F32 t_enter = (-radius - s0) / ndir;
  F32 t_exit  = ( radius - s0) / ndir;
  if( t_enter > t_exit ) { F32 tmp=t_enter; t_enter=t_exit; t_exit=tmp; }
  if( t_exit < 0.f - BSP_TRACE_EPSILON || t_enter > 1.f + BSP_TRACE_EPSILON )
    return 0;

  F32 t = m_clamp( t_enter, 0.f, 1.f );
  VEC3 step = trace->in_start + dir * t;

  if( !aabb_contains_point( fhullh, step ) )
    return 0;

  if( !point_in_inflated_poly( step, face, hull, norm ) )
    return 0;

  trace->hit = 1;
  trace->frac = t;
  trace->point = step;
  trace->normal = norm;
  trace->propid = face->propid;
  trace->faceid = face_idx;
  return 1;
}

U8 bsp_trace_sweep_aabb_to_leaf_edges(
  BSP* bsp,
  BSP_TRACE* trace,
  const AABB& hull,
  I32 leaf_idx,
  F32* in_best
) {
  BSP_LEAF* leaf = &bsp->leaves.data[leaf_idx];
  U8 hit = 0;
  F32 best = *in_best;
 
  BSP_TRACE besthit = *trace;
  VEC3 hullh = ( hull.max - hull.min ) * 0.5f;

  for( U32 i = 0; i < leaf->edges.size; ++i ) {
    BSP_EDGE* e = &leaf->edges[i];
    BSP_FACE* f = &bsp->faces.data[e->face];

   if( !bsp_trace_should_hit_face( trace, f ) )
     continue;

    VEC3 n = e->plane.normal;
    F32  d = e->plane.dist;
    F32  r = aabb_support_radius( hull, n );

    VEC3 dir  = trace->in_end - trace->in_start;
    F32  s0   = vec_dot( n, trace->in_start ) - d;
    F32  ndir = vec_dot( n, dir );

    if( ndir > 0.f ) continue;
    if( s0 <= -(r + BSP_TRACE_EPSILON) ) continue;
 
    F32 t_enter = ( r - s0) / ndir;
    F32 t_exit  = (-r - s0) / ndir;
    if( t_enter > t_exit ) { F32 tmp = t_enter; t_enter = t_exit; t_exit = tmp; }

    if( t_exit < -BSP_TRACE_EPSILON || t_enter > 1.f + BSP_TRACE_EPSILON )
      continue;

    F32 t = m_clamp( t_enter, 0.f, 1.f );
    VEC3 step = trace->in_start + (dir) * t;
    VEC3 edir = e->v2 - e->v1;
    F32 elen = vec_len( edir );
    if( elen < 0.001f )
      continue;
    edir /= elen;

    F32 along = vec_dot( step - e->v1, edir );
    F32 expand;
    if( fabsf( edir.z ) < BSP_NORM_EPSILON )
      expand = (edir.x > edir.y)? hullh.y : hullh.x;
    else
      expand = hullh.z;

    if( along < -( expand + BSP_EDGE_TOLERANCE ) || ( along > elen + expand ) )
      continue;

    VEC3 proj = e->v1 + edir * m_clamp( along, 0.f, elen );
    VEC3 ec = vec_cross( edir, n );
    F32 nlen = vec_len( ec );
    if( nlen < 0.001f )
      continue;
    ec /= nlen;
    F32 dist = vec_dot( proj - step, ec );

    expand = aabb_support_radius( hull, ec );
    if( fabsf( dist ) > expand )
      continue;

    if( t < best ) {
      best = t;
      *in_best = best;
      besthit.frac = t;
      besthit.point = step;
      besthit.normal = n;
      besthit.propid = f->propid;
      besthit.faceid = e->face;
      besthit.leafid = leaf_idx;
      hit = 1;
    }
  }

  if( hit ) {
    *trace = besthit;
    trace->hit = 1;
  }

  return hit;
}

U8 bsp_trace_sweep_aabb_to_leaf(
  BSP* bsp,
  BSP_TRACE* trace,
  const AABB& hull,
  I32 leaf_idx
) {
  BSP_LEAF* leaf = &bsp->leaves.data[leaf_idx];
  U8 hit = 0;
  F32 best = FLT_MAX;

  BSP_TRACE besthit = *trace;
  BSP_TRACE edgehit = *trace;

  for( U32 i = 0; i < leaf->count; ++i ) {
    U32 face_idx = leaf->first + i;
    BSP_FACE* f = &bsp->faces.data[face_idx];

    if( !bsp_trace_should_hit_face( trace, f ) )
      continue;

    BSP_TRACE tr = *trace;
    if( !bsp_trace_sweep_aabb_to_face( &tr, hull, f, face_idx ) )
      continue;

    if( tr.frac < best ) {
      best = tr.frac;
      besthit = tr;
      besthit.leafid = leaf_idx;
      hit = 1;
    }
  }

  best = FLT_MAX;
  U8 ehit = bsp_trace_sweep_aabb_to_leaf_edges( bsp, &edgehit, hull, leaf_idx, &best );
  if( hit || ehit ) {
    if( ehit && !hit )
      *trace = edgehit;
    else if( !ehit && hit )
      *trace = besthit;
    else if( edgehit.frac <= besthit.frac + BSP_TRACE_EPSILON )
      *trace = edgehit;
    else {
      *trace = besthit;
      trace->normal = edgehit.normal;
    }
    trace->hit = 1;
    return 1;
  }

  return 0;
}

U8 bsp_trace_sweep_aabb(
  BSP_TRACE* trace,
  BSP* bsp,
  const AABB& hull,
  I32 node_idx
) {
  VEC3 start = trace->in_start;
  VEC3 end = trace->in_end;
  if( bsp_is_leaf( node_idx ) ) {
    I32 i = bsp_leaf_index( node_idx );
    return bsp_trace_sweep_aabb_to_leaf( bsp, trace, hull, i );
  }

  BSP_NODE* node = &bsp->nodes.data[node_idx];
  VEC3 n = node->plane.normal;
  F32 d = node->plane.dist;

  F32 s_dist = vec_dot( n, start ) - d;
  F32 e_dist = vec_dot( n, end )   - d;
  F32 offset = aabb_support_radius( hull, n );

  if( s_dist > offset && e_dist > offset )
    return bsp_trace_sweep_aabb( trace, bsp, hull, node->front );

  if( s_dist < -offset && e_dist < -offset )
    return bsp_trace_sweep_aabb( trace, bsp, hull, node->back );

  VEC3 dir = end - start;
  F32 denom = ( e_dist - s_dist );

  if( fabsf(denom) < BSP_TRACE_EPSILON ) {
    I32 near = (s_dist >= 0.f) ? node->front : node->back;
    I32 far  = (s_dist >= 0.f) ? node->back  : node->front;
    if( bsp_trace_sweep_aabb( trace, bsp, hull, near ) )
      return 1;
    return bsp_trace_sweep_aabb( trace, bsp, hull, far );
  }

  F32 tin  = ( offset - s_dist) / denom;
  F32 tout = (-offset - s_dist) / denom;
  if( tin > tout ) { F32 tmp = tin; tin = tout; tout = tmp; }

  tin = m_clamp( tin, 0.f, 1.f );
  tout = m_clamp( tout , 0.f, 1.f );

  I32 near = (s_dist > 0.f) ? node->front : node->back;
  I32 far  = (s_dist > 0.f) ? node->back  : node->front;

  BSP_TRACE tr_near = *trace;
  BSP_TRACE tr_far  = *trace;

  U8 hit_near = bsp_trace_sweep_aabb( &tr_near, bsp, hull, near );
  U8 hit_far  = bsp_trace_sweep_aabb( &tr_far , bsp, hull, far );

  if( hit_near && hit_far ) {
    *trace = (tr_near.frac <= tr_far.frac) ? tr_near : tr_far;
    return 1;
  }

  if( hit_near ) { *trace = tr_near; return 1; }
  if( hit_far )  { *trace = tr_far; return 1; }

  if( tin <= BSP_TRACE_EPSILON || tin >= 1.f - BSP_TRACE_EPSILON ) {
    F32 nudge = (denom >= 0.f ? 1.f : -1.f) * BSP_TRACE_EPSILON;
    VEC3 nudged = start + dir * nudge;
    trace->in_start = nudged;
    return bsp_trace_sweep_aabb( trace, bsp, hull, far );
  }

  return bsp_trace_sweep_aabb(trace, bsp, hull, far );
}

U8 bsp_trace( BSP_TRACE* trace, BSP* bsp, AABB hull ) {
  if( !trace || !bsp )
    return 0;

  trace->startsolid = 0;
  trace->hit = 0;
  bsp_trace_nudge( trace );

  VEC3 start = trace->in_start;
  VEC3 end = trace->in_end;

  // move trace up by half of the hull - extend at feet instead of center
  VEC3 off = VEC3{ 0, 0, hull.max.z - hull.min.z } * 0.5f;

  trace->in_start += off;
  trace->in_end += off;

  U8 ret = bsp_trace_sweep_aabb( trace, bsp, hull, bsp->root );

  trace->in_start = start;
  trace->in_end = end;

  if( !ret )
    trace->point = end;
  else
    trace->point -= off;

  return ret;
}

U8 bsp_trace( BSP_TRACE* trace, BSP* bsp, AABB hull, VEC3 start, VEC3 end ) {
  if( !trace || !bsp )
    return 0;

  trace->in_start = start;
  trace->in_end = end;
  return bsp_trace( trace, bsp, hull );
}