/* 
   Project: coevolutionary GP moto driving
   Dana Vrajitoru
   road_manager.cc
   
   The class that handles the road and some basic functionality
   related to it.
  
**/

#include "road_manager.h"
#include <fstream>
#include <cmath>
#include "general.h"

// default constructor
Road_manager::Road_manager() 
  : position(), coords()
{
  road_id = glGenLists(1);
  centerline_id = glGenLists(1);
  centerline = false; 
  dotted = false;
  angle = 0.0;
  scale = 1.0;
  nr_trigs = 0;
  nr_points = 0;
  epsilon = 0.0001;
}

// Constructor with a file name - read the road from a file.
Road_manager::Road_manager(const char *filename)
  : position(), coords()
{
  road_id = glGenLists(1);
  centerline_id = glGenLists(1);
  centerline = false; 
  dotted = false;
  angle = 0.0;
  scale = 1.0;
  nr_trigs = 0;
  epsilon = 0.0001;
  read(filename);
}

// destructor
Road_manager::~Road_manager()
{
  glDeleteLists(road_id, 1);
  glDeleteLists(centerline_id, 1);
  if (d_pars.size())
    d_pars.erase(d_pars.begin(), d_pars.end());
  if (normals.size())
    normals.erase(normals.begin(), normals.end());
  if (neighbors.size())
    neighbors.erase(neighbors.begin(), neighbors.end());
  if (edges.size())
    edges.erase(edges.begin(), edges.end());
  if (is_border.size())
    is_border.erase(is_border.begin(), is_border.end());
}

// Set and get these values;
void Road_manager::set_scale(float val)
{
  scale = val;
}

void Road_manager::set_angle(float val)
{
  angle = val;
}

void Road_manager::set_position(const Point3f &data)
{
  position.set_data(data);
}

void Road_manager::set_position(float x, float y, float z)
{
  position.set_data(x, y, z);
}

GLfloat Road_manager::get_scale()
{
  return scale;
}

GLfloat Road_manager::get_angle()
{
  return angle;
}

Point3f &Road_manager::get_position()
{
  return position;
}

// Tells us if the road has a visible centerline.
bool Road_manager::has_centerline()
{
  return centerline;
}

// Tells us if the centerline is dotted.
bool Road_manager::is_dotted()
{
  return dotted;
}

// Perform some operations on the data itself to accelerate the
// computations.
void Road_manager::zoom(float val)
{
  for (int i=0; i<nr_trigs; i++)
    for (int j=0; j<3; j++)
      coords[i][j] *= val;
}

void Road_manager::translate(float x, float y, float z)
{
  Point3f vec(x, y, z);
  for (int i=0; i<nr_trigs; i++)
    for (int j=0; j<3; j++)
      coords[i][j] += vec;
}

void Road_manager::rotate_x(float angle)
{
  float rangle=d2r(angle);
  for (int i=0; i<nr_trigs; i++)
    for (int j=0; j<3; j++)
      coords[i][j].rotate_x(rangle);
}

void Road_manager::rotate_y(float angle)
{
  float rangle=d2r(angle);
  for (int i=0; i<nr_trigs; i++)
    for (int j=0; j<3; j++)
      coords[i][j].rotate_y(rangle);
}

void Road_manager::rotate_z(float angle)
{
  float rangle=d2r(angle);
  for (int i=0; i<nr_trigs; i++)
    for (int j=0; j<3; j++)
      coords[i][j].rotate_z(rangle);
}


