#ifndef intersection_h
#define intersection_h

#include "mymath/mymath.h"

//forward declarations
class sphere;
class plane;
class aabb;
class frustum;

namespace inner
{
  static bool is_on_right_side( sphere* a, plane* b );
  static bool is_on_right_side( aabb* a, plane* b );

  static bool is_inside( aabb* a, sphere* b );
  static bool is_inside( aabb* a, aabb* b );
  static bool is_inside( sphere* a, aabb* b );
  static bool is_inside( sphere* a, sphere* b );

  static bool intersect( sphere* a, sphere* b );
  static bool intersect( sphere* a, plane* b );
  static bool intersect( plane* a, plane* b );
  static bool intersect( aabb* a, aabb* b );
  static bool intersect( aabb* a, sphere* b );
  static bool intersect( aabb* a, plane* b );
}

//generic abstract shape class
//needed so that any shape can intersect any other shape
class shape
{
  public:
    //needed for frustum culling
    virtual bool is_on_right_side( plane* p ) = 0;

    //needed for bvh construction, order matters
    bool is_inside( shape* s ){ return s->_is_inside(this); }
    virtual bool _is_inside( shape* s ) = 0;

    virtual bool __is_inside( sphere* a ) = 0;
    virtual bool __is_inside( aabb* a ) = 0;

    //for intersection test
    virtual bool intersects( shape* s ) = 0;

    //specific intersections
    virtual bool _intersect( sphere* s ) = 0;
    virtual bool _intersect( plane* s ) = 0;
    virtual bool _intersect( aabb* s ) = 0;
};

class sphere : public shape
{
  public:
    //define a sphere by a center and a radius
    mm::vec3 center;
    float radius;

    bool is_on_right_side( plane* p )
    {
      return inner::is_on_right_side( this, p );
    }

    bool _is_inside( shape* a )
    {
      return a->__is_inside(this);
    }

    bool __is_inside( sphere* a )
    {
      return inner::is_inside( this, a );
    }

    bool __is_inside( aabb* a )
    {
      return inner::is_inside( this, a );
    }

    bool intersects( shape* s )
    {
      return s->_intersect( this );
    }

    bool _intersect( sphere* s )
    {
      return inner::intersect( this, s );
    }

    bool _intersect( plane* s )
    {
      return inner::intersect( this, s );
    }

    bool _intersect( aabb* s )
    {
      return inner::intersect( s, this );
    }

    sphere( mm::vec3 c = mm::vec3(), float r = float() ) : center( c ), radius( r ) {}
};

class plane : public shape
{
  public:
    //define a plane by a normal and a point
    mm::vec3 normal, point;
    float d; //cache -(normal dot point)

    bool _is_inside( shape* a )
    {
        //not implemented
        assert(false);
        return false;
    }

    bool __is_inside( sphere* a )
    {
        //not implemented
        assert(false);
        return false;
    }

    bool __is_inside( aabb* a )
    {
        //not implemented
        assert(false);
        return false;
    }

    bool is_on_right_side( plane* p )
    {
        //not implemented
        assert(false);
        return false;
    }

    //define a plane by 3 points
    void set_up( const mm::vec3& a, const mm::vec3& b, const mm::vec3& c )
    {
      mm::vec3 tmp1, tmp2;

      tmp1 = a - b;
      tmp2 = c - b;

      normal = mm::normalize( mm::cross( tmp2, tmp1 ) );
      point = a;
      d = -mm::dot( normal, point );
    }

    //signed distance
    float distance( const mm::vec3& p )
    {
      return d + mm::dot( normal, p );
    }

    bool intersects( shape* s )
    {
      return s->_intersect( this );
    }

    bool _intersect( sphere* s )
    {
      return inner::intersect( s, this );
    }

    bool _intersect( plane* s )
    {
      return inner::intersect( this, s );
    }

    bool _intersect( aabb* s )
    {
      return inner::intersect( s, this );
    }

    //define a plane by a normal and a point
    plane( const mm::vec3& n = mm::vec3(), const mm::vec3& p = mm::vec3() ) : normal( n ), point( p )
    {
      d = -mm::dot( n, p );
    }

