#include <cassert>
#include <fstream>
#include <iostream>
using namespace std;


#include <boxOBBhalfspaceD2.h>
#include <boxOBBhalfspaceD2test.h>
#include <commandline.h>
#include <pointsdisplay.h>
#include <graphmisc.h>
#include <menusystem.h>
#include <zpr.h>


boxOBBhalfspaceD2<point2<double>,double> * boxOBBhalfspaceD2test::Aptr;
boxOBBhalfspaceD2<point2<double>,double> * boxOBBhalfspaceD2test::Bptr;

gobjContainer boxOBBhalfspaceD2test::boxes(true);

bool * boxOBBhalfspaceD2test::help = 0;


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

  gobj::global->draw();

  glerrordisplay();
  
  glutSwapBuffers();
}

void boxOBBhalfspaceD2test::keyboard01(unsigned char key, int x, int y)
{
  static double delta = 0.1;

  assert(Aptr!=0);
  assert(Bptr!=0);

  switch (key)
  {
    case 27:
      exit(0);
      break;

    case 'j': Bptr->center.x += delta; break; 
    case 'J': Bptr->center.x -= delta; break; 
    case 'k': Bptr->center.y += delta; break; 
    case 'K': Bptr->center.y -= delta; break; 
    case 'l': 
    {
      transrotate2D tr(delta); 
      tr.eval(Bptr->arm1);
      tr.eval(Bptr->arm2);
      break;
    }
    case 'L': 
    {
      transrotate2D tr(-delta); 
      tr.eval(Bptr->arm1);
      tr.eval(Bptr->arm2);
      break;
    }

    case 'a': Aptr->center.x += delta; break; 
    case 'A': Aptr->center.x -= delta; break; 
    case 's': Aptr->center.y += delta; break; 
    case 'S': Aptr->center.y -= delta; break; 
    case 'd': 
    {
      transrotate2D tr(delta); 
      tr.eval(Aptr->arm1);
      tr.eval(Aptr->arm2);
      break;
    }
    case 'D': 
    {
      transrotate2D tr(-delta); 
      tr.eval(Aptr->arm1);
      tr.eval(Aptr->arm2);
      break;
    }

    case 'p': 
      cout << (stringc)(*Aptr) << endl;
      cout << (stringc)(*Bptr) << endl;
      break;

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

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

  update01();
  glutPostRedisplay();
}

void boxOBBhalfspaceD2test::update01()
{
  assert(Aptr!=0);
  assert(Bptr!=0);

  bool intersection = (Aptr->intersects(*Bptr));

  vector< point2<double> > pts(4);
  Aptr->cornerpoints(pts[0],pts[1],pts[2],pts[3]);

  boxes.nuke();

  boxes.push(new gobjglColor3ub(184,134,11));

  vector<point2<double> > v(&Aptr->qi[0],&Aptr->qi[0]+4);
  pointsdisplay2D<point2<double> > qidp(boxes,v,true,true,false);
  qidp.gq->radius=0.01;

  boxes.push(new gobjglBegin(GL_LINES));
  boxes.push(new gobjglVertex2f(pts[0]));
  boxes.push(new gobjglVertex2f(pts[1]));
  boxes.push(new gobjglVertex2f(pts[1]));
  boxes.push(new gobjglVertex2f(pts[2]));
  boxes.push(new gobjglVertex2f(pts[2]));
  boxes.push(new gobjglVertex2f(pts[3]));
  boxes.push(new gobjglVertex2f(pts[3]));
  boxes.push(new gobjglVertex2f(pts[0]));

  Bptr->cornerpoints(pts[0],pts[1],pts[2],pts[3]);

  boxes.push(new gobjglColor3ub(0,191,255));



  boxes.push(new gobjglVertex2f(pts[0]));
  boxes.push(new gobjglVertex2f(pts[1]));
  boxes.push(new gobjglVertex2f(pts[1]));
  boxes.push(new gobjglVertex2f(pts[2]));
  boxes.push(new gobjglVertex2f(pts[2]));
  boxes.push(new gobjglVertex2f(pts[3]));
  boxes.push(new gobjglVertex2f(pts[3]));
  boxes.push(new gobjglVertex2f(pts[0]));

  boxes.push(new gobjglColor3ub(255,0,255));
  boxes.push(new gobjglVertex2f(Aptr->center));
  boxes.push(new gobjglVertex2f(Bptr->center));

  boxes.push(new gobjglEnd());

  boxes.push(new gobjglColor3ub(0,191,255));
  pointsdisplay2D<point2<double> > pidp(boxes,pts,true,true,false);
  pidp.gq->radius=0.01;

  if (intersection)
  {
    double transparency=0.4;
    Aptr->cornerpoints(pts[0],pts[1],pts[2],pts[3]);
    //boxes.push(new gobjglColor3ub(255,0,0));

    boxes.push(new gobjglEnable(GL_BLEND));
    boxes.push(
      new gobjglBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) );
    boxes.push(
      new gobjglColor4f(1.0,0.0,0.0,transparency));

    boxes.push(new gobjglBegin(GL_TRIANGLES));
    boxes.push(new gobjglVertex2f(pts[0]));
    boxes.push(new gobjglVertex2f(pts[1]));
    boxes.push(new gobjglVertex2f(pts[2]));
    boxes.push(new gobjglVertex2f(pts[2]));
    boxes.push(new gobjglVertex2f(pts[3]));
    boxes.push(new gobjglVertex2f(pts[0]));

    Bptr->cornerpoints(pts[0],pts[1],pts[2],pts[3]);
    boxes.push(new gobjglVertex2f(pts[0]));
    boxes.push(new gobjglVertex2f(pts[1]));
    boxes.push(new gobjglVertex2f(pts[2]));
    boxes.push(new gobjglVertex2f(pts[2]));
    boxes.push(new gobjglVertex2f(pts[3]));
    boxes.push(new gobjglVertex2f(pts[0]));
    boxes.push(new gobjglEnd());

    boxes.push(new gobjglDisable(GL_BLEND));
  }
}

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

  OpenGLinitialisation();

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

  zpr zz;

  xGraphics.set();

  commandline cmd(argc,argv);

  string in;

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

  ifstream filein(in.c_str());
  assert(filein.good()==true);
  if (filein.good()==false)
    return;

  typedef point2<double> pt2;

  pt2 center;
  pt2 arm1;
  pt2 arm2;
  double arm1len;
  double arm2len;

  filein >> center;
  filein >> arm1;
  filein >> arm2;
  filein >> arm1len;
  filein >> arm2len;
  boxOBBhalfspaceD2<point2<double>,double> 
    A(center,arm1,arm2,arm1len,arm2len);

  filein >> center;
  filein >> arm1;
  filein >> arm2;
  filein >> arm1len;
  filein >> arm2len;
  boxOBBhalfspaceD2<point2<double>,double> 
    B(center,arm1,arm2,arm1len,arm2len);


  Aptr = & A;
  Bptr = & B;
  gobjpush(&boxes);

  update01();

  menusystem * menu = 
    new menusystem(0,0,true,point2<GLint>(20,20),10);

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

  menu->addfont12("OBB intersection test");
  menu->addnewline();
  menu->addnewline();
  menu->addfont10("j J   Move box B left or right.");
  menu->addnewline();
  menu->addfont10("k K   Move box B up or down.");
  menu->addnewline();
  menu->addfont10("l L   Rotate box B.");
  menu->addnewline();

  menu->addfont10("a A   Move box A left or right.");
  menu->addnewline();
  menu->addfont10("s S   Move box A up or down.");
  menu->addnewline();
  menu->addfont10("d D   Rotate box A.");
  menu->addnewline();

  menu->addnewline();
  menu->addfont10("p     Print boxes to terminal.");
  menu->addnewline();
  menu->addfont10("+ -   Increase or decrease change.");
  menu->addnewline();
  menu->addfont10("h    Toggle this help menu.");
  menu->addnewline();
  menu->addfont10("ESC   Quit");


  zz.update();
  glutMainLoop();
}


