#ifndef BSPTREED2_H
#define BSPTREED2_H

#include <iostream>
#include <vector>
using namespace std;

#include <halfspaceD2.h>
#include <linechopped.h>
#include <treeindexedD2.h>
#include <treeindexedD2iter.h>

template< typename PT, typename PD, typename INDX >
class bsptreeD2find;

/**
\brief Binary Space Partition tree 2D.

*/
template< typename PT, typename PD, typename INDX >
class bsptreeD2
{
public:

  /** Large in size to represent an infinite line. */
  static PD tmin;
  /** Large in size to represent an infinite line. */
  static PD tmax;

  /** The tree is the bsp tree with indexes to the
      other data structures. The nodes to the half-spaces
      and the regions to whatever the client indexes into. */
  treeindexedD2<INDX> tree;

  /** The half-spaces have the same index as their 
      corresponding nodes. */
  vector< halfspaceD2<PT,PD> > vi;

  /** Are there no halfspaces in the bsp tree? */
  boolc empty() const
    { return (vi.size()==1); }

  /** Construct with a root node and two leafs. */
  bsptreeD2();

  /** First halfspace put in the tree. */
  void addroot( halfspaceD2<PT,PD> const & h );
  /** Insert a halfspace left of mv halfspace. */
  void addleft
  ( 
    INDX const mv, 
    INDX const mvbitlength, 
    halfspaceD2<PT,PD> const & h 
  );
  /** Insert a halfspace right of mv halfspace. */
  void addright
  ( 
    INDX const mv, 
    INDX const mvbitlength, 
    halfspaceD2<PT,PD> const & h 
  );

  /** Find the region given the point. 
      tree.current points to region i. */
  void find( INDX & i, PT const & pt );

  /** Clip the line with the half-spaces. The treeindex
      iterator contains a path which is travelled as
      the line is clipped.  The last node in the path
      is not used for clipping. */
  template< typename treeindexedD2iterINDX >
  void clip
  ( 
    PD & t0, 
    PD & t1, 
    PT const & La, 
    PT const & Lm,
    treeindexedD2iterINDX const & i1
  );

  // <TODO>
  //void find( treeindexedD2node<> & khnd, bool & sees, PT const & pt );

  
  /** Read in half-spaces to build the tree.  eg
withindexes=1
4
0.0 -1.0 0.0 1.0    0 0
-2.0 1.0 -1.0 1.0   5 1
1.0 -0.5 2.0 -0.5   0 2
4.0 0.0 4.0 1.0     4 3
or without indexes
withindexes=0
4
0.0 -1.0 0.0 1.0   
-2.0 1.0 -1.0 1.0 
1.0 -0.5 2.0 -0.5
4.0 0.0 4.0 1.0   
  */
  istream & serializeInverse(istream & istr);
  
  /** The half-spaces chop the line segment from p0 to p1 into
      a list of line segments where intersection ocurres. */
  void find
  ( 
    linechoppedindexed<PT,PD,INDX> & L, 
    PT const & p0,
    PT const & p1
  );

  /** Each half-space must intersect its parent. */
  boolc validhalfspaces
  ( 
    INDX & errorh, 
    INDX & errorhparent 
  );

  void visibility
  ( 
    vector<INDX> & vnds, 
    PT const & p0 
  ) const;

private:

  /** The indexes are for nodes furthest to nearest point p0. */
  void visibility
  ( 
    vector<INDX> & vnds, 
    treeindexedD2node<INDX> const * nd,
    PT const & p0 
  ) const;

  boolc serializeInversePre
  ( 
    istream & istr,
    bool & withindexes,
    uint & n
  );

};


template< typename PT, typename PD, typename INDX >
istream & operator >> (istream & istr, bsptreeD2<PT,PD,INDX> & x)
  { return x.serializeInverse(istr); }


/*!
\brief Search the bsp tree.
*/
template< typename PT, typename PD, typename INDX >
class bsptreeD2find
{
public:

  /** The bsp tree. */
  bsptreeD2<PT,PD,INDX> & bsp;

  /** List of nodes defining the current path from the
      last find. */
  vector< treeindexedD2node<INDX> * > path;

  /** Pass the bsp tree reference. */
  bsptreeD2find( bsptreeD2<PT,PD,INDX> & bsp_ )
    : bsp(bsp_) {}
  
  /** Find the index i that the point pt is in. */
  void find(  INDX & i, PT const & pt ); 

};


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

template< typename PT, typename PD, typename INDX >
void bsptreeD2<PT,PD,INDX>::addroot( halfspaceD2<PT,PD> const & h )
{
  assert(vi.size()==1);

  vi.push_back(h);
}

