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

// i have no idea how this works tbh i pasted this
// todo : backface culling (return 0 if d < BSP_TRACE_EPSILON)
U8 bsp_segment_intersects_tri(
  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( fabsf( d ) < BSP_TRACE_EPSILON ) {
    // parallel
    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_segment( BSP_TRACE* trace, BSP* bsp, I32 leafidx, const VEC3& va, const VEC3& vb ) {
  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];

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

      // todo : skip func for props

      F32 t;
      VEC2 uv;

      if( !bsp_segment_intersects_tri( va, vb, v0, v1, v2, &t, &uv ) )
        continue;

      if( t < best ) {
        best = t;
        trace->hit = didhit = 1;
        trace->frac = t;
        trace->point = va + (vb - va) * 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_segment( BSP_TRACE* trace, BSP* bsp, I32 nodeidx, const VEC3& va, const VEC3& vb ) {
  if( bsp_is_leaf( nodeidx ) ) {
    I32 i = bsp_leaf_index( nodeidx );
    return bsp_trace_leaf_segment( trace, bsp, i, va, vb );
  }

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

  if( da > BSP_TRACE_EPSILON && db > BSP_TRACE_EPSILON )
    return bsp_trace_segment( trace, bsp, node->front, va, vb );
  if( da < -BSP_TRACE_EPSILON && db < -BSP_TRACE_EPSILON )
    return bsp_trace_segment( trace, bsp, node->back, va, vb );

  VEC3 dir = vb - va;
  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_segment( trace, bsp, near, va, vb ) )
      return 1;
    return bsp_trace_segment( trace, bsp, far, va, vb );
  }

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

  VEC3 iplane = va + dir * t;
  if( bsp_trace_segment( trace, bsp, near, va, iplane ) )
    return 1;

  if( t <= BSP_TRACE_EPSILON || t >= 1.0f - BSP_TRACE_EPSILON ) {
    return bsp_trace_segment( trace, bsp, far, iplane, vb );
  }

  return bsp_trace_segment( trace, bsp, far, iplane, vb );
}

// 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_segment( trace, bsp, bsp->root, start, end );
}



inline U8 point_in_inflated_poly(
  const VEC3& point,
  const BSP_FACE* face,
  const AABB& hull,
  const VEC3& norm
){
  VEC3 center{};
  U32 c = face->verts.size;
  for( U32 i = 0; i < c; ++i )
    center = center + face->verts.data[i].pos;
  if( c ) center = center * (1.0f / c);

  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 *= (1.0f/ len);
    if( vec_dot( center - a, in ) < 0.0f )
      in = in * -1.0f;

    F32 dist   = vec_dot( point - a, in );
    F32 expand = aabb_support_radius( hull, in );
    if( dist < -( expand - BSP_EDGE_TOLERANCE ) )
      return 0;

    U8 isaxis = 0;
    for( U32 i = 0; i < 3; ++i ) {
      if( norm == vec3_axis[i] || norm == (vec3_axis[i] * -1.f) ) {
        isaxis = 1; break;
      }
    }

    // for non-axis-aligned faces the point of contact is infinitely thin
    // todo : this isnt foolproof, check for hit direction if it comes from up/down
    // just ignore this, only apply to walls
    if( !isaxis ) {
      if( dist < BSP_EDGE_TOLERANCE )
        return 0;
    }
  }
  return 1;
}


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

  VEC3 hullv  = hull.max - hull.min;
  AABB fhull  = { face->mins - hullv, face->maxs + hullv };
  AABB fhullh = { face->mins - hullv * 0.5f, face->maxs + hullv * 0.5f };
  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 );

  VEC3 dirn = vec_normalize( dir );
  F32 tdot  = vec_dot( dirn, norm );
  if( !isnan( tdot ) && tdot >= 0 ) // dont collide with rear side
    return 0;

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

  if( ndir > BSP_TRACE_EPSILON )
    return 0;

  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( fhull, step ) )
    return 0;

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

  step.x = m_clamp( step.x, fhullh.min.x, fhullh.max.x );
  step.y = m_clamp( step.y, fhullh.min.y, fhullh.max.y );
  step.z = m_clamp( step.z, fhullh.min.z, fhullh.max.z );

  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,
  const VEC3& start,
  const VEC3& end,
  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;
  for( U32 i = 0; i < leaf->edges.size; ++i ) {
    BSP_EDGE* e = &leaf->edges[i];
    BSP_FACE* f = &bsp->faces.data[e->face];

    if( f->propid == MAPPROP_SKYBOX )
      continue;

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

    F32 s0   = vec_dot( n, start ) - d;
    F32 ndir = vec_dot( n, end - start );

    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 = start + (end - start) * t;

    VEC3 edir = e->v2 - e->v1;
    F32 elen = vec_len( edir );
    if( elen < 0.001f )
      continue;
    edir *= (1.f / elen);

    F32 along = vec_dot( step - e->v1, edir );
    VEC3 proj = e->v1 + edir * m_clamp( along, 0.f, elen );
    F32 dist = vec_dist( proj, step );

    F32 expand = aabb_support_radius( hull, n );
    if( dist > BSP_TRACE_EPSILON ) {
      if( dist > expand - BSP_EDGE_TOLERANCE )
        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,
  const VEC3& start,
  const VEC3& end,
  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( f->propid == MAPPROP_SKYBOX )
      continue;

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

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

  U8 ehit = bsp_trace_sweep_aabb_to_leaf_edges( bsp, &edgehit, hull, start, end, leaf_idx, &best );

  if( hit || ehit ) {
    if( ehit && !hit )
      *trace = edgehit;
    else if( !ehit && hit )
      *trace = besthit;
    else if( edgehit.frac < besthit.frac )
      *trace = edgehit;
    else
      *trace = besthit;
    trace->hit = 1;
    return 1;
  }

  return 0;
}

U8 bsp_trace_sweep_aabb(
  BSP_TRACE* trace,
  BSP* bsp,
  const AABB& hull,
  const VEC3& start,
  const VEC3& end,
  I32 node_idx
) {
  if( bsp_is_leaf( node_idx ) ) {
    I32 i = bsp_leaf_index( node_idx );
    return bsp_trace_sweep_aabb_to_leaf( bsp, trace, hull, start, end, 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, start, end, node->front );

  if( s_dist < -offset && e_dist < -offset )
    return bsp_trace_sweep_aabb( trace, bsp, hull, start, end, 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, start, end, near ) )
      return 1;
    return bsp_trace_sweep_aabb( trace, bsp, hull, start, end, 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 );

  VEC3 mid = start + dir * tin;
  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, start, mid, near );
  U8 hit_far  = bsp_trace_sweep_aabb( &tr_far , bsp, hull, mid,    end, 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 = mid + dir * nudge;
    return bsp_trace_sweep_aabb( trace, bsp, hull, nudged, end, far );
  }

  return bsp_trace_sweep_aabb(trace, bsp, hull, mid, end, 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, trace->in_start, trace->in_end, bsp->root );

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

  if( !ret )
    trace->point = trace->in_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 );
}