// motion.cc by Mikael Beckius 2000-04-17 - .
// 
#include "motion.hh"
using namespace std;

int delay = 2; // shouldn't be here, but gives seg fault if declred in the Motion class.

Motion::Motion(){
  leda_string path = "";
  //leda_vector InitialState; 
  //leda_vector GoalState;
  prob = new Problem(new WorldPolygonal(path),new Model2DPoint(path), path);
  //rrt = new RRTExtExt(new Problem(new WorldPolygonal(path),new Model2DPoint(path), path));
  rrt  = new RRTExtExt(prob);
  cout<<"Motion"<<endl;
  rrt->Explore();
  rrt->Plan(0.4,pathplan);
  cout<<"Motion done!"<<endl;
}


Motion::Motion(vector<PQPObject*> &obst, vector<PQPObject*> &rob):gdist(0.5){
  leda_string path = "";
  float x = 0, y = 0, z = 0;
  robots = &rob;
  obstacles = &obst;
  PQPObject *obj = (*robots)[0];
  Robot* robo = static_cast<Robot*>(obj);


  robo->getTrans(x,y,z);
  //leda_vector q(x,y,z,robo->getRotY());
  //leda_vector q2(70.0,4,-70,robo->getRotY());
  
  model =  new Model3Dy(path);
  world = new WorldPQP(path,obst,rob);
  prob = new Problem(world,model, path);
  
  //prob->InitialState = q;
  //prob->GoalState  = q2;
  
  //cout<<"Motion"<<endl;
  rrt  = new RRTExtExt(prob);
  //rrt = new RRTExtExt(new Problem(new WorldPQP(path,obst,rob),new Model2DPoint(path), path));
  
  //rrt->Explore();
  cout<<"Avstand: "<<rrt->GoalDist<<endl;
  //rrt->Plan(gdist,pathplan);
  //reversePath = pathplan;
  //cout<<"Path length: "<<pathplan.length()<<endl;
}

void Motion::setGoal(float *m){
  PQPObject *obj = (*robots)[0];
  Robot* robo = static_cast<Robot*>(obj);
  //cout<<"ROT: "<<robo->getRotY()<<endl;
  
  leda_vector q(4);
  q[0]=m[12]; q[1] = m[13]; q[2] = m[14]; q[3] = robo->getRotY();
  prob->GoalState  = q;
  //cout<<q2<<endl;
}

void Motion::setInit(){
  float x = 0, y = 0, z = 0;
  PQPObject *obj = (*robots)[0];
  Robot* robo = static_cast<Robot*>(obj);
  robo->getTrans(x,y,z);
  leda_vector q(4);
  q[0]=x; q[1] = y; q[2] = z; q[3] = robo->getRotY();
  //leda_vector q2(70.0,-70,robo->getRotY());
  prob->InitialState = q;
  //cout<<q<<endl;
}


void Motion::start(){
  //rrt->Explore();
  rrt->G.clear();
  rrt->GG.clear();
  setInit();
  pathplan.clear();
  rrt->Plan(gdist,pathplan);
  reversePath = pathplan;
  cout<<"Path length: "<<pathplan.length()<<endl;
  cout<<"revpath length: "<<reversePath.length()<<endl;
}

void Motion::makePath(){
  //rrt->Explore();
  rrt->Plan(gdist,pathplan);
}


void Motion::draw(){
  drawPath();
  drawRRT();
}

void Motion::drawRobot(){
  leda_node n;
  leda_vector q;
  //list_item L.first();
  
  PQPObject *obj = (*robots)[0];
  Robot* robo = static_cast<Robot*>(obj);
  //cout<<"Delay: "<<delay<<endl;
  
  // Show path.
  if (reversePath.length() > 0){
    if (delay == 0){
      n = reversePath.pop();
      delay = 3;
      q = rrt->P->StateToConfiguration(rrt->G.inf(n));
      robo->setPos(q);
    }
  
    if(delay > 0)
      delay--;
  }
}


void Motion::drawPath(){
  leda_node n;
  leda_vector q,q2;

  // Show goal
  q = rrt->P->StateToConfiguration(rrt->P->GoalState);
  glPushMatrix();
  glColor3f(1.0,1.0,0.0);
  glTranslatef(q[0],q[1],q[2]);
  glutSolidSphere(0.35,15,15);
  glPopMatrix();
  
  if (pathplan.length() > 0){
    // Show init.
    q2 = rrt->P->StateToConfiguration(rrt->P->InitialState);
    glPushMatrix();
    glColor3f(0.0,1.0,1.0);
    glTranslatef(q2[0],q2[1],q2[2]);
    glutSolidSphere(0.35,15,15);
    glPopMatrix();
    
    // Show path.
    forall(n,pathplan) {
      q = rrt->P->StateToConfiguration(rrt->G.inf(n));
      glPushMatrix();
      glColor3f(1.0,0.0,0.0);
      glTranslatef(q[0],q[1],q[2]);
      glutSolidSphere(.15,5,5);
      glPopMatrix();
    }
    glPushMatrix();
    glDisable(GL_LIGHTING);
    glBegin(GL_LINES);
    forall(n,pathplan) {
      q = rrt->P->StateToConfiguration(rrt->G.inf(n));
      glVertex3f(q2[0],q2[1],q2[2]);
      glVertex3f(q[0],q[1],q[2]);
      q2 = q;
    }
    glEnd();
    glEnable(GL_LIGHTING);
    glPopMatrix();
  }
}
  



void Motion::drawRRT(){
  leda_node n;
  leda_vector q;

  // Show goal
  q = rrt->P->StateToConfiguration(rrt->P->GoalState);
  glPushMatrix();
  glColor3f(1.0,1.0,0.0);
  glTranslatef(q[0],q[1],q[2]);
  //glutSolidSphere(0.75,15,15);
  glPopMatrix();
  
  // Show init
  q = rrt->P->StateToConfiguration(rrt->P->InitialState);
  glPushMatrix();
  glColor3f(0.0,1.0,1.0);
  glTranslatef(0,0,0);
  glTranslatef(q[0],q[1],q[2]);
  //glutSolidSphere(1.15,15,15);
  glPopMatrix();

  //cout<<"Start: "<<q<<endl;
  // Show tree
  if (rrt->G.number_of_nodes() > 0) {
    forall_nodes(n,rrt->G) {
      q = rrt->P->StateToConfiguration(rrt->G.inf(n));
      //cout<<"random: "<<q<<endl;
      glPushMatrix();
      glColor3f(0.0,0.0,1.0);
      glTranslatef(q[0],q[1],q[2]);
      glutSolidSphere(.15,5,5);
      //w->draw_point(q[0],q[1],brown);
      glPopMatrix();
    }
  }

  // Show second tree (if it exists)
  if (rrt->GG.number_of_nodes() > 0) {
    forall_nodes(n,rrt->GG) {
      q = rrt->P->StateToConfiguration(rrt->G.inf(n));
      glPushMatrix();
      glColor3f(0.0,1.0,0.0);
      glTranslatef(q[0],q[1],q[2]);
      glutSolidSphere(.15,15,15);
      glPopMatrix();
      //w->draw_point(q[0],q[1],brown);
    }
  }  
}



Motion::~Motion(){
  delete rrt;
}