// Function to be called to generate the display lists.
void Road_manager::draw()
{
  float red = 0, rscale = max[1] - min[1];
  int i;
  if (normals.size() != coords.size())
    compute_normals();
  glNewList(road_id, GL_COMPILE);
  //glBegin(GL_TRIANGLES);
  glBegin(GL_TRIANGLES);
  for (i=0; i<nr_trigs; i++) {
    normals[i].gl_normal();
    coords[i][0].gl_vertex();
    coords[i][1].gl_vertex();
    coords[i][2].gl_vertex();
  }
  glEnd();
  glTranslatef(0, 0.01, 0);
  glLineWidth(2);
  for (i=0; i<nr_trigs; i++) {
    glColor3f(0.2, 0.2, 0.2);
    //red = abs(coords[i][0][1]/rscale);
    //glColor3f(red, 1, 0);
    glBegin(GL_LINE_LOOP);
    normals[i].gl_normal();
    coords[i][0].gl_vertex();
    coords[i][1].gl_vertex();
    coords[i][2].gl_vertex();
    glEnd();
  }
  //glEnd();
  glEndList();
  if (centerline) {
    glNewList(centerline_id, GL_COMPILE);
    glLineWidth(4);
    glBegin(GL_LINE_LOOP);
    for (i=0; i<nr_points; i++) 
      center_coords[i].gl_vertex();
    glEnd();
    glEndList();
  }
}

// Function to be called to generate the display lists.
void Road_manager::draw_sml()
{
  float red = 0, rscale = max[1] - min[1];
  int i;
  if (normals.size() != coords.size())
    compute_normals();
  glNewList(road_id, GL_COMPILE);
  //glBegin(GL_TRIANGLES);
  glBegin(GL_TRIANGLES);
  for (i=0; i<nr_trigs; i++) {
    normals[i].gl_normal();
    coords[i][0].gl_vertex();
    coords[i][1].gl_vertex();
    coords[i][2].gl_vertex();
  }
  glEnd();
  glEndList();
}

// Function to be called to display the lists.
void Road_manager::display()
{
  glPushMatrix();
  glRotatef(angle, 0, 1, 0);
  //  glRotatef(-90, 1, 0, 0);
  position.gl_translate();
  glScalef(scale, scale, scale);
  //glColor3f(0.0, 1.0, 0.0);
  glColor3f(0.25, 0.25, 0.25);
  glCallList(road_id);
  if (centerline) {
    glColor3f(1.0, 1.0, 0.0);    
    glCallList(centerline_id);
  }
  glPopMatrix();
}

// Read the triangles from a file.
bool Road_manager::read(const char *filename)
{
  if (coords.size())
    coords.erase(coords.begin(), coords.end());
  nr_trigs = 0;

  ifstream fin;
  int  j;
  char  txt[150], junk;
  vector3f triangle(3);

  fin.open(filename);
  if (!fin) {
    cout << "Could not read the road from file:" << endl
	 << filename << endl;
    return false;
  }

  do {
    for (j=0; j<3; j++)
      fin >> triangle[j][0] >> triangle[j][1] >> triangle[j][2];
    while(fin.good()) {
      coords.push_back(triangle);
      nr_trigs++;
      for (j=0; j<3; j++)
	fin >> triangle[j][0] >> triangle[j][1] >> triangle[j][2];
    }
    fin.clear(); 
    do 
      fin.get(junk);
    while (fin.good() && junk != '\n');
  } while(fin.good());
  fin.close();
  return true;
}

// Read the centerline from a file.
bool Road_manager::read_center(const char *filename)
{
  if (center_coords.size())
    center_coords.erase(center_coords.begin(), center_coords.end());
  nr_points = 0;

  ifstream fin;
  Point3f point;
  
  fin.open(filename);
  if (!fin) {
    cout << "Could not read the road from file:" << endl
	 << filename << endl;
    return false;
  }

  do {
    fin >> point[0] >> point[1] >> point[2];
    if (fin.good()) {
      center_coords.push_back(point);
      nr_points++;
    }
  } while(fin.good());
  fin.close();
  if (nr_points > 0)
    centerline = true; 
  return true;
}

// Compute all the normals to the triangles - these will be used over
// and over, want to speed up the computations.
void Road_manager::compute_normals()
{
  if (normals.size() != 0)
    normals.erase(normals.begin(), normals.end());
  Point3f normal;
  for (int i=0; i<nr_trigs; i++) {
    normal.compute_normal_vector(coords[i][0], coords[i][1], 
				 coords[i][2]);  
    if (normal[1] < 0)
      normal *= -1;
    normals.push_back(normal);
  }
}