    plane( const mm::vec3& a, const mm::vec3& b, const mm::vec3& c )
    {
      set_up( a, b, c );
    }
};

class aabb : public shape
{
  public:
    mm::vec3 pos; //center of the aabb
    mm::vec3 extents; //half-width/height of the aabb
    mm::vec3 min, max; //minimum/maximum apex of the aabb

    //returns the vertices of the triangles of the aabb in counter clockwise order
    std::vector<mm::vec3> get_vertices()
    {
      std::vector<mm::vec3> v;

      //left
      v.push_back( mm::vec3( min.x, max.yz ) );
      v.push_back( mm::vec3( min.x, max.y, min.z ) );
      v.push_back( mm::vec3( min.xyz ) );

      v.push_back( mm::vec3( min.xyz ) );
      v.push_back( mm::vec3( min.xy, max.z ) );
      v.push_back( mm::vec3( min.x, max.yz ) );

      //front
      v.push_back( mm::vec3( min.xy, max.z ) );
      v.push_back( mm::vec3( max.x, min.y, max.z ) );
      v.push_back( mm::vec3( max.xyz ) );

      v.push_back( mm::vec3( max.xyz ) );
      v.push_back( mm::vec3( min.x, max.yz ) );
      v.push_back( mm::vec3( min.xy, max.z ) );

      //right
      v.push_back( mm::vec3( max.xy, min.z ) );
      v.push_back( mm::vec3( max.xyz ) );
      v.push_back( mm::vec3( max.x, min.y, max.z ) );

      v.push_back( mm::vec3( max.x, min.y, max.z ) );
      v.push_back( mm::vec3( max.x, min.yz ) );
      v.push_back( mm::vec3( max.xy, min.z ) );

      //back
      v.push_back( mm::vec3( max.xy, min.z ) );
      v.push_back( mm::vec3( max.x, min.yz ) );
      v.push_back( mm::vec3( min.xyz ) );

      v.push_back( mm::vec3( min.xyz ) );
      v.push_back( mm::vec3( min.x, max.y, min.z ) );
      v.push_back( mm::vec3( max.xy, min.z ) );

      //top
      v.push_back( mm::vec3( min.x, max.y, min.z ) );
      v.push_back( mm::vec3( min.x, max.yz ) );
      v.push_back( mm::vec3( max.xyz ) );

      v.push_back( mm::vec3( max.xyz ) );
      v.push_back( mm::vec3( max.xy, min.z ) );
      v.push_back( mm::vec3( min.x, max.y, min.z ) );

      //bottom
      v.push_back( mm::vec3( max.x, min.y, max.z ) );
      v.push_back( mm::vec3( min.xy, max.z ) );
      v.push_back( mm::vec3( min.xyz ) );

      v.push_back( mm::vec3( min.xyz ) );
      v.push_back( mm::vec3( max.x, min.yz ) );
      v.push_back( mm::vec3( max.x, min.y, max.z ) );

      return v;
    }

    mm::vec3 get_pos_vertex( const mm::vec3& n )
    {
      mm::vec3 res = min;

      if( n.x >= 0 )
        res.x = max.x;

      if( n.y >= 0 )
        res.y = max.y;

      if( n.z >= 0 )
        res.z = max.z;

      return res;
    }

    void expand( const mm::vec3& p )
    {
      min = mm::min( min, p );
      max = mm::max( max, p );
      extents = ( max - min ) / 2.0f;
      pos = min + extents;
    }

    mm::vec3 get_neg_vertex( const mm::vec3& n )
    {
      mm::vec3 res = max;

      if( n.x >= 0 )
        res.x = min.x;

      if( n.y >= 0 )
        res.y = min.y;

      if( n.z >= 0 )
        res.z = min.z;

      return res;
    }

    bool is_on_right_side( plane* p )
    {
      return inner::is_on_right_side( this, p );
    }

    bool _is_inside( shape* a )
    {
      return a->__is_inside(this);
    }

    bool __is_inside( sphere* a )
    {
      return inner::is_inside( this, a );
    }

    bool __is_inside( aabb* a )
    {
      return inner::is_inside( this, a );
    }

