/*
Com S 476/576 Course Software 
Release 0.1  02/22/00

Copyright 1999,2000

Steven M. LaValle, Iowa State University
*/

#include <fstream.h>
#include <math.h>


#include <LEDA/file.h>
#include <LEDA/array.h>
#include <LEDA/list.h>
#include <LEDA/matrix.h>
#include <LEDA/point.h>
#include <LEDA/polygon.h>
#include <LEDA/stream.h>
#include <LEDA/string.h>
#include <LEDA/vector.h>
#include <LEDA/graph.h>

#include "world.hh"

#ifndef PI
#define PI 3.1415926535897932385
#endif
#ifndef INFINITY
#define INFINITY 1.0e40
#endif
#ifndef sqr
#define sqr(x) ((x)*(x))
#endif
#ifndef min
#define min(x,y) ((x<y) ? x : y)
#endif

// Some models will interpolate differently because of 
// topology (e.g., S^1, P^3)
// This is the Euclidean default
leda_vector World::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
				const double &a) {
  return a*x1 + (1.0-a)*x2;
}


// *********************************************************************
// *********************************************************************
// CLASS:     World::WorldPolygonal
// 
// *********************************************************************
// *********************************************************************

// Constructor
WorldPolygonal::WorldPolygonal(leda_string path = ""):World(path) {

  if ((path.length() > 0)&&(path[path.length()-1] != '/'))
    path += "/";

  FilePath = path;
  
  if (is_file(FilePath + "Obst")) {
    file_istream fin(FilePath + "Obst");
    fin >> Obst;
  }
}



// This simple collision checker uses LEDA's p.inside method.
// Certainly more efficient methods exist (incremental).
bool WorldPolygonal::CollisionFree(const leda_vector &x) {
  leda_polygon p;

  forall(p,Obst) {
    if (p.inside(leda_point(x))) {
      return false;
    }
  }

  return true;
}


double WorldPolygonal::DistanceComp(const leda_vector &x) {
  leda_polygon p;
  double d_min,d;

  d_min = INFINITY;

  if (CollisionFree(x)) {
    forall(p,Obst) {
      d = p.distance(leda_point(x));
      if (d < d_min)
	d_min = d;
    }
  }
  else
    d_min = -1.0;
  
  return d_min;
}



leda_list<leda_polygon> WorldPolygonal::EnvironmentToPolygons() {
  return Obst;
}


leda_list<leda_polygon> WorldPolygonal::RobotToPolygons(const leda_vector &x) {
  leda_list<leda_polygon> trobot;

  leda_point p1(0.5,0.5);
  leda_point p2(-0.5,0.5);
  leda_point p3(-0.5,-0.5);
  leda_point p4(0.5,-0.5);
  leda_list<leda_point> pl;
  pl.push(p4); pl.push(p3); pl.push(p2); pl.push(p1);
  leda_polygon p(pl);
  trobot.push(p.translate(x));
  
  return trobot;
}



// *********************************************************************
// *********************************************************************
// CLASS:     World::WorldPolygonalRigid
// 
// *********************************************************************
// *********************************************************************

// Constructor
WorldPolygonalRigid::WorldPolygonalRigid(leda_string path = ""):WorldPolygonal(path) {

  if ((path.length() > 0)&&(path[path.length()-1] != '/'))
    path += "/";

  FilePath = path;
  
  if (is_file(FilePath + "Robot")) {
    file_istream fin(FilePath + "Robot");
    fin >> Robot;
  }
}


// This is a very naive collision checker
// This should certainly be replaced some day with a 
// a more efficient algorithm.  It would be best to add
// a 2D incremental distance algorithm, based on the
// ideas of Lin and Canny, and Mirtich.  One could also
// use bitmaps since this is only 2D.

bool WorldPolygonalRigid::CollisionFree(const leda_vector &x) {
  leda_polygon op,rp;
  leda_segment seg,seg2;

  leda_list<leda_polygon> trobot = RobotToPolygons(x);

  forall(rp,trobot) {
    forall(op,Obst) {
      forall_segments(seg,rp) {
	forall_segments(seg2,op) {
	  if (seg.intersection(seg2)) {
	    return false;
	  }
	}
      }
    }
  }

  // Check for containment
  forall(rp,trobot) {
    forall(op,Obst) {
      if (op.contains(rp.vertices().head()))
	return false;
    }
  }

  return true;
}


// This should certainly be replaced some day with a 
// a more efficient algorithm.  It would be best to add
// a 2D incremental distance algorithm, based on the
// ideas of Lin and Canny, and Mirtich.

double WorldPolygonalRigid::DistanceComp(const leda_vector &x) {
  leda_polygon rp,op;
  double d_min,d;
  leda_point rpt;

  d_min = INFINITY;

  leda_list<leda_polygon> trobot = RobotToPolygons(x);

  // This is obviously inefficient
  if (CollisionFree(x)) {
    forall(rp,trobot) {
      forall_vertices(rpt,rp) {
	forall(op,Obst) {
	  d = op.distance(rpt);
	  if (d < d_min)
	    d_min = d;
	}
      }
    }
  }
  else
    d_min = -1.0;
  
  return d_min;
}