// Compute the d parameter in the equation of the plane for each
// triangle. This is used to compute the projection of an object onto
// the road and can speed up computations too.
void Road_manager::compute_d_pars()
{
  float d;
  Point3f normal;
  if (d_pars.size() != 0)
    d_pars.erase(d_pars.begin(), d_pars.end());
  if (normals.size() != coords.size())
    compute_normals();
  for (int i=0; i<nr_trigs; i++) {
    normal = normals[i];
    // This will give us the last term in the equation ax+by+cz+d=0 for
    // the triangle plane, where a, b, c is the normal to the plane.
    d = -normal.scalarprod(coords[i][0]);
    d_pars.push_back(d);
  }
}

// Compute the bounding box, the min and max pointers.
void Road_manager::compute_bbox()
{
  min = coords[0][0];
  max = coords[0][0];
  for (int i=0; i<nr_trigs; i++) 
    for (int j=0; j<3; j++)
      for (int k=0; k<3; k++) {
	if (coords[i][j][k] < min[k])
	  min[k] = coords[i][j][k];
	if (coords[i][j][k] > max[k])
	  max[k] = coords[i][j][k];
      }
  if (position) {
    min += position;
    max = position;
  }
}

// For each triangle, find the neighbors
void Road_manager::compute_neighbors()
{
  vectori blanc;
  int i, j, k, l, n1, n2, m1, m2;
  for (i=0; i<nr_trigs; i++) {
    neighbors.push_back(blanc);
    edges.push_back(blanc);
    is_border.push_back(-1);
  }
  for (i=0; i<nr_trigs-1; i++) 
    for (j=i+1; j<nr_trigs; j++) {
      n1 = n2 = m1 = m2 = -1;
      for (k=0; k<3; k++)
	for (l=0; l<3; l++) 
	  if (coords[i][k] == coords[j][l]) {
	    if (n1 == -1) {
	      n1 = k;
	      m1 = l;
	    }
	    else if (n2 == -1) {
	      n2 = k;
	      m2 = l;
	    }
	    else
	      cout << "triangles overlap: " << i << ' ' << j << endl;
	  }
      if (n1 != -1) {
	neighbors[i].push_back(j);
	neighbors[j].push_back(i);
	if (n2 != -1) {
	  if (n2-n1 == 1)
	    edges[i].push_back(n1);
	  else
	    edges[i].push_back(n2);
	  if (m2 > m1)
	    if (m2-m1 == 1)
	      edges[j].push_back(m1);
	    else
	      edges[j].push_back(m2);
	  else
	    if (m1-m2 == 1)
	      edges[j].push_back(m2);
	    else
	      edges[j].push_back(m1);
	}
	else {
	  edges[i].push_back(n1+3);
	  edges[j].push_back(m1+3);
	}
      }
    }
  for (i=0; i<nr_trigs; i++) {
    bool neighb[3] = {false, false, false};
    for (k=0; k<edges[i].size(); k++) {
      if (edges[i][k] < 3)
	neighb[edges[i][k]] = true;
    }
    for (l=0; l<3; l++)
      if (!neighb[l]) 
	is_border[i] = l;
  }
}

// Displays the coordinates
void Road_manager::print_coordinates()
{
  int i, k;
  for (i=0; i<nr_trigs; i++) {
    cout << i << ": " << endl;
    for (k=0; k<3; k++)
      cout << coords[i][k] << endl;
  }
}

// Displays the neighbors structure
void Road_manager::print_neighbors()
{
  int i, k;
  for (i=0; i<nr_trigs; i++) {
    cout << i << ": ";
    for (k=0; k<edges[i].size(); k++)
      cout << neighbors[i][k] << "-" << edges[i][k] << ' ';
    cout << "border: " << is_border[i] << endl;
  }
}