int boxOBBhalfspaceD2test::unittest01()
{
  typedef point2<double> pt2;
  pt2 pi[4] = 
  { 
    pt2(6.0,0.0), pt2(7.0,0.0), 
    pt2(7.0,3.0), pt2(6.0,3.0)
  };

  boxOBBhalfspaceD2<pt2,double> bx;
  bx.construct(pi[0],pi[1],pi[2],pi[3]);

  assertreturnOS( 
    zero<double>::test( (bx.center-pt2(6.5,1.5)).dot() ) );

  pt2 zi[4];
  bx.cornerpoints(zi[0],zi[1],zi[2],zi[3]);

  for (uint i=0; i<4; ++i)
    assertreturnOS( zero<double>::test( (pi[i]-zi[i]).dot() ) );

  return 0;
}

int boxOBBhalfspaceD2test::unittest02(int & argc, char** argv)
{
  typedef point2<double> pt2;

  commandline cmd(argc,argv);

  string p[5];
  string pstr[5]={"p0","p1","p2","p3","q"};

  pt2 pi[5];

  bool inside(true);
  cmd.mapvar(inside,"inside");

  for (uint i=0; i<5; ++i)
  {
    cmd.mapvar(p[i],pstr[i]);
    if ( p[i].empty() )
      return 1;

    pi[i].serializeInverseBrackets(p[i]);

    cout << "pi[" << i << "]=" << pi[i] << endl;
  }

  boxOBBhalfspaceD2<pt2,double> bx;
  bx.construct(pi[0],pi[1],pi[2],pi[3]);
  
  if (inside)
  {
    if (bx.intersects(pi[4])==false)
      return 1;

    return 0;
  }

  if (bx.intersects(pi[4])==true)
    return 1;

  return 0;
}