template< typename PT, typename PD, typename INDX >
void bsptreeD2<PT,PD,INDX>::addleft
( 
  INDX const mv, 
  INDX const mvbitlength, 
  halfspaceD2<PT,PD> const & h 
)
{
  tree.move(mv,mvbitlength);
  tree.addleft();
  vi.push_back(h);
}

template< typename PT, typename PD, typename INDX >
void bsptreeD2<PT,PD,INDX>::addright
( 
  INDX const mv, 
  INDX const mvbitlength, 
  halfspaceD2<PT,PD> const & h 
)
{
  tree.move(mv,mvbitlength);
  tree.addright();
  vi.push_back(h);
}


template< typename PT, typename PD, typename INDX >
void bsptreeD2<PT,PD,INDX>::visibility
( 
  vector<INDX> & vnds, 
  PT const & p0 
) const
{
  vnds.clear();

  if (tree.root==0)
    return;

  visibility(vnds,tree.root,p0);
}

template< typename PT, typename PD, typename INDX >
void bsptreeD2<PT,PD,INDX>::visibility
( 
  vector<INDX> & vnds, 
  treeindexedD2node<INDX> const * nd,
  PT const & p0 
) const
{
  assert(nd!=0);

  // Is the node internal?
  if (nd->isleaf()==false)
  {
    halfspaceD2<PT,PD> const & h(vi[nd->index]);
    if (h.isInside(p0))
    {
      visibility(vnds,nd->right,p0);
      visibility(vnds,nd->left,p0);
    }
    else
    {
      visibility(vnds,nd->left,p0);
      visibility(vnds,nd->right,p0);
    }
  }
  else
    vnds.push_back(nd->index);
}

template< typename PT, typename PD, typename INDX >
void bsptreeD2find<PT,PD,INDX>::find
( 
  INDX & i, 
  PT const & pt 
) 
{
  treeindexedD2<INDX> & tree(bsp.tree);
  vector< halfspaceD2<PT,PD> > & vi(bsp.vi);

  tree.reset();

  assert(tree.current!=0);

  while (tree.current->isleaf() == false)
  {
    assert(tree.current->index < vi.size());

    path.push_back(tree.current);
    if (vi[tree.current->index].isInside(pt))
      tree.moveleft();
    else
      tree.moveright();
  
    assert(tree.current!=0);
  }

  path.push_back(tree.current);
  i = tree.current->index;
}




template< typename PT, typename PD, typename INDX >
template< typename treeindexedD2iterINDX >
void bsptreeD2<PT,PD,INDX>::clip
( 
  PD & t0, 
  PD & t1, 
  PT const & La, 
  PT const & Lm,
  treeindexedD2iterINDX const & i1
)
{
  INDX index;
  INDX index2;

  uint sz = i1.path.size();

  if (sz<=1)
    return;
  
  --sz;

//cout << SHOW(t0) << " " SHOW(t1) << " ";
//cout << SHOW(La) << " " << SHOW(Lm) << endl;

  for (uint i=0; i<sz; ++i)
  {
    //cout << SHOW(i1.path[i]->index) << endl;
    index = i1.path[i]->index;
    index2 = i1.path[i+1]->index;
//cout << SHOW(index) << endl;
//cout << SHOW(index2) << endl;

    if ( i1.path[i]->isleftinternal(index2) )
      vi[index].clip(t0,t1,La,Lm);
    else
    {
      if ( i1.path[i]->isrightinternal(index2) )
      {
        vi[index].clipNeg(t0,t1,La,Lm);
      }
      else
        assert(false);
    }
  }

}


template< typename PT, typename PD, typename INDX >
boolc bsptreeD2<PT,PD,INDX>::serializeInversePre
( 
  istream & istr,
  bool & withindexes,
  uint & n
)
{
  withindexes=false;
  bool withindexestag(false);

  string s;
  istr >> s;
  string::size_type pos=s.find("withindexes=");
  if (pos==string::npos)
    return istr;

  s.erase(pos,12);
  if (s=="1")
  {
    withindexes=true;
    withindexestag=true;
  }

  if (s=="true")
  {
    withindexes=true;
    withindexestag=true;
  }

  if ((s=="0")||(s=="false"))
    withindexestag=true;

  if (withindexestag==false)
    return false;
    
  istr >> n;

  if (n==0)
    return false;

  return true;
}