    bool intersects( shape* s )
    {
      return s->_intersect( this );
    }

    bool _intersect( sphere* s )
    {
      return inner::intersect( this, s );
    }

    bool _intersect( plane* s )
    {
      return inner::intersect( this, s );
    }

    bool _intersect( aabb* s )
    {
      return inner::intersect( s, this );
    }

    void reset_minmax()
    {
      min = mm::vec3( FLT_MAX );
      max = mm::vec3( -FLT_MAX );
    }

    aabb()
    {
        reset_minmax();
    }

    aabb( const mm::vec3& p, const mm::vec3& e ) : pos( p ), extents( e )
    {
      min = pos - extents;
      max = pos + extents;
    }
};

//haxx
#ifdef _WIN32
#undef FAR
#endif

class frustum : public shape
{
  public:
    shape* planes[6];

    enum type
    {
      TOP = 0, BOTTOM, LEFT, RIGHT, NEAR, FAR
    };

    frustum()
    {
      for( int c = 0; c < 6; ++c )
        planes[c] = new plane();
    }

    bool _is_inside( shape* a )
    {
        //not implemented
        assert(false);
        return false;
    }

    bool __is_inside( sphere* a )
    {
        //not implemented
        assert(false);
        return false;
    }

    bool __is_inside( aabb* a )
    {
        //not implemented
        assert(false);
        return false;
    }

    bool is_on_right_side( plane* p )
    {
        //not implemented
        assert(false);
        return false;
    }

    void set_up( const mm::camera<float>& cam )
    {
      mm::vec3 nc = cam.pos - cam.view_dir * cam.get_frame()->near_ll.z;
      mm::vec3 fc = cam.pos - cam.view_dir * cam.get_frame()->far_ll.z;

      mm::vec3 right = -mm::normalize( mm::cross( cam.up_vector, cam.view_dir ) );

      float nw = cam.get_frame()->near_lr.x - cam.get_frame()->near_ll.x;
      float nh = cam.get_frame()->near_ul.y - cam.get_frame()->near_ll.y;

      float fw = cam.get_frame()->far_lr.x - cam.get_frame()->far_ll.x;
      float fh = cam.get_frame()->far_ul.y - cam.get_frame()->far_ll.y;

      //near top left
      mm::vec3 ntl = nc + cam.up_vector * nh - right * nw;
      mm::vec3 ntr = nc + cam.up_vector * nh + right * nw;
      mm::vec3 nbl = nc - cam.up_vector * nh - right * nw;
      mm::vec3 nbr = nc - cam.up_vector * nh + right * nw;

      mm::vec3 ftl = fc + cam.up_vector * fh - right * fw;
      mm::vec3 ftr = fc + cam.up_vector * fh + right * fw;
      mm::vec3 fbl = fc - cam.up_vector * fh - right * fw;
      mm::vec3 fbr = fc - cam.up_vector * fh + right * fw;

      static_cast<plane*>( planes[TOP] )->set_up( ntr, ntl, ftl );
      static_cast<plane*>( planes[BOTTOM] )->set_up( nbl, nbr, fbr );
      static_cast<plane*>( planes[LEFT] )->set_up( ntl, nbl, fbl );
      static_cast<plane*>( planes[RIGHT] )->set_up( nbr, ntr, fbr );
      static_cast<plane*>( planes[NEAR] )->set_up( ntl, ntr, nbr );
      static_cast<plane*>( planes[FAR] )->set_up( ftr, ftl, fbl );
    }

    bool intersects( shape* s )
    {
      for( int c = 0; c < 6; ++c )
      {
        if( !s->is_on_right_side( ( plane* )planes[c] ) )
          return false;
      }

      return true;
    }

    virtual bool _intersect( sphere* s )
    {
      for( int c = 0; c < 6; ++c )
      {
        if( !inner::intersect( s, ( plane* )planes[c] ) )
          return false;
      }

      return true;
    }

    virtual bool _intersect( plane* s )
    {
      for( int c = 0; c < 6; ++c )
      {
        if( !inner::intersect( s, ( plane* )planes[c] ) )
          return false;
      }

      return true;
    }