/* Not used for now but it's good to keep the equations.
// Compute the angles between the plane of each triangle and the xz
// plane for the same reason as the normals.
void Road_manager::compute_angles()
{
  Point3f normal;
  float angle;
  if (angles.size() != 0)
    angles.erase(angles.begin(), angles.end());
  if (normals.size() != coords.size())
    compute_normals();
  for (int i=0; i<nr_trigs; i++) {
    normal = normals[i];
    if (normal[0] ==0 && normal[2] == 0)
      angle = 0;
    else {
      angle = r2d(asin(normal[2]/
		       sqrt(normal[0]*normal[0]+normal[2]*normal[2]))) + 90;
      if (normal[0] < 0)
	angle = 180.0 - angle;
      if (angle < 0)
	angle += 360;
      cout << normal << ' ' << angle << endl;
    }
    angles.push_back(angle);
  }
}
*/


//returns the center coordinate size 
int Road_manager::center_size()
{
  return (center_coords.size() - 1);
}

//to find the distance between two points
float Road_manager::distance_center(const Point3f &p, int prev)
{
 
  return sqrt((p[0]-center_coords[prev][0])*(p[0]-center_coords[prev][0]) +           
	      (p[1]-center_coords[prev][1])*(p[1]-center_coords[prev][1]) +
              (p[2]-center_coords[prev][2])*(p[2]-center_coords[prev][2]));
}

//returns the position on the center line which is
//near to the current position of moto

int Road_manager::locate_center(const Point3f &position,int prev)
{
  
  //calculates the total number of points on the line
  int coord_length = center_coords.size();

  //initialize array with the length equal to 
  // total number of points in the line.
  float distance[coord_length];

  //declare floats to calculate distances
  float distance1= 0.0;
  float distance2 =0.0;
  float distance3 =0.0;
 

  // initialize minimum as float varaible
  float  minimum = 0;
  // initialize return_val as int
  int return_val = 0;
  
  // if the bike is in the initial position
  if(prev == -1) {   
    for(int i = 0 ; i < coord_length; i++)
      distance[i] = distance_center(position,i);
    
    //find the min distance from position to center_coords
    minimum = distance[0];
    return_val = 0;

    for(int i = 0; i < 53; i++){
      if(minimum > distance[i]){
	minimum = distance[i];
        return_val = i;
      }
    }
    
  }

  else{
    
    distance1 = distance_center(position,prev); 
    if (prev == 0)
      distance2 = distance_center(position, coord_length-1);
    else
      distance2 = distance_center(position,prev-1);
    if (prev == coord_length-1)
      distance3 = distance_center(position,0);
    else
      distance3 = distance_center(position,prev+1);
 
    //find the minimum of prev,prev - 1,prev + 1 to the current 
    //position
    minimum = distance1;
    return_val = prev;
    if(distance2 < minimum){
      minimum = distance2;
      if (prev == 0)
	return_val = coord_length-1;
      else
	return_val = prev-1;
    }
    if (distance3 < minimum){
      minimum = distance3;
      if (prev == coord_length-1)
	return_val = 0;
      else
	return_val = prev+1;
    } 
  }   
 return return_val;
}


// Locate a position inside the mesh. If the point intersects more
// than one triangle, choose the one closest to it.
int Road_manager::locate(const Point3f &position)
{
  vector<int> intersect;
  int i, min, trig;
  float dist, distm;
  for (i=0; i<nr_trigs; i++) 
    if (position.inside_xz(coords[i][0], coords[i][1], 
			   coords[i][2]))
      intersect.push_back(i);
  if (intersect.size() == 0)
    return -1;
  if (intersect.size()==1)
    trig = intersect[0];
  else {
    min = 0;  
    distm = fabs((coords[intersect[0]][0][1]+
		 coords[intersect[0]][1][1]+
		 coords[intersect[0]][2][1])/3.0- position[1]);
    for (i=1; i<intersect.size(); i++) {
      dist = fabs((coords[intersect[i]][0][1]+
		   coords[intersect[i]][1][1]+
		   coords[intersect[i]][2][1])/3.0- position[1]);
      if (dist < distm) {
	min = i;
	distm = dist;
      }
    }
    trig = intersect[min];
  }
  return trig;
}


