#ifndef BOXOBBHALFSPACED2_H
#define BOXOBBHALFSPACED2_H

#include <sstream>
using namespace std;

#include <line.h>
#include <typedefs.h>
#include <zero.h>

/*!
\brief OBB and OBB intersection test.

Also a point inside OBB test too.

Arm 1 and 2 are perpendicular unit vectors.  
*/
template< typename PT, typename PD >
class boxOBBhalfspaceD2
{
public:

  /** The centroid. */
  PT center;
  /** The arm is a unit vector. */
  PT arm1;
  /** The arm is a unit vector. */
  PT arm2;
  /** Half the length of the box in arm1 direction. */
  PD arm1len;
  /** Half the length of the box in arm2 direction. */
  PD arm2len;

  /** Exposed points for visualization (Testing) and reuse. */
  static PT qi[4];
  /** Exposed points for visualization (Testing) and reuse. */
  static PT pi[5];


  /** Unconstructed state. */
  boxOBBhalfspaceD2()
    {}

  /** Construct a OBB. */
  boxOBBhalfspaceD2
  (
    PT const & _center,
    PT const & _arm1,
    PT const & _arm2,
    PD const _arm1len,
    PD const _arm2len
  );

  /** Construct a OBB from 4 anticlockwise points. */
  template< typename W >
  void construct
  (
    W const & p0,
    W const & p1,
    W const & p2,
    W const & p3
  );

  /** Is the point inside the box? */
  boolc intersects( PT const & p ) const;

  /** Do the bounding boxes intersect? */
  boolc intersects( boxOBBhalfspaceD2<PT,PD> & B2 );

  /** Does this boxes half-spaces facing outwards see B2? */
  boolc sees( boxOBBhalfspaceD2<PT,PD> & B2 );

  /** Get this boxes corner points, from bottom left anti clockwise. */
  void cornerpoints( PT & p0, PT & p1, PT & p2, PT & p3 ) const;

  /** Serialize this object by writing it out as a string. */
  operator stringc () const;

};


//---------------------------------------------------------
// Implementation

template< typename PT, typename PD >
PT boxOBBhalfspaceD2<PT,PD>::qi[4];

template< typename PT, typename PD >
PT boxOBBhalfspaceD2<PT,PD>::pi[5];

template< typename PT, typename PD >
boxOBBhalfspaceD2<PT,PD>::operator stringc () const
{
  stringstream ss;  

  ss << center << "  " << arm1 << "  " << arm2 << "  ";
  ss << arm1len << "  " << arm2len;

  return ss.str();
}


template< typename PT, typename PD >
void boxOBBhalfspaceD2<PT,PD>::cornerpoints
( 
  PT & p0, 
  PT & p1, 
  PT & p2, 
  PT & p3 
) const
{
  p0 = center - arm1*arm1len - arm2*arm2len;
  p1 = center + arm1*arm1len - arm2*arm2len;
  p2 = center + arm1*arm1len + arm2*arm2len;
  p3 = center - arm1*arm1len + arm2*arm2len;
}

template< typename PT, typename PD >
template< typename W >
void boxOBBhalfspaceD2<PT,PD>::construct
(
  W const & p0,
  W const & p1,
  W const & p2,
  W const & p3
)
{
  center = ((p0+p1+p2+p3)*0.25);
  arm1 = (p1-p0);
  arm1.normalize();
  arm2 = (p3-p0);
  arm2.normalize();
  arm1len = (p0-p1).distance()*0.5;
  arm2len = (p0-p3).distance()*0.5;

  assert( zero<PD>::test((p0+arm1*arm1len*2.0+arm2*arm2len*2.0-p2).dot())==true );
}

template< typename PT, typename PD >
boxOBBhalfspaceD2<PT,PD>::boxOBBhalfspaceD2
(
  PT const & _center,
  PT const & _arm1,
  PT const & _arm2,
  PD const _arm1len,
  PD const _arm2len
)
  : center(_center), arm1(_arm1), arm2(_arm2), arm1len(_arm1len),
  arm2len(_arm2len)
{
  assert( zero<PD>::test(arm1.dot(arm2)) == true );
  assert( zero<PD>::test(arm1.dot())==false );
  assert( zero<PD>::test(arm2.dot())==false );
  assert( zero<PD>::test(arm1.dot()-1.0)==true );
  assert( zero<PD>::test(arm2.dot()-1.0)==true );
}

template< typename PT, typename PD >
boolc boxOBBhalfspaceD2<PT,PD>::intersects
( 
  boxOBBhalfspaceD2<PT,PD> & B2 
) 
{
  if (sees(B2)==false)
    return true;

  return ! B2.sees(*this);
}

template< typename PT, typename PD >
boolc boxOBBhalfspaceD2<PT,PD>::sees
( 
  boxOBBhalfspaceD2<PT,PD> & B2 
) 
{
  B2.cornerpoints(pi[0],pi[1],pi[2],pi[3]);

//cout << SHOW(arm1) << endl;
//cout << SHOW(arm2) << endl;
//cout << SHOW(arm1len) << endl;
//cout << SHOW(arm2len) << endl;

  //static point2<double> qi[4];
  qi[0] = center + arm1*(arm1len);
  qi[1] = center + arm1*(-arm1len);
  qi[2] = center + arm2*(arm2len);
  qi[3] = center + arm2*(-arm2len);

  pi[4] = B2.center;

//cout << SHOW(center) << endl;
//cout << SHOW(qi[2]) << endl;

  uint count=0;
  for (uint k=0; k<5; ++k)
  {
//cout << SHOW(k) << endl;
//cout << SHOW((pi[k]-qi[0]).dot(arm1)>0.0) << endl;
    if ((pi[k]-qi[0]).dot(arm1)>0.0)
    {
      ++count;
      continue;
    }
//cout << SHOW((pi[k]-qi[1]).dot(arm1)<0.0) << endl;
    if ((pi[k]-qi[1]).dot(arm1)<0.0)
    {
      ++count;
      continue;
    }
//cout << SHOW((pi[k]-qi[2]).dot(arm2)>0.0) << endl;
    if ((pi[k]-qi[2]).dot(arm2)>0.0)
    {
      ++count;
      continue;
    }
//cout << SHOW((pi[k]-qi[3]).dot(arm2)<0.0) << endl;
    if ((pi[k]-qi[3]).dot(arm2)<0.0)
    {
      ++count;
    }
  }
//cout << SHOW(count) << endl;

  return (count==5);
}

template< typename PT, typename PD >
boolc boxOBBhalfspaceD2<PT,PD>::intersects( PT const & p ) const
{
  //if ((p-(center+arm1*arm1len)).dot(arm1) > (PD)0)
  if ((p-(center+arm1*arm1len)).dot(arm1) > zero<PD>::val)
    return false;
  //if ((p-(center-arm1*arm1len)).dot(arm1) < (PD)0)
  if (((center-arm1*arm1len)-p).dot(arm1) > zero<PD>::val)
    return false;

  //if ((p-(center+arm2*arm2len)).dot(arm2) > (PD)0)
  if ((p-(center+arm2*arm2len)).dot(arm2) > zero<PD>::val)
    return false;
  //if ((p-(center-arm2*arm2len)).dot(arm2) < (PD)0)
  if (((center-arm2*arm2len)-p).dot(arm2) > zero<PD>::val)
    return false;

  return true;
}


#endif