template< typename PT, typename PD, typename INDX >
boolc bsptreeD2<PT,PD,INDX>::validhalfspaces
(
  INDX & errorh,
  INDX & errorhparent 
)
{
  PT La;
  PT Lm;
  PT Lb;

  PD t0;
  PD t1;

  PD q0;
  PD q1;

  for ( treeindexedD2iterinternal<INDX> i1(tree); 
    !i1; ++i1 )
  {
    halfspaceD2<PT,PD> & h1(vi[i1()->index]);

    Lm = h1.p1-h1.p0;
    La = h1.p0;
    Lb = h1.p1;

    t0 = tmin;
    t1 = tmax;

    uint sz = i1.path.size();
    if (sz==0)
      continue;

    clip(t0,t1,La,Lm,i1);

    line<PT,PD> L1(Lm,La);

//cout << SHOW(i1()->index) << endl;
    errorhparent = i1()->index;
//cout << SHOW(errorhparent) << endl;

    // If the left node is a half-space, test that it
    // intersects its parent.
    if (i1()->left->isleaf()==false)
    {
      errorh = i1()->left->index;

      halfspaceD2<PT,PD> & h2(vi[i1()->left->index]);
      line<PT,PD> L2(h2.p1-h2.p0,h2.p0);
      if (L1.intersects(q0,q1,L2)==false)
        return false;
      
      // Is q0 in [t0,t1]?
      if (q0<t0)
        return false;

      if (q0>t1)
        return false;
    }

    if (i1()->right->isleaf()==false)
    {
      errorh = i1()->right->index;

      halfspaceD2<PT,PD> & h2(vi[i1()->right->index]);
      line<PT,PD> L2(h2.p1-h2.p0,h2.p0);
      if (L1.intersects(q0,q1,L2)==false)
        return false;
      
      // Is q0 in [t0,t1]?
      if (q0<t0)
        return false;

      if (q0>t1)
        return false;
    }
  }

  return true;
}

template< typename PT, typename PD, typename INDX >
istream & bsptreeD2<PT,PD,INDX>::serializeInverse(istream & istr)
{
  tree.construct();

  bool withindexes;
  uint n;
  if (serializeInversePre(istr,withindexes,n)==false)
    return istr;

  PT p0;
  PT p1;

  INDX i0;
  INDX i1;

  treeindexedD2node<INDX> * parent;

  istr >> p0;
  istr >> p1;
  vi.push_back(halfspaceD2<PT,PD>(p0,p1));
  if (withindexes)
  {
    istr >> i0;
    istr >> i1;
    tree.root->left->index = i0;
    tree.root->right->index = i1;
  }

  if (n==1)
    return istr;

  INDX ifind;

  bsptreeD2find<PT,PD,INDX> bspf(*this);

  for (uint i=1; i<n; ++i)
  {
    istr >> p0;
    istr >> p1;
    vi.push_back(halfspaceD2<PT,PD>(p0,p1));

//cout << SHOW( (stringc)tree ) << endl << endl;

    bspf.find(ifind,p0);
    parent = bspf.path[ bspf.path.size()-2 ];
    tree.current = parent;
    assert(parent!=0);

    if ((ifind==parent->left->index)&&(parent->left->isleaf()))
    {
      tree.addleft();

      if (withindexes)
      {
        istr >> i0;
        istr >> i1;

        tree.moveleft();
        tree.current->left->index=i0;
        tree.current->right->index=i1;
      }

      continue;
    }

    if ((ifind==parent->right->index)&&(parent->right->isleaf()))
    {
      tree.addright();

      if (withindexes)
      {
        istr >> i0;
        istr >> i1;

        tree.moveright();
        tree.current->left->index=i0;
        tree.current->right->index=i1;
      }

      continue;
    }

    assert(false);
  }

//INDX errorh, errorhparent;
//cout << SHOW(validhalfspaces(errorh,errorhparent)) << "  ";
//cout << SHOW(errorh) << " " << SHOW(errorhparent) << endl;

  return istr;
}

template< typename PT, typename PD, typename INDX >
void bsptreeD2<PT,PD,INDX>::find( INDX & i, PT const & pt ) 
{
  tree.reset();

  assert(tree.current!=0);

  while (tree.current->isleaf() == false)
  {
    assert(tree.current->index < vi.size());
    if (vi[tree.current->index].isInside(pt))
      tree.moveleft();
    else
      tree.moveright();
  
    assert(tree.current!=0);
  }

  i = tree.current->index;
}

template< typename PT, typename PD, typename INDX >
bsptreeD2<PT,PD,INDX>::bsptreeD2()
{
  vi.push_back( halfspaceD2<PT,PD>() );
}


template< typename PT, typename PD, typename INDX >
void bsptreeD2<PT,PD,INDX>::find
( 
  linechoppedindexed<PT,PD,INDX> & L, 
  PT const & p0,
  PT const & p1
)
{
  //<TODO> What is happening?
  assert(false);

  L.ln.constructfromendpoints(p0,p1);
  L.chain.clear();
  L.chain.push_back
  ( 
    pointindexed< point2<PD>, INDX >( point2<PD>(0.0,1.0), (INDX)0)
  ); 

}


#endif