// Handle S^1 topology properly (for rotation)
leda_vector WorldPolygonalRigid::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
					 const double &a) {

  leda_vector v;

  v = a*x1 + (1.0-a)*x2;

  if (x2[2] - x1[2] > PI) {
    if (x1[2] > x2[2])
      v[2] = a*x1[2] + (1.0-a)*(x2[2]+PI);
    else
      v[2] = a*(x1[2]+PI) + (1.0-a)*x2[2];
  }

  if (v[2] > 2.0*PI)
    v[2] -= 2.0*PI;

  return v;
}



leda_list<leda_polygon> WorldPolygonalRigid::RobotToPolygons(const leda_vector &q) {
  leda_list<leda_polygon> trobot;
  leda_polygon p,tp;

  forall(p,Robot) {
    tp = p.rotate(q[2]).translate(q[0],q[1]);
    trobot.push_front(tp);
  }

  return trobot;
}


////////////////////////////
// PQP world! Mikael Beckius
////////////////////////////


WorldPQP::WorldPQP(leda_string path, std::vector<PQPObject*> &obst, std::vector<PQPObject*> &rob):World(path){
  obstacles = obst;
  robots = rob;
}

// WorldPQP::~WorldPQP() {}


bool WorldPQP::CollisionFree(const leda_vector &q){
  PQP_REAL R1[3][3],R2[3][3],T1[3],T2[3];
  PQP_CollideResult cres;

  //cout<<"collstart"<<endl;
  float x,y,z,rotY;
  for(register int j = 0; j < robots.size(); j++){
    //leda_vector qx(4);

    rotY = robots[j]->getRotY();
    robots[j]->getTrans(x,y,z);
    robots[j]->move(q[0]-x,q[1]-y,q[2]-z);
    if(q.dim() == 4)
      //robots[j]->setPos(q);
      robots[j]->setRotY(q[3]);
    OGLtoMV(R2, T2, robots[j]->mm);
    //for(register int s = 0; s < 16; s ++)
    //cout<<robots[j]->mm[s]<<endl;
    //cout<<"worldPQP"<<endl;
    robots[j]->move(-q[0]+x,-q[1]+y,-q[2]+z);
    if(q.dim() == 4)
      //robots[j]->setPos(qx);
      robots[j]->setRotY(rotY);
    //float m[16];
    //rotate(q[3],m,1.0);
    //OGLtoMV(R2, T2, m);
    //T2[0] = q[0];
    //T2[1] = q[1]; //4.0;
    //T2[2] = q[2];
    //MRotY(R2, q[2]);
    for(register int i = 0; i < obstacles.size(); i++){
      OGLtoMV(R1, T1, obstacles[i]->mm);
      PQP_Collide(&cres,R1,T1,obstacles[i]->pqp,R2,T2,robots[j]->pqp, PQP_FIRST_CONTACT);
      
      if (cres.NumPairs() > 0){
	//cout<<"collision"<<endl;
	return false;
      }
    }
  }
  //cout<<"collision free"<<endl;
  return true;
}


double WorldPQP::DistanceComp(const leda_vector &q){
  double d_min = INFINITY;
  PQP_REAL R1[3][3],R2[3][3],T1[3],T2[3];
  PQP_DistanceResult res;

  cout<<"diststart"<<endl;

  if(CollisionFree(q))
    for(register int j = 0; j < robots.size(); j++)
      for(register int i = 0; i < obstacles.size(); i++){
	OGLtoMV(R2, T2, robots[j]->mm);
	T2[0] = q[0];
	T2[1] = q[1];
	T2[2] = q[2];
	//MRotY(R2, robots[j]->getRotY()+q[2]);
	OGLtoMV(R1, T1, obstacles[i]->mm);
	PQP_REAL rel_err = 0.0;
	PQP_REAL abs_err = 0.0;
	PQP_Distance(&res,R1,T1,obstacles[i]->pqp,R2,T2,robots[j]->pqp,rel_err,abs_err);
	if (res.Distance() < d_min)
	  d_min = res.Distance();
      }
      
  else
    d_min = -1.0;
  
  cout<<"distance: "<<d_min<<endl;
 
  return d_min;
}


leda_list<leda_polygon> WorldPQP::EnvironmentToPolygons(){
  leda_list<leda_polygon> polygons;
  return polygons;
}


leda_list<leda_polygon> WorldPQP::RobotToPolygons(const leda_vector &q){
  leda_list<leda_polygon> polygons;
  return polygons;
}


leda_vector WorldPQP::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
					const double &a) {
  leda_vector v;

  v = a*x1 + (1.0-a)*x2;
  
  if (x2[3] - x1[3] > PI) {
    if (x1[3] > x2[3])
      v[3] = a*x1[3] + (1.0-a)*(x2[3]+PI);
    else
      v[3] = a*(x1[3]+PI) + (1.0-a)*x2[3];
  }
  
  if (v[3] > 2.0*PI)
    v[3] -= 2.0*PI;
  
  return v;
}