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



#include <bsptreeD2disp01.h>
#include <bsptreeD2disp02.h>
#include <bsptreeD2disp03.h>
#include <bsptreeD2dispregions01.h>
#include <commandline.h>
#include <push_back.h>
#include <tokenizer.h>
#include <treeindexedD2iter.h>
#include <treeindexedD2test.h>
#include <typedefs.h>
#include <zpr.h>

template<>
double zero<double>::val = 1E-12;

point2<double> treeindexedD2test::targetprev;
point2<double> treeindexedD2test::target;
gobjMySphereDraw * treeindexedD2test::targetsphere = 0;
gobjContainer * treeindexedD2test::targetg = 0;

template<>
bsptreeD2<treeindexedD2test::pt2,double,uint> treeindexedD2test::bsp;

bool * treeindexedD2test::help = 0;

vector< boxOBBhalfspaceD2< treeindexedD2test::pt2, double > > 
  treeindexedD2test::viboxes;
vector< vector< uint > > treeindexedD2test::viboxesintersections;

menusystem * treeindexedD2test::messages = 0;

double treeindexedD2test::keyboarddelta = 0.1;

template<>
double bsptreeD2<treeindexedD2test::pt2,double,uint>::tmin = -10000.0;
template<>
double bsptreeD2<treeindexedD2test::pt2,double,uint>::tmax =  10000.0;


void treeindexedD2test::test01()
{
  cout << "Testing treeindexedD2 printing and addleft, addright." << endl;

  treeindexedD2<uint> t1;

  cout << "Created a root node." << endl;
  cout << (stringc)t1 << endl << endl;

  cout << "addleft" << endl;
  t1.addleft();
  cout << (stringc)t1 << endl << endl;

  cout << "Created another root node." << endl;
  treeindexedD2<uint> t2;
  cout << (stringc)t2 << endl << endl;
  cout << "addright" << endl;
  t2.addright();
  cout << (stringc)t2 << endl << endl;


}


void treeindexedD2test::test02()
{
  cout << "Builing and printing a simple tree." << endl;
  treeindexedD2<uint> t1;

  t1.addleft();
  cout << "addleft" << endl;
  cout << (stringc)t1 << endl;

  t1.moveleft();
  t1.addleft();
  cout << "moveleft, addleft" << endl;
  cout << (stringc)t1 << endl;

  t1.reset();
  t1.moveleft();
  t1.addright();
  cout << "reset, moveleft, addright" << endl;

  cout << (stringc)t1 << endl;

}


int treeindexedD2test::unittest01(int argc, char** argv)
{
  commandline cmd(argc,argv);

  string in;
  cmd.mapvar(in,"in");
  if ( in.empty() )
  {
    cout << "error: in=filename expected" << endl;
    return 1;
  }

  ifstream filein(in.c_str());
  assert(filein.good()==true);
  if (filein.good()==false)
  {
    cout << "error: " << in << " file can not be opened." << endl;
    return 1;
  }

  treeindexedD2<uint> t1;
  filein >> t1;

  cout << (stringc)t1 << endl;

  string s1;
  assertreturnOS(tokenizermisc::readfile(s1,in.c_str()));

  string s2( (stringc)t1 );

  assertreturnOS(tokenizermisc::comparewithoutspace(s1,s2));

  return 0;
}