    virtual bool _intersect( aabb* s )
    {
      for( int c = 0; c < 6; ++c )
      {
        if( !inner::intersect( s, ( plane* )planes[c] ) )
          return false;
      }

      return true;
    }
};

namespace inner
{
  //only tells if the sphere is on the right side of the plane!
  bool is_on_right_side( sphere* a, plane* b )
  {
    float dist = b->distance( a->center );

    if( dist < -a->radius )
      return false;

    return true;
  }

  //only tells if the sphere is on the right side of the plane!
  bool is_on_right_side( aabb* a, plane* b )
  {
    if( b->distance( a->get_pos_vertex( b->normal ) ) < 0 )
      return false;

    return true;
  }

  bool intersect( sphere* a, sphere* b )
  {
    mm::vec3 diff = a->center - b->center;
    float dist = mm::dot( diff, diff );

    float rad_sum = b->radius + a->radius;

    if( dist > rad_sum * rad_sum ) //squared distance check
      return false;

    return true;
  }

  //checks if a sphere intersects a plane
  bool intersect( sphere* a, plane* b )
  {
    float dist = b->distance( a->center );

    if( abs( dist ) <= a->radius )
      return true;

    return false;
  }

  bool intersect( plane* a, plane* b )
  {
    mm::vec3 vector = mm::cross( a->normal, b->normal );

    //if the cross product yields a null vector
    //then the angle is either 0 or 180
    //that is the two normals are parallel
    // sin(alpha) = 0
    // ==> |a| * |b| * sin(alpha) = 0
    if( mm::equal( vector, mm::vec3( 0 ) ) )
      return false;

    return true;
  }

  bool intersect( aabb* a, aabb* b )
  {
    mm::vec3 t = b->pos - a->pos;

    if( abs( t.x ) > ( a->extents.x + b->extents.x ) )
      return false;

    if( abs( t.y ) > ( a->extents.y + b->extents.y ) )
      return false;

    if( abs( t.z ) > ( a->extents.z + b->extents.z ) )
      return false;

    return true;
  }

  bool intersect( aabb* a, sphere* b )
  {
    //square distance check between spheres and aabbs
    mm::vec3 vec = b->center - mm::clamp( b->center - a->pos, a->min, a->max );

    float sqlength = mm::dot( vec, vec );

    if( sqlength > b->radius * b->radius )
      return false;

    return true;
  }

  bool intersect( aabb* a, plane* b )
  {
    mm::vec3 p = a->get_pos_vertex( b->normal );
    mm::vec3 n = a->get_neg_vertex( b->normal );

    float dist_p = b->distance( p );
    float dist_n = b->distance( n );

    if( ( dist_n > 0 && dist_p > 0 ) ||
        ( dist_n < 0 && dist_p < 0 ) )
      return false;

    return true;
  }

  //is a inside b?
  bool is_inside( sphere* a, aabb* b )
  {
      mm::vec3 spheremax = a->center + a->radius;
      mm::vec3 spheremin = a->center - a->radius;

      if( lessThanEqual(spheremax, b->max) && greaterThanEqual(spheremin, b->min) )
          return true;

      return false;
  }

  //is a inside b?
  bool is_inside( aabb* a, sphere* b )
  {
    mm::vec3 minvec = a->min - b->center;
    mm::vec3 maxvec = a->max - b->center;
    float sqrmaxlength = mm::dot(maxvec, maxvec);
    float sqrminlength = mm::dot(minvec, minvec);
    float sqrradius = b->radius * b->radius;

    if( sqrmaxlength <= sqrradius && sqrminlength <= sqrradius )
        return true;

    return false;
  }

  //is a inside b?
  bool is_inside( aabb* a, aabb* b )
  {
    if( mm::greaterThan( a->min, b->min ) && mm::lessThan( b->max, a->max ) )
      return true;

    return false;
  }

  //is a inside b?
  bool is_inside( sphere* a, sphere* b )
  {
    mm::vec3 spheredist = b->center - a->center;

    if( mm::dot(spheredist, spheredist) <= b->radius * b->radius )
      return true;

    return false;
  }
}

#endif