// Check if the position is inside a given triangle.
bool Road_manager::is_inside(const Point3f &position, int trig)
{
  return (position.inside_xz(coords[trig][0], coords[trig][1], 
			     coords[trig][2]));
}

// Locate a position inside the triangles that are direct neighbors of
// the old triangle. When the moto moves out of a given triangle, we
// might want to start looking for its current location in the
// neighborhood first.
int Road_manager::locate_neighborhood(const Point3f &position, 
				      int trig)
{
  int i, j;
  if (trig < 0 || trig >= neighbors.size())
    return -1;
  for (j=0; j<neighbors[trig].size(); j++) {
    i = neighbors[trig][j];
    if (position.inside_xz(coords[i][0], coords[i][1], 
			   coords[i][2]))
      return i;
  }
  return -1;
}

// Find the intersection between a vertical in the given position and
// the plane of the specified triangle.
float Road_manager::project_y_diff(const Point3f &position, int trig)
{
  if (trig < 0 || trig >= nr_trigs)
    return 0;
  Point3f normal, proj;
  float d;
  normal = normals[trig];
  d = d_pars[trig];
  if (normal[1] == 0)
    return 0;
  else
    return (-(normal.scalarprod(position)+d)/normal[1]);
}

// Some function for the interaction with the motorcycle or any other
// object on it.
float Road_manager::get_slope(const Point3f &position, float angle)
{
  int trig = locate(position);
  return get_slope(position, angle, trig);
}

// Some function for the interaction with the motorcycle or any other
// object on it. The triangle is given.
float Road_manager::get_slope(const Point3f &position, float angle, 
			      int trig)
{
  if (trig < 0 || trig >= nr_trigs)
    return 0;
  Point3f normal, proj;
  float rangle = -d2r(angle), abs_angle;
  normal = normals[trig];
  proj[0] = cos(rangle);
  proj[2] = sin(rangle);
  if (normal[1] == 0)
    return 90;
  else if (normal[0] ==0 && normal[2] == 0)
    return 0;
  else {
    proj[1] = -(proj[0] * normal[0] + proj[2] * normal[2]) / normal[1];
    abs_angle = r2d(asin(proj[1]/proj.norm()));
    return abs_angle;
  }
}

// Some methods for tracing rays inside triangles and get some
// information about the surrounding of the motorcycle on the road.

// Trace a ray inside a triangle. The ray is given by a point
// supossedly inside the triangle and a direction. We must find the
// position where the ray gets out of the triangle and the triangle
// into which it goes. If the exit edge is a border, return -1 for the
// neighbor triangle.
int Road_manager::trace(int trig, const Point3f &pos, const Point3f &dir, 
			Point3f &out_pos)
{
  int edge=-1, vertex=-1;
  edge = intersect_trig(trig, pos, dir, out_pos);
  if (edge > 2) {
    vertex = edge-3;
    edge = -1;
  }
  if (edge != -1)
    return trace_edge(trig, edge);
  else if (vertex != -1)
    return trace_vertex(trig, vertex, out_pos, dir);
  else
    return -2; // not inside the triangle
}

