#include <cfloat>

#include "trace.h"
#include "../../util/math.h"
#include "bsp.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 VEC3 bsp_face_get_normal( const BSP_FACE* f ){
  const VEC3 v0 = f->verts.data[0].pos;
  const VEC3 v1 = f->verts.data[1].pos;
  const VEC3 v2 = f->verts.data[2].pos;
  return vec_normalize( vec_cross( v1 - v0, v2 - v0 ) );
}

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 in = vec_cross( norm, b - a );
    F32 len = vec_len( in );
    if( len <= 0.00001f ) 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_TRACE_EPSILON * 4.f ) )
      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 norm = bsp_face_get_normal( face );
  F32  d = vec_dot( norm, face->verts[0].pos );
  F32 radius = aabb_support_radius( hull, norm );

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

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

  if( fabsf( 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 = start + dir * t;

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

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

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

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;

  // start hull at origin rather than in middle
  trace->in_start.z += ( hull.max.z - hull.min.z );
  trace->in_end.z += ( hull.max.z - hull.min.z );

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

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

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