int treeindexedD2test::unittest02()
{
  typedef point2<double> pt2;
  bsptreeD2<pt2,double,uint> t1; 

  cout << "Building a simple 2D partition." << endl;
  cout << " Using manual insertion commands : move to the node before" << endl;
  cout << " inserting new node, then adding associated half-space to vi." << endl << endl;

  t1.vi.push_back( 
    halfspaceD2<pt2,double>(pt2(0.0,0.0),pt2(0.0,1.0)) );

  t1.tree.reset();
  t1.tree.addleft();
  t1.vi.push_back(
    halfspaceD2<pt2,double>(pt2(-1.0,1.0),pt2(0.0,1.0)) );

  t1.tree.reset();
  t1.tree.addright();
  t1.vi.push_back(
    halfspaceD2<pt2,double>(pt2(0.0,-0.5),pt2(1.0,-0.5)) );

  t1.tree.reset();
  t1.tree.moveright();
  t1.tree.addleft();
  t1.vi.push_back(
    halfspaceD2<pt2,double>(pt2(4.0,0.0),pt2(4.0,1.0)) );

  cout << (stringc)t1.tree << endl << endl;

  cout << "Testing if points are in correct regions." << endl;

  uint i[5];
  t1.find(i[0],pt2(1.0,1.0));
  assertreturnOS(i[0]==2);
  t1.find(i[1],pt2(-2.0,-10.0));
  assertreturnOS(i[1]==3);
  t1.find(i[2],pt2(-5.0,-5.0));
  assertreturnOS(i[2]==3);
  t1.find(i[3],pt2(1.0,-2.0));
  assertreturnOS(i[3]==4);
  t1.find(i[4],pt2(6.0,2.0));
  assertreturnOS(i[4]==5);

  return 0;
}


void treeindexedD2test::keyboard03
(
  unsigned char key, 
  int x, 
  int y
)
{
  switch (key)
  {
    case 27: exit(0); break;

    case '+': keyboarddelta *= 10.0; if (keyboarddelta==0.0) keyboarddelta=0.1; break;
    case '-': keyboarddelta /= 10.0; break;

    case 'h': if (help!=0) *help = !*help; break;
  }

  update03();
}


// http://www.codeproject.com/KB/openGL/GLUT_WINDOW_TEMPLATE.aspx
void treeindexedD2test::special03(int key, int x, int y)
{
  switch (key)
  {
    case GLUT_KEY_RIGHT : target.x += keyboarddelta; break; 
    case GLUT_KEY_LEFT : target.x -= keyboarddelta; break; 
    case GLUT_KEY_UP : target.y += keyboarddelta; break; 
    case GLUT_KEY_DOWN : target.y -= keyboarddelta; break; 
  }

  update03();
}


void treeindexedD2test::update03()
{
  assert(targetg!=0);
  assert(targetsphere!=0);

  targetg->kill(1);
  targetg->nuke();
  
  uint i;
  bsp.find(i,target);
  switch (i)
  {
    case 1: targetg->push(new gobjglColor3ub(220,20,60)); break;
    case 2: targetg->push(new gobjglColor3ub(0,206,209)); break;
    case 3: targetg->push(new gobjglColor3ub(255,140,0)); break;
    case 4: targetg->push(new gobjglColor3ub(143,188,139)); break;
    case 5: targetg->push(new gobjglColor3ub(184,134,11)); break;

    default:
      targetg->push(new gobjglColor3ub(255,0,0));
  }

  targetg->push(targetsphere);

  targetsphere->x = target.x;
  targetsphere->y = target.y;

  glutPostRedisplay();
}

void treeindexedD2test::display01()
{ 
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  gobj::global->draw();


  glerrordisplay();
  
  glutSwapBuffers();
}