// Find out if the ray coming in intersect the triangle again in any
// points that is different from the starting one. Returns -2 if
// there's no new intersection, meaning that the ray doesn't go into
// the triangle. The new intersection point if stored in the parameter
// out_pos. The returned value is 0-2 if the intersection is on an
// edge, and 3-5 if it's a vertex, in which case the vertex in
// question is the returned value -3.
int Road_manager::intersect_trig(int trig, const Point3f &pos, 
				 const Point3f &dir, Point3f &out_pos)
{
  int edge=-1, vertex=-1, edge1=-1, vertex1=-1, i, ret, ret1;
  float u[3];
  Point3f a[3], ed[3], intr, intr1, d, d1;
  for (i=0; i<3; i++)
    a[i] = coords[trig][i];
  for (i=0; i<3; i++) {
    ed[i] = a[(i+1) % 3];
    ed[i] -= a[i];
    u[i] = a[i].intersect_xz(ed[i], pos, dir);
  }
  for (i=0; i<3; i++) {
    if (u[i] >= 0 && u[i] <= 1) { //&& !pos.on_line_xz(a[i], ed[i])) {
      if (edge == -1 && vertex == -1) {
	intr = a[i];
	ed[i] *= u[i];
	intr += ed[i];
	if (u[i] == 1)
	  vertex = (i+1) % 3;
	else if (u[i] == 0)
	  vertex == i;
	else
	  edge = i;
      }
      else {
	intr1 = a[i];
	ed[i] *= u[i];
	intr1 += ed[i];
	if (u[i] == 1)
	  vertex1 = (i+1) % 3;
	else if (u[i] == 0)
	  vertex1 == i;
	else
	  edge1 = i;
      }
    }
  }
  out_pos = pos;
  if (edge1 == -1 && vertex1 == -1) 
    if (edge != -1 || vertex != -1) 
      ret = 0;
    else 
      ret = -2;
  else {
    d = intr;
    d -= pos;
    d1 = intr1;
    d1 -= pos;
    if (d[0] * d1[0] + d[2] * d1[2] >= 0) {// d and d1 go in the same direction
      // they both go in the direction of movement
      if (d[0] * dir[0] + d[2] * dir[2]>= 0) 
	// first point is closer
	if (d[0]*d[0]+d[2]*d[2] < d1[0]*d1[0]+d1[2]*d1[2])
	  ret = 1;
	else
	  ret = 0;
      else
	ret = -2;
    }
    else {
      if (d[0] * dir[0] + d[2] * dir[2]> 0)
	ret = 0;
      else
	ret = 1;

    }
  }
  if (ret == 0) {
    out_pos = intr;
    if (edge != -1)
      return edge;
    else if (vertex != -1)
      return vertex+3;
  }
  else if (ret == 1) {
    out_pos = intr1;
    if (edge1 != -1)
      return edge1;
    else if (vertex1 != -1)
      return vertex1+3;
  }
  else
    return ret;
}

// Find a neighbor sharing the same edge. Return -1 if that edge is a
// border.
int Road_manager::trace_edge(int trig, int edge)
{
  if (is_border[trig] == edge)
    return -1;
  int i, j;
  for (j=0; j<neighbors[trig].size(); j++) {
    i = neighbors[trig][j];
    if (edges[trig][j] == edge)
      return i;
  }
  return -1; // found a border
}

// Find a neighbor sharing the same vertex. It has to check both the
// triangles having in common an edge containing this vertex and those
// having this vertex in common. Returns -1 if it doesn't find
// anything, meaning that we found a border.
int Road_manager::trace_vertex(int trig, int vertex, const Point3f &pos, 
			       const Point3f &dir)
{
  Point3f p;
  int i, j, ed, k;
  for (j=0; j<neighbors[trig].size(); j++) {
    i = neighbors[trig][j];
    ed = edges[trig][j];
    if (ed == vertex+3 || ed == vertex || ed == (vertex+1) % 3) {
      k = intersect_trig(i, pos, dir, p);
      if (k >= 0)
	return k;
    }
  }
  return -1; // found a border
}

// Trace a ray inside the triangle mesh until it gets out of the mesh
// or until it cannot find a new intersection anymore.
int Road_manager::trace_out(int trig, const Point3f &pos, const Point3f &dir, 
			    Point3f &out_pos)
{
  Point3f old_pos = pos;
  int triangle = trig, oldt = trig;
  while (oldt >= 0) {
    triangle = trace(triangle, old_pos, dir, out_pos);
    //cout << triangle << ' ' << out_pos << ' ' << old_pos << endl;
    if (out_pos.almost_eq(old_pos)) // triangle == oldt && 
      oldt = -1;
    else {
      old_pos = out_pos;
      oldt = triangle;
    }
  }
  return triangle;
}