void treeindexedD2test::test03(int argc, char** argv)
{
  glutInit(&argc,argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowSize(800,600);
  glutCreateWindow("");
  glutDisplayFunc(display01);

// TURNED OFF FOR TESTING.
  glutKeyboardFunc(keyboard03);
  glutSpecialFunc(special03);

  OpenGLinitialisation();

  glEnable(GL_CULL_FACE);
  glEnable(GL_NORMALIZE);

  xGraphics.set();

  commandline cmd(argc,argv);

  gobjpush(new myaxes(1.0));

  targetg = new gobjContainer(true);

  gobjpush(targetg);


  gobjQuadric * q2 = new gobjQuadric();
  q2->radius = 0.2;
  q2->slices=30;
  q2->loops=7;
  gobjpush(q2);

  targetsphere = new gobjMySphereDraw(target,q2);



  typedef point2<double> pt2;

  cout << "Building a simple 2D partition." << endl;
  cout << " Using manual insertion commands : move to the node before" << endl;
  cout << " inserting new node, then adding associated half-space to vi." << endl << endl;

  bsp.vi.push_back( 
    halfspaceD2<pt2,double>(pt2(0.0,0.0),pt2(0.0,1.0)) );

  bsp.tree.reset();
  bsp.tree.addleft();
  bsp.vi.push_back(
    halfspaceD2<pt2,double>(pt2(-1.0,1.0),pt2(0.0,1.0)) );

  bsp.tree.reset();
  bsp.tree.addright();
  bsp.vi.push_back(
    halfspaceD2<pt2,double>(pt2(0.0,-0.5),pt2(1.0,-0.5)) );

  bsp.tree.reset();
  bsp.tree.moveright();
  bsp.tree.addleft();
  bsp.vi.push_back(
    halfspaceD2<pt2,double>(pt2(4.0,0.0),pt2(4.0,1.0)) );

  

  bsptreeD2disp03<pt2,double,uint> * disp
    = new bsptreeD2disp03<pt2,double,uint>(bsp,0.02);
    //= new bsptreeD2disp03<pt2,double,uint>(bsp,-10000.0,10000.0,0.02);
  disp->update();
  gobjpush(disp);
  
  bsptreeD2dispregions01<pt2,double,uint> *dispregions
    = new bsptreeD2dispregions01<pt2,double,uint>(bsp);
  dispregions->delta=0.4;
  dispregions->update();
  gobjpush(dispregions);
  
  gobjpush(new gobjglDisable(GL_LIGHTING));


  menusystem * menu = 
    new menusystem(point2<GLint>(60,30),10);
  menu->fontcolor = point4<float>(218.0/255.0,165.0/255.0,32.0/255.0,0.75);

  menu->fontcolorenable();

  gobjSwitch<> * menuswitch = new gobjSwitch<>(menu,true);
  help = & menuswitch->isdrawn;
  gobjpush(menuswitch);


  gobjpush(new gobjglEnable(GL_LIGHTING));

  menu->addfont12("BSP Tree Demo",2);

  menu->addfont10("[page up] move up",1);
  menu->addfont10("[page down] move down",1);
  menu->addfont10("[< home] move left",1);
  menu->addfont10("[end >] move right",2);

//  menu->addfont10("j J   Move ball right or left.",1);
//  menu->addfont10("k K   Move ball up or down.",2);

  menu->addfont10("+ -   Increase or decrease change.",1);
  menu->addfont10("h    Toggle this help menu.",2);
  menu->addfont10("ESC      Quit");



  gobjpush(new gobjglEnable(GL_LIGHTING));

//  glMatrixMode(GL_MODELVIEW);
//  glLoadIdentity();

  zpr zz;

// Unable to work out why this does not work.
//  glMatrixMode(GL_MODELVIEW);
//  glLoadIdentity();
//  gluLookAt(
//    0.0,0.0,0.0,  // COP target
//    0.0,0.0,5.0,  // Reference
//    0.0,1.0,0.0); // Up

  zz.update();

  update03();

  glutMainLoop();

}

int treeindexedD2test::unittest04()
{
  treeindexedD2<uint> t1;
  string s(
"4 \
1 2 2 0 0 \
2 3 4 1 0 \
3 1 4 2 0 \
4 3 5 2 1");
  stringstream ss(s);
  ss >> t1;
  
  cout << (stringc)t1 << endl << endl;

  t1.move(0*1+1*2,2);
  assertreturnOS(t1.current->index==4);
  t1.move(1*1,1);
  assertreturnOS(t1.current->index==2);
  t1.move(0*1+1*2+0*4,3);
  assertreturnOS(t1.current->index==3);
  t1.move(0*1+1*2+1*4,3);
  assertreturnOS(t1.current->index==5);
  t1.move(0*1+0*2+1*4,3);
  assertreturnOS(t1.current->index==4);
  t1.move(0*1+0*2+0*4,3);
  assertreturnOS(t1.current->index==1);

  vector< uint > v;
  t1.pathindex(v,0*1+0*2+0*4,3);
  assertreturnOS(v[0]==1);
  assertreturnOS(v[1]==2);
  assertreturnOS(v[2]==3);
  t1.pathindex(v,0*1+1*2+1*4,3);
  assertreturnOS(v[0]==1);
  assertreturnOS(v[1]==2);
  assertreturnOS(v[2]==4);

  return 0;
}

void treeindexedD2test::test04()
{
  treeindexedD2<uint> t1;
  string s(
"4 \
1 2 2 0 0 \
2 3 4 1 0 \
3 1 4 2 0 \
4 3 5 2 1");
  stringstream ss(s);
  ss >> t1;


  treeindexedD2iter<uint> i1(t1,false);

  cout << "Iterating over every node." << endl;
  for ( i1.reset(); !i1; ++i1)
    cout << i1()->index << " ";
  cout << endl << endl;

  cout << "Iterating over the leaves." << endl;
  for ( i1.reset(); !i1; ++i1)
    if (i1()->isleaf())
      cout << i1()->index << " ";
  cout << endl << endl;
  
  cout << "Iterating over the internal nodes." << endl;
  for ( i1.reset(); !i1; ++i1)
    if (!i1()->isleaf())
      cout << i1()->index << " ";
  cout << endl << endl;


  cout << "Iterating over the leaves." << endl;
  treeindexedD2iterleaf<uint> i2(t1);
  for ( i2.reset(); !i2; ++i2)
    cout << i2()->index << " ";
  cout << endl << endl;

  cout << "Iterating over the internal nodes." << endl;
  for ( treeindexedD2iterleaf<uint> i3(t1); !i3; ++i3 )
    cout << i3()->index << " ";  
  cout << endl << endl;
}

void treeindexedD2test::keyboard05
(
  unsigned char key, 
  int x, 
  int y
)
{
  switch (key)
  {
    case 27: exit(0); break;

    case '+': keyboarddelta *= 10.0; if (keyboarddelta==0.0) keyboarddelta=0.1; break;
    case '-': keyboarddelta /= 10.0; break;

    case 't': messages->addfont12start( 
                (stringc)target,1); break;
    case 'r': 
    {
      uint reg;
      bsp.find(reg,target);
      { 
        stringstream ss; 
        ss << reg;
        messages->addfont12start(ss.str(),1); 
      }
    }
    break;

    case 'h': if (help!=0) *help = !*help; break;

    case 'T': 
      messages->addfont10blockstart((stringc)bsp.tree,1); 
      cout << (stringc)bsp.tree << endl;
      break;

    case ' ':messages->scrolldown(); break;
    case 'v':
    {
      vector<uint> vis;
      bsp.visibility(vis,target);
      string str;
      for (uint i=0; i<vis.size(); ++i)
      {
        stringstream ss;
        ss << vis[i];
        str += ss.str();
        str += " ";
      }
      messages->addfont10paragraphstart(str,50);
    }
  }

  update05();
}


// http://www.codeproject.com/KB/openGL/GLUT_WINDOW_TEMPLATE.aspx
void treeindexedD2test::special05(int key, int x, int y)
{
  switch (key)
  {
    case GLUT_KEY_RIGHT : 
      targetprev = target; 
      target.x += keyboarddelta; break; 
    case GLUT_KEY_LEFT : 
      targetprev = target;
      target.x -= keyboarddelta; break; 
    case GLUT_KEY_UP : targetprev = target;
              target.y += keyboarddelta; break; 
    case GLUT_KEY_DOWN : targetprev = target;
              target.y -= keyboarddelta; break;
  }

  update05();
}


void treeindexedD2test::update05()
{
  assert(targetsphere!=0);

  uint i;
  bsp.find(i,target);

  vector<uint> & v2(viboxesintersections[i]);
  uintc sz = v2.size();
  for (uint k=0; k<sz; ++k)
  {
    if (viboxes[ v2[k] ].intersects(target))
    {
      target = targetprev;
      break;
    }
  }

  targetsphere->x = target.x;
  targetsphere->y = target.y;

  glutPostRedisplay();
}

void treeindexedD2test::viboxesbuild()
{
  viboxes.clear();

  typedef point2<double> pt2;

  // Some boxes in the scene. Each box is a no entry region.
  // Each row is a box of anti-clockwise 2D points.
  double boxes[] = 
  {
    0.0,0.0,  2.0,0.0,  2.0,2.0,  0.0,2.0,
    6.0,0.0,  7.0,0.0,  7.0,3.0,  6.0,3.0,
    5.0,4.0,  7.0,4.0,  7.0,6.0,  5.0,6.0,
    3.0,1.0,  5.0,1.0,  5.0,6.0,  3.0,6.0,
    1.0,3.0,  3.0,3.0,  3.0,4.0,  1.0,4.0,
    0.0,5.0,  2.0,5.0,  2.0,6.0,  0.0,6.0
  };

  boxOBBhalfspaceD2< pt2, double > b;

  // The first index is a dummy variable because
  //   0 is used to indicate no object.
  viboxes.push_back( b );

  for (uint i=0; i<6; ++i)
  {
    b.construct
    ( 
      pt2(boxes[i*8+0],boxes[i*8+1]),
      pt2(boxes[i*8+2],boxes[i*8+3]),
      pt2(boxes[i*8+4],boxes[i*8+5]),
      pt2(boxes[i*8+6],boxes[i*8+7])
    );
    viboxes.push_back(b);
  }

  // This map knowledge could have been put
  // into a script and read in.

  viboxesintersections.resize(17);

  push_back(viboxesintersections[1],3,6);
  push_back(viboxesintersections[2],3,4,5);
  push_back(viboxesintersections[3],3,4,5);
  push_back(viboxesintersections[4],2,3,4);
  push_back(viboxesintersections[5],2,4);
  push_back(viboxesintersections[6],1,5,6);
  push_back(viboxesintersections[7],4);
  push_back(viboxesintersections[8],2);
  push_back(viboxesintersections[9],6,5);
  push_back(viboxesintersections[10],1,5);
  push_back(viboxesintersections[11],1,4,5);
  push_back(viboxesintersections[12],1);
  push_back(viboxesintersections[13],1);
  push_back(viboxesintersections[14],3,4,6,5);
  push_back(viboxesintersections[15],5);
  push_back(viboxesintersections[16],3,4,5);
  
}

  
int treeindexedD2test::test05(int argc, char** argv)
{
  glutInit(&argc,argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowSize(800,600);
  glutCreateWindow("");
  glutDisplayFunc(display01);
  glutKeyboardFunc(keyboard05);
  glutSpecialFunc(special05);

  OpenGLinitialisation();

  glEnable(GL_DEPTH_TEST);
  glEnable(GL_CULL_FACE);
  glEnable(GL_NORMALIZE);

  xGraphics.set();

  commandline cmd(argc,argv);

  typedef point2<double> pt2;

  viboxesbuild();

  vector<string> ptlabels;
  vector<pt2> ptlist;
  for (uint i=1; i<viboxes.size(); ++i)
  {
    {
      stringstream ss;
      ss << i;
      ptlabels.push_back(ss.str());
      ptlist.push_back(viboxes[i].center);
    }
  }
  gobjpush(new gobjglPushAttrib(GL_LIGHTING));
  gobjpush(new gobjglPushAttrib(GL_CURRENT_BIT));
  gobjpush(new gobjglDisable(GL_LIGHTING));

  gobjpush(new gobjglColor3f(0.0,0.0,1.0));
  pointsdisplay2D< pt2 > pd( *gobj::global, ptlist, ptlabels );

  gobjpush(new gobjglPopAttrib());
  gobjpush(new gobjglPopAttrib());

  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);

  gobjContainer * geom = new gobjContainer(true);
  //geom->push(new gobjglEnable(GL_BLEND));
  //geom->push(
  //  new gobjglBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) );
  geom->push(new gobjglColor4f(0.0,0.0,191.0/255.0,0.8));
  geom->push(new gobjglEnable(GL_LIGHTING));
  geom->push(new gobjglBegin(GL_QUADS));

  pt2 pi[4];
  for (uint k=1; k<viboxes.size(); ++k)
  {
    viboxes[k].cornerpoints(pi[0],pi[1],pi[2],pi[3]);
    for (uint i=0; i<4; ++i)
      geom->push(new gobjglVertex2f(pi[i].x,pi[i].y));
  }
   
  geom->push(new gobjglEnd());
  //geom->push(new gobjglDisable(GL_BLEND));

  gobjpush(geom);

  gridsquare * gs = new gridsquare(1,0xaaaa,7,6,7.0,6.0);
  gobjpush(gs);


  string in;
  cmd.mapvar(in,"in");
  if ( in.empty() )
  {
    cout << "error: in=filename expected" << endl;
    return 1;
  }

  ifstream filein(in.c_str());
  assert(filein.good()==true);
  if (filein.good()==false)
  {
    cout << "error: " << in << " file can not be opened." << endl;
    return 1;
  }

  messages = 
      new menusystem(0,0,true,point2<GLint>(60,250),10);
  gobjpush(messages);
  messages->fontcolor = point4<float>(1.0,0.0,0.0,0.8);
  messages->fontcolorenable();
  messages->lightingdisable();

  filein >> bsp;
  uint errorh, errorhparent;
  //cout << SHOW( << "  ";
  //cout << SHOW(errorh) << " " << SHOW(errorhparent) << endl;
  bool errorres = bsp.validhalfspaces(errorh,errorhparent);
  if (errorres==false)
  {
    string str;
    str += "error:  errorh=";
    { stringstream ss; ss << errorh; str += ss.str(); }
    str += " errorhparent=";
    { stringstream ss; ss << errorhparent; str += ss.str(); }
    
    messages->addfont12start(str,1);
  }

  gobjpush(new gobjglDisable(GL_LIGHTING));
  bsptreeD2disp03<pt2,double,uint> * disp
    = new bsptreeD2disp03<pt2,double,uint>(bsp,0.04);
  disp->update();
  gobjpush(disp);
  
  bsptreeD2dispregions01<pt2,double,uint> *dispregions
    = new bsptreeD2dispregions01<pt2,double,uint>(bsp);
  dispregions->delta=0.4;
  dispregions->update();
  gobjpush(dispregions);

//cout << (stringc)bsp.tree << endl;

  gobjQuadric * q2 = new gobjQuadric();
  q2->radius = 0.1;
  q2->slices=30;
  q2->loops=7;
  gobjpush(q2);

  gobjpush(new gobjglPushAttrib(GL_CURRENT_BIT));
  gobjpush(new gobjglPushAttrib(GL_LIGHTING));
  gobjpush(new gobjglEnable(GL_LIGHTING));
  targetsphere = new gobjMySphereDraw(target,q2);
  gobjpush( new 
    gobjglColor4f( 153.0/255.0, 50.0/255.0, 204.0/255.0, 0.92));
  gobjpush(targetsphere);
  gobjpush( new gobjglPopAttrib() );
  gobjpush( new gobjglPopAttrib() );

  target=pt2(2.5,0.5);
  update05();

  //gobjpush(new gobjglDisable(GL_LIGHTING));
  menusystem * menu = 
    new menusystem(0,0,true,point2<GLint>(60,30),10);
  menu->fontcolor = point4<float>(218.0/255.0,165.0/255.0,32.0/255.0,0.75);

  gobjSwitch<> * menuswitch = new gobjSwitch<>(menu,true);
  help = & menuswitch->isdrawn;
  gobjpush(menuswitch);
  //gobjpush(new gobjglEnable(GL_LIGHTING));

  menu->addfont12("BSP Tree Demo",2);

  menu->addfont10("[page up] move up",1);
  menu->addfont10("[page down] move down",1);
  menu->addfont10("[< home] move left",1);
  menu->addfont10("[end >] move right",2);
//  menu->addfont10("j k   Move ball left or right.",1);
//  menu->addfont10("d f   Move ball down or up.",2);

  menu->addfont10("t     Print the targets position.",1);
  menu->addfont10("r     Print the targets region.",1);
  menu->addfont10("T     Print the tree.",1);
  menu->addfont10("v     Print the visibility",2);

  menu->addfont10("+ -   Increase or decrease change.",1);
  menu->addfont10("h    Toggle this help menu.",2);
  menu->addfont10("ESC      Quit");
  
  zpr zz;

  zz.update();
  glutMainLoop();

  return 0;
}