//	Vamos Automotive Simulator
//  Copyright (C) 2001--2003 Sam Varner
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include <vamos/track/Road_Segment.h>
#include <vamos/geometry/Gl_Texture_Image.h>
//#include <vamos/geometry/Ac3d.h>

//#include <GL/glut.h>

#include <cmath>
#include <cassert>
#include <iostream>

using namespace Vamos_Geometry;

//=============================================================================
Vamos_Track::
Kerb::Kerb (const std::vector <Two_Point>& profile,
			double start, 
			double start_transition_length, 
			double start_transition_width,
			double end, 
			double end_transition_length, 
			double end_transition_width) :
  m_points (profile),
  m_profile (profile),
  m_start (start),
  m_start_transition_length (start_transition_length),
  m_start_transition_width (start_transition_width),
  m_end (end),
  m_end_transition_length (end_transition_length),
  m_end_transition_width (end_transition_width),
  m_full_length (m_end < 0.0)
{
}

void Vamos_Track::
Kerb::length (double length)
{
  if (m_full_length)
	{
	  m_end = length;
	}
}

double Vamos_Track::
Kerb::width () const
{
  return (m_points.size () > 0) ? (m_points.end () - 1)->x : 0.0;
}

bool Vamos_Track::
Kerb::on_kerb (double dist) const
{
  //!!TODO handle skew
  return bool ((dist >= m_start) 
			   && ((dist < m_end) || (m_end < 0.0)));
}

double Vamos_Track::
Kerb::elevation (double along, double from_inside)
{
  //!!TODO handle skew
  if (!on_kerb (along) || (from_inside < 0.0) || (from_inside > width ()))
	{
	  return 0.0;
	}
  return m_profile.interpolate (from_inside);
}

double Vamos_Track::
Kerb::angle (double along, double from_inside)
{
  if (!on_kerb (along) || (from_inside < 0.0) || (from_inside > width ()))
	{
	  return 0.0;
	}
  Two_Point norm = m_profile.normal (from_inside);
  return std::atan2 (norm.y, norm.x) - pi / 2.0;
}

const Two_Point& Vamos_Track::
Kerb::point (size_t substrip)
{
  assert (substrip < m_points.size ());
  return m_points [substrip];
}

//=============================================================================
Vamos_Track::Braking_Marker::
Braking_Marker (double distance,
				Two_Point size,
				Two_Point offset,
				Side side,
				std::string image_file) :
  m_distance (distance),
  m_offset (offset),
  m_side (side),
  mp_image (new Gl_Texture_Image (image_file, true, true, size.x, size.y))
{
}

Vamos_Track::
Braking_Marker::~Braking_Marker ()
{
  delete mp_image;
}


//=============================================================================
Vamos_Track::Road_Segment::
Segment_Iterator::Segment_Iterator (double res, size_t strips)
  : texture_coordinates (Two_Point (0.0, 0.0)),
	resolution (res)
{
}

void Vamos_Track::Road_Segment::
Segment_Iterator::start (const Three_Vector& start_coords, double start_angle)
{
  coordinates = start_coords;
  normal = Three_Vector (0.0, 0.0, 1.0);
  distance = 0.0;
  angle = start_angle;

  left_side = true;
  first = true;
  last = false;
  substrip = 0;
}

//=============================================================================
Vamos_Track::
Road_Segment::Road_Segment (double resolution, double length, 
                            double radius, double skew,
							const std::vector <Two_Point>& left_width, 
							const std::vector <Two_Point>& right_width, 
							const std::vector <Two_Point>& left_road_width, 
							const std::vector <Two_Point>& right_road_width, 
							Kerb* left_kerb, Kerb* right_kerb,
							double left_wall_height, double right_wall_height,
							//const std::vector <Two_Point>& elevation_points,
							const std::vector <Two_Point>& bank_points,
							double end_bank, 
							double bank_pivot,
							const std::vector <Material_Handle>& materials,
							const std::vector <Braking_Marker*>& 
							braking_markers) :
  m_gl_texture_name (0),
  m_gl_list_id (0),
  m_length (length),
  m_radius (radius),
  m_start_skew (skew),
  m_end_skew (-skew),
//  m_arc (radius == 0.0 ? 0.0 : length / radius),
  mp_left_width (new Linear_Interpolator (left_width)),
  mp_right_width (new Linear_Interpolator (right_width)),
  mp_left_road_width (new Linear_Interpolator (left_road_width)),
  mp_right_road_width (new Linear_Interpolator (right_road_width)),
  mp_left_kerb (left_kerb),
  mp_right_kerb (right_kerb),
  m_left_wall_height (left_wall_height),
  m_right_wall_height (right_wall_height),
  //m_elevation_points (elevation_points),
  //mp_elevation_curve (0),
  m_bank_points (bank_points),
  m_start_bank (0.0),
  m_end_bank (end_bank),
  m_bank_pivot (bank_pivot),
  mp_bank_curve (0),
  m_materials (materials),
  m_last_segment (false),
  m_min_x (0.0),
  m_max_x (0.0),
  m_min_y (0.0),
  m_max_y (0.0),
  m_texture_offsets (materials.size ()),
  mp_iterator (new Segment_Iterator (resolution, materials.size ()))
{
  for (std::vector <Braking_Marker*>::const_iterator 
		 it = braking_markers.begin ();
	   it != braking_markers.end ();
	   it++)
	{
	  m_braking_markers.push_back (*it);
	}
}

Vamos_Track::
Road_Segment::~Road_Segment ()
{
  delete mp_iterator;
  delete [] m_gl_texture_name;
  delete mp_bank_curve;
  delete mp_right_road_width;
  delete mp_left_road_width;
  delete mp_right_width;
  delete mp_left_width;

  delete mp_left_kerb;
  delete mp_right_kerb;

  for (std::vector <Braking_Marker*>::iterator 
		 it = m_braking_markers.begin ();
	   it != m_braking_markers.end ();
	   it++)
	{
	  delete *it;
	}

  glDeleteLists (m_gl_list_id, 1);
/*  for (std::vector <GLuint>::iterator it = m_scenery_lists.begin ();
	   it != m_scenery_lists.end ();
	   it++)
	{
	  glDeleteLists (*it, 1);
	}*/
}

void Vamos_Track::
Road_Segment::set_skew (double skew)
{
  m_start_skew = skew;
  m_end_skew = -skew;
}

/*void Vamos_Track::
Road_Segment::build_elevation (Spline* elevation,
							   double start_distance)
{
  mp_elevation_curve = elevation;
  for (std::vector <Two_Point>::iterator it = m_elevation_points.begin ();
	   it != m_elevation_points.end ();
	   it++)
	{
	  mp_elevation_curve->load (*it + Two_Point (start_distance, 0.0));
	}

  if (m_last_segment)
	{
	  // Remove any elevation points near or greater than m_length.
	  mp_elevation_curve->remove_greater (start_distance + m_length - 10.0);
	  // Force the elevation curve to return to zero.
	  mp_elevation_curve->load (Two_Point (start_distance + m_length, 0.0)); 
	}
}*/

float normscale = -1.0f;

void Vamos_Track::
Road_Segment::build (double start_distance,
					 const Three_Vector& start_coords, 
					 double start_angle,
					 double start_bank,
					 const std::vector <double>& texture_offsets,
					 GLuint detailtex)
{
/*  for (std::vector <GLuint>::iterator it = m_scenery_lists.begin ();
	   it != m_scenery_lists.end ();
	   it++)
	{
	  glDeleteLists (*it, 1);
	}
  m_scenery_lists.clear ();*/

/*  for (std::vector <Model_Info>::iterator it = m_models.begin ();
	   it != m_models.end ();
	   it++)
	{
	  Ac3d model (it->file, it->scale, it->translation, 
				  deg_to_rad (1.0) * it->rotation);
	  m_scenery_lists.push_back (model.build ());
	}*/

  m_start_distance = start_distance;

  glPixelStorei (GL_UNPACK_ALIGNMENT, 1);

  if (m_gl_list_id != 0)
	{
	  glDeleteLists (m_gl_list_id, 1);
	}
  m_gl_list_id = glGenLists (1);
  glNewList (m_gl_list_id, GL_COMPILE);

	//glCullFace(GL_BACK);
	  //glEnable (GL_CULL_FACE);
	glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1);
	
  // Put the textures on a shiny white surface so we can see shading.
  glColorMaterial (GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
  glEnable (GL_COLOR_MATERIAL);
  glColor3f (1.0, 1.0, 1.0);
  GLfloat specular [] = { 0.0, 0.0, 0.0, 0.0 };
  glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, specular);
  GLfloat shininess [] = { 0.0 };
  glMaterialfv (GL_FRONT_AND_BACK, GL_SHININESS, shininess);

  glTexEnvf (GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);

  normscale = -1.0f;
  
  // Draw the left barrier.
  start (start_coords, start_angle, start_bank);
  draw_strip (LEFT_BARRIER, texture_offsets [LEFT_BARRIER], detailtex);

  normscale = 1.0f;
  
  glDisable(GL_STENCIL_TEST);
  
  // Draw the left shoulder and kerb.
  start (start_coords, start_angle, start_bank);
  //glDepthMask (GL_FALSE);
  draw_strip (LEFT_SHOULDER, texture_offsets [LEFT_SHOULDER], detailtex);
  glDepthMask (GL_TRUE);
  start (start_coords, start_angle, start_bank);
  draw_strip (LEFT_KERB, texture_offsets [LEFT_KERB], detailtex);
  start (start_coords, start_angle, start_bank);
  glColorMask (GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
  draw_strip (LEFT_SHOULDER, texture_offsets [LEFT_SHOULDER], detailtex);
  glColorMask (GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);

  // Draw the track.
  start (start_coords, start_angle, start_bank);
  draw_strip (TRACK, texture_offsets [TRACK], detailtex);

  // Draw the right shoulder and kerb.
  start (start_coords, start_angle, start_bank);
  //glDepthMask (GL_FALSE);
  draw_strip (RIGHT_SHOULDER, texture_offsets [RIGHT_SHOULDER], detailtex);
  glDepthMask (GL_TRUE);
  start (start_coords, start_angle, start_bank);
  draw_strip (RIGHT_KERB, texture_offsets [RIGHT_KERB], detailtex);
  start (start_coords, start_angle, start_bank);
  glColorMask (GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
  draw_strip (RIGHT_SHOULDER, texture_offsets [RIGHT_SHOULDER], detailtex);
  glColorMask (GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);

  normscale = -1.0f;

  glEnable(GL_STENCIL_TEST);

  // Draw the right barrier.
  start (start_coords, start_angle, start_bank);
  draw_strip (RIGHT_BARRIER, texture_offsets [RIGHT_BARRIER], detailtex);
  
  normscale = 1.0f;

  // Draw the braking markers.
  for (std::vector <Braking_Marker*>::iterator 
		 it = m_braking_markers.begin ();
	   it != m_braking_markers.end ();
	   it++)
	{
	  (*it)->image ()->activate ();

	  double along = m_length - (*it)->distance ();
	  double from_center = (*it)->offset ().x;
 	  if ((*it)->side () == RIGHT)
 		{
 		  from_center = -(from_center 
						  + right_road_width (along) 
						  + (*it)->image ()->width ());
 		}
 	  else
 		{
 		  from_center += left_road_width (along);
 		}

	  double angle = start_angle + arc() * along / m_length;

	  double x1 = m_center[0] - from_center * sin (angle);
	  double y1 = m_center[1] + from_center * cos (angle);
	  if (m_radius == 0.0)
		{
		  x1 += along * cos (angle);
		  y1 += along * sin (angle);
		}
	  else
		{
 		  x1 += m_radius * sin (angle);
 		  y1 -= m_radius * cos (angle);
		}
		double z1 = elevationxy (x1,y1) + (*it)->offset ().y;
	  //double z1 = elevation (along, from_center) + (*it)->offset ().y;
		//double z1 = (*it)->offset ().y;
		//double z1 = 0;

	  double x2 = x1 - (*it)->image ()->width () * sin (angle);
	  double y2 = y1 + (*it)->image ()->width () * cos (angle);
	  double z2 = z1 + (*it)->image ()->height ();
	  
		glCullFace(GL_BACK);
	  glEnable (GL_CULL_FACE);

	  glBegin (GL_QUADS);

	  glNormal3d (-cos (angle), -sin (angle), 0.0);
	  //glNormal3d (cos (angle), sin (angle), 0.0);
	  glTexCoord2d (1.0, 1.0);
	  glVertex3f (x1, y1, z1);
	  glTexCoord2d (1.0, 0.0);
	  glVertex3f (x1, y1, z2);
	  glTexCoord2d (0.0, 0.0);
	  glVertex3f (x2, y2, z2);
	  glTexCoord2d (0.0, 1.0);
	  glVertex3f (x2, y2, z1);

	  glNormal3d (cos (angle), sin (angle), 0.0);
	  //glNormal3d (-cos (angle), -sin (angle), 0.0);
	  glTexCoord2d (0.01, 0.01);
	  glVertex3f (x1, y1, z1);
	  glTexCoord2d (0.0, 0.01);
	  glVertex3f (x2, y2, z1);
	  glTexCoord2d (0.0, 0.0);
	  glVertex3f (x2, y2, z2);
	  glTexCoord2d (0.01, 0.0);
	  glVertex3f (x1, y1, z2);

	  glEnd ();

	  glDisable (GL_CULL_FACE);
	}
  
  glFlush ();

  glPushMatrix ();
  glTranslatef (m_start_coords [0], m_start_coords [1], m_start_coords [2]);
  glRotatef (rad_to_deg (m_start_angle), 0.0, 0.0, 1.0);
  //std::for_each (m_scenery_lists.begin (), m_scenery_lists.end (), glCallList);
  glPopMatrix ();
	glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 0);
  glEndList ();
}

void Vamos_Track::
Road_Segment::draw_strip (size_t i, double texture_offset, GLuint detailtex)
{
	//if (m_materials [i]->type() == Material::GRASS)
		//return;
	
	bool detail = false;
	
	if (m_materials [i]->type() == Material::GRASS)
		detail = true;
	
	if (m_materials [i]->type() == Material::ASPHALT)
	{
		detail = true;
		//glCullFace(GL_BACK);
    	//glEnable(GL_CULL_FACE);
	}
	
	//glCullFace(GL_BACK);
    //glEnable(GL_CULL_FACE);
	
	//if (m_materials [i]->type() == Material::ASPHALT || m_materials [i]->type() == Material::GRASS || m_materials [i]->type() == Material::GRAVEL)
	if (m_materials [i]->type() == Material::ASPHALT || m_materials [i]->type() == Material::GRAVEL)
	{
		//glDepthMask(0);
		//glColorMask(0, 0, 0, 0);
		detail = true;
	}
	
	utility.SelectTU(0);
	
  Gl_Texture_Image* image = m_materials [i]->texture_image ();
  image->activate ();

  mp_iterator->first = true;

	//activate detail texturing
	if (detail && utility.numTUs() > 1)
	{
		utility.SelectTU(1);
		glEnable(GL_TEXTURE_2D);
		//image->activate ();
		glBindTexture(GL_TEXTURE_2D, detailtex);
		glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_COMBINE_ARB);
		glTexEnvi(GL_TEXTURE_ENV, GL_SOURCE0_RGB_ARB, GL_PREVIOUS_ARB);
		glTexEnvi(GL_TEXTURE_ENV, GL_SOURCE1_RGB_ARB, GL_TEXTURE);
		glTexEnvi(GL_TEXTURE_ENV,GL_COMBINE_RGB_ARB,GL_MODULATE);
		glTexEnvi(GL_TEXTURE_ENV, GL_RGB_SCALE_ARB, 2);
		
		if (utility.numTUs() > 2)
		{
			utility.SelectTU(2);
			glEnable(GL_TEXTURE_2D);
			//image->activate ();
			glBindTexture(GL_TEXTURE_2D, detailtex);
			glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_COMBINE_ARB);
			glTexEnvi(GL_TEXTURE_ENV, GL_SOURCE0_RGB_ARB, GL_PREVIOUS_ARB);
			glTexEnvi(GL_TEXTURE_ENV, GL_SOURCE1_RGB_ARB, GL_TEXTURE);
			glTexEnvi(GL_TEXTURE_ENV,GL_COMBINE_RGB_ARB,GL_MODULATE);
			glTexEnvi(GL_TEXTURE_ENV, GL_RGB_SCALE_ARB, 2);
		}
	}
	else
	{
		utility.SelectTU(3);
		glDisable(GL_TEXTURE_2D);
		utility.SelectTU(2);
		glDisable(GL_TEXTURE_2D);
		utility.SelectTU(1);
		glDisable(GL_TEXTURE_2D);
		utility.SelectTU(0);
		glEnable(GL_TEXTURE_2D);
	}
	
	if (utility.numTUs() > 3 && state.GetTreeDetail() != "Off" && state.GetTreeDetail() != "FoliageOnly")
	{
		utility.SelectTU(3);
		glEnable(GL_TEXTURE_2D);
		glBindTexture(GL_TEXTURE_2D, trees.GetCompositeShadow());
		glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_COMBINE_ARB);
		glTexEnvi(GL_TEXTURE_ENV, GL_SOURCE0_RGB_ARB, GL_PREVIOUS_ARB);
		glTexEnvi(GL_TEXTURE_ENV, GL_SOURCE1_RGB_ARB, GL_TEXTURE);
		glTexEnvi(GL_TEXTURE_ENV,GL_COMBINE_RGB_ARB,GL_MODULATE);
		glTexEnvi(GL_TEXTURE_ENV, GL_RGB_SCALE_ARB, 1);
	}
	
	utility.SelectTU(0);
	/*if (m_materials [i]->type() == Material::GRASS)
	{
		utility.SelectTU(2);
		glDisable(GL_TEXTURE_2D);
		utility.SelectTU(1);
		glDisable(GL_TEXTURE_2D);
		utility.SelectTU(0);
		glEnable(GL_TEXTURE_2D);
		
		glDepthMask(0);
		
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	}*/
	
  Three_Vector vertex = next_vertex (i);
  glNormal3d (normscale*mp_iterator->normal [0], 
			  normscale*mp_iterator->normal [1], 
			  normscale*mp_iterator->normal [2]);
  double tex_width = image->width (); 
  double tex_height = image->height ();
//  tex_height = 10.0;
//  tex_width = 10.0;
//cout << tex_height << "," << tex_width << endl;
  double tex_x = (tex_width > 0.0)
	? mp_iterator->texture_coordinates.x / tex_width : 0.0;
  double tex_y = (tex_height > 0.0)
	? mp_iterator->texture_coordinates.y / tex_height : 0.0;
  tex_y += texture_offset;
  //glTexCoord2d (tex_x, tex_y);
  	float detailscalex = 1.0/5.0;
	float detailscaley = 1.0/50.0;
	float detailscale2x = 1.0/4.0;
	float detailscale2y = 1.0/4.0;
	float scalex = 1.0;
	float scaley = 1.0;
	
	if (m_materials [i]->type() == Material::GRASS)
	{
		//detailscalex = 1.0 / 10.0;
		//detailscalex = 19.0;
		detailscalex = 5.0;
		detailscaley = detailscalex;
		//detailscale2x = 10.0;
		detailscale2x = 49.0;
		detailscale2y = detailscale2x;
		
		scalex = 0.5;
		//scalex = 1.0;
		scaley = scalex;
	}
	
	if (m_materials [i]->type() == Material::GRAVEL)
	{
		//detailscalex = 1.0/5.0;
		detailscalex = 1.0/2.0;
		detailscaley = detailscalex;
		detailscale2x = 100.0;
		detailscale2y = detailscale2x;
		
		scalex = 0.5;
		scaley = scalex;
	}
	
	if (m_materials [i]->type() == Material::ASPHALT)
	{
		scalex = 1.0;
		scaley = 1.0;
	}
	
  utility.TexCoord2d2f(0, tex_x/scalex, tex_y/scaley);
  
	if (detail && utility.numTUs() > 1)
	{
  		utility.TexCoord2d2f(1, tex_x/detailscalex, tex_y/detailscaley);
		if (utility.numTUs() > 2)
			utility.TexCoord2d2f(2, tex_x/detailscale2x, tex_y/detailscale2y);
		if (utility.numTUs() > 3)
			utility.TexCoord2d2f(3, (vertex[0]-terrain.GetOffset().x)/terrain.GetSize().x, (-vertex[1]-terrain.GetOffset().z)/terrain.GetSize().z);
		
		//if ((vertex[0]-terrain.GetOffset().x)/terrain.GetSize().x > 0.9)
			//cout << (vertex[0]-terrain.GetOffset().x)/terrain.GetSize().x << endl;
		//cout << vertex[1] << endl;
	}
  glVertex3d (vertex [0], vertex [1], vertex [2]);
  
  vertex = next_vertex (i);
  glNormal3d (normscale*mp_iterator->normal [0], 
			  normscale*mp_iterator->normal [1], 
			  normscale*mp_iterator->normal [2]);
  tex_x = (tex_width > 0.0)
	? mp_iterator->texture_coordinates.x / tex_width : 1.0;
  //glTexCoord2d (tex_x, tex_y);
	utility.TexCoord2d2f(0, tex_x/scalex, tex_y/scaley);
	if (detail && utility.numTUs() > 1)
	{
  		utility.TexCoord2d2f(1, tex_x/detailscalex, tex_y/detailscaley);
		if (utility.numTUs() > 2)
			utility.TexCoord2d2f(2, tex_x/detailscale2x, tex_y/detailscale2y);
		if (utility.numTUs() > 3)
			utility.TexCoord2d2f(3, (vertex[0]-terrain.GetOffset().x)/terrain.GetSize().x, (-vertex[1]-terrain.GetOffset().z)/terrain.GetSize().z);
	}
  glVertex3d (vertex [0], vertex [1], vertex [2]);
  
  while (!mp_iterator->last)
	{
	  vertex = next_vertex (i);
	  glNormal3d (normscale*mp_iterator->normal [0], 
				  normscale*mp_iterator->normal [1], 
				  normscale*mp_iterator->normal [2]);
	  tex_x = (tex_width > 0.0)
		? mp_iterator->texture_coordinates.x / tex_width : 0.0;
	  tex_y = (tex_height > 0.0)
		? mp_iterator->texture_coordinates.y / tex_height : 1.0;
	  tex_y += texture_offset;
	  //glTexCoord2d (tex_x, tex_y);
		utility.TexCoord2d2f(0, tex_x/scalex, tex_y/scaley);
		if (detail && utility.numTUs() > 1)
		{
			utility.TexCoord2d2f(1, tex_x/detailscalex, tex_y/detailscaley);
			if (utility.numTUs() > 2)
				utility.TexCoord2d2f(2, tex_x/detailscale2x, tex_y/detailscale2y);
			if (utility.numTUs() > 3)
				utility.TexCoord2d2f(3, (vertex[0]-terrain.GetOffset().x)/terrain.GetSize().x, (-vertex[1]-terrain.GetOffset().z)/terrain.GetSize().z);
		}
	  glVertex3d (vertex [0], vertex [1], vertex [2]);
	  
	  vertex = next_vertex (i);
	  glNormal3d (normscale*mp_iterator->normal [0], 
				  normscale*mp_iterator->normal [1], 
				  normscale*mp_iterator->normal [2]);
	  
	  tex_x = (tex_width > 0.0) ?
		mp_iterator->texture_coordinates.x / tex_width : 1.0;
	  //glTexCoord2d (tex_x, tex_y);
		utility.TexCoord2d2f(0, tex_x/scalex, tex_y/scaley);
		if (detail && utility.numTUs() > 1)
		{
			utility.TexCoord2d2f(1, tex_x/detailscalex, tex_y/detailscaley);
			if (utility.numTUs() > 2)
				utility.TexCoord2d2f(2, tex_x/detailscale2x, tex_y/detailscale2y);
			if (utility.numTUs() > 3)
				utility.TexCoord2d2f(3, (vertex[0]-terrain.GetOffset().x)/terrain.GetSize().x, (-vertex[1]-terrain.GetOffset().z)/terrain.GetSize().z);
		}
	  glVertex3d (vertex [0], vertex [1], vertex [2]);
	} 
  glEnd ();

  m_texture_offsets [i] = tex_y;
	
	glDisable(GL_BLEND);
	glDisable(GL_CULL_FACE);
	glDepthMask(1);
	glColorMask(1,1,1,1);
}

void Vamos_Track::
Road_Segment::add_model_info (const Model_Info& info)
{
  //m_models.push_back (info);
}

void Vamos_Track::
Road_Segment::draw () const
{
  glCallList (m_gl_list_id);
}

/*double Vamos_Track::
Road_Segment::elevation (double along, double from_center)
{
  //assert (mp_elevation_curve != 0);
  //double elev = mp_elevation_curve->interpolate (along + m_start_distance)
	//+ bank_height (along, from_center);
	
	//double elev = bank_height (along, from_center);
	double elev = 0;

  // Add the kerb's height.
  double diff = from_center - left_road_width (along);
  elev += mp_left_kerb->elevation (along, diff);
  diff = -from_center - right_road_width (along);
  elev += mp_right_kerb->elevation (along, diff);

  return elev;
}*/

double Vamos_Track::
Road_Segment::angle (double along) const
{
  return m_start_angle + arc() * along / m_length;
}

Three_Vector Vamos_Track::
Road_Segment::barrier_normal (double along, double from_center) const
{
  double this_angle = angle (along);
  if (from_center > 0.0)
	{
	  return Three_Vector (sin (this_angle), -cos (this_angle), 0.0);
	}
  return Three_Vector (-sin (this_angle), cos (this_angle), 0.0);
}

double Vamos_Track::
Road_Segment::bank (double along) const
{
  return deg_to_rad (mp_bank_curve->interpolate (along));
}

double Vamos_Track::
Road_Segment::bank_height (double along, double from_center) const
{
  return (m_bank_pivot - from_center) * sin (bank (along));
}


Three_Vector Vamos_Track::
Road_Segment::normal (double along, double from_center) const
{
  //assert (mp_elevation_curve != 0);

  //Two_Point norm = mp_elevation_curve->normal (along + m_start_distance);
	Two_Point norm (0,1);

  double bank = deg_to_rad (mp_bank_curve->interpolate (along));
  double diff = from_center - left_road_width (along);
  bank -= mp_left_kerb->angle (along, diff);
  diff = -from_center - right_road_width (along);
  bank += mp_right_kerb->angle (along, diff);

  double x = norm.x;
  double y = sin (bank);
  double z = cos (bank) * norm.y;
  double this_angle = m_start_angle + arc() * along / m_length;
  return Three_Vector (cos (this_angle) * x - sin (this_angle) * y,
					   sin (this_angle) * x + cos (this_angle) * y,
					   z);
}

const Material_Handle Vamos_Track::
Road_Segment::material_at (double along, double from_center)
{
  //!!TODO handle skew
  double left_kerb_width = 0.0;
  double right_kerb_width = 0.0;

  if (mp_left_kerb->on_kerb (along))
	{
	  left_kerb_width = mp_left_kerb->width ();
	}

  if (mp_right_kerb->on_kerb (along))
	{
	  right_kerb_width = mp_right_kerb->width ();
	}

  if (from_center > left_road_width (along) + left_kerb_width)
	{
	  return m_materials [1];
	}
  else if (from_center > left_road_width (along))
	{
	  return m_materials [2];
	}
  else if (from_center > -right_road_width (along))
	{
	  return m_materials [3];
	}
  else if (from_center > -right_road_width (along) - right_kerb_width)
	{
	  return m_materials [4];
	}
  else
	{
	  return m_materials [5];
	}
}


void Vamos_Track::
Road_Segment::start (const Three_Vector& start_coords, 
					 double start_angle,
					 double start_bank)
{
  m_center[0] = start_coords [0] - m_radius * sin (start_angle);
  m_center[1] = start_coords [1] + m_radius * cos (start_angle);

  m_start_coords = start_coords;
  if (m_radius == 0.0)
	{
	  m_end_coords = 
		Three_Vector (start_coords [0] + m_length * cos (start_angle),
					  start_coords [1] + m_length * sin (start_angle),
					  0.0);
	}
  else
	{
	  m_end_coords = 
		Three_Vector (m_center[0] + m_radius * sin (start_angle + arc()),
					  m_center[1] - m_radius * cos (start_angle + arc()),
					  0.0);
	}

  m_start_angle = start_angle;
  if (m_start_angle > 2.0 * pi)
	m_start_angle -= 2.0 * pi;
  else if (m_start_angle < 0.0)
	m_start_angle += 2.0 * pi;

  mp_iterator->start (start_coords, start_angle);
  
  // Initialize bank stuff.
  m_start_bank = start_bank;

  m_bank_points.clear ();
  m_bank_points.insert (m_bank_points.begin (), 
						Two_Point (0.0, m_start_bank));
  m_bank_points.
	push_back (Two_Point (m_length, m_end_bank));
  
  delete mp_bank_curve;
  mp_bank_curve = new Spline (m_bank_points, 0.0, 0.0);
}

void Vamos_Track::
Road_Segment::increment_kerb_distance (const Kerb& kerb)
{
  // Go to the end of the kerb if we were at the end transition.
  if ((mp_iterator->distance >= kerb.end ())
	  && ((kerb.end () - mp_iterator->distance) 
		  < (0.01 * mp_iterator->resolution)))
	{
	  mp_iterator->distance = kerb.transition_end ();
	  mp_iterator->last = true;
	}
  else if (mp_iterator->distance < kerb.start ())
	{
	  mp_iterator->distance = kerb.start ();
	}
  else if ((mp_iterator->distance + mp_iterator->resolution) 
		   >= kerb.transition_end ())
	{
	  mp_iterator->distance = kerb.end ();
	}
  else
	{
	  mp_iterator->distance += mp_iterator->resolution;
	}
}

// Return the corrdinates for the next vertex to be plotted.  Also,
// calculate the texture coordinates and store them in the iterator.
// Any neccary state information is also stored in the iterator.
Three_Vector Vamos_Track::
Road_Segment::next_vertex (int strip)
{
  //assert (mp_elevation_curve != 0);

  // Remember the last coordinates.
  Three_Vector last_coords = mp_iterator->coordinates;
  double last_tex_dist = mp_iterator->texture_coordinates.y;
  //  double left_kerb_width = mp_left_kerb->width ();
  //  double right_kerb_width = mp_right_kerb->width ();
  // True if we've reached the end of the strip.
  bool left_start_transition = false;
  bool left_end_transition = false;
  bool right_start_transition = false;
  bool right_end_transition = false;

  // mp_iterator->first is true if we're starting a strip.
  if (mp_iterator->first)
	{
	  glEnd ();
	  glBegin (GL_QUAD_STRIP);
	  //glBegin (GL_LINE_STRIP);

	  if (strip == LEFT_KERB)
		{
		  // Bail out if there's no kerb.
		  if (mp_left_kerb->transition_end () == 0.0)
			{
			  mp_iterator->last = true;
			  return mp_iterator->coordinates;
			}
		  mp_iterator->distance = mp_left_kerb->transition_start ();
		}
	  else if (strip == RIGHT_KERB)
		{
		  // Bail out if there's no kerb.
		  if (mp_right_kerb->transition_end () == 0.0)
			{
			  mp_iterator->last = true;
			  return mp_iterator->coordinates;
			}
		  mp_iterator->distance = mp_right_kerb->transition_start ();
		}
	  else
		{
		  mp_iterator->distance = 0.0;
		}
	  last_tex_dist = 0.0;
	  mp_iterator->texture_coordinates.y = 0.0;
	}
  else if (mp_iterator->left_side)
	{
	  // Clamp to the end of the segment.
	  if ((m_length - mp_iterator->distance - mp_iterator->resolution) 
		  < (0.01 * mp_iterator->resolution))
		{
		  mp_iterator->distance = m_length;
		  mp_iterator->last = true;
		}
 	  else if (strip == LEFT_KERB)
 		{
 		  increment_kerb_distance (*mp_left_kerb);
 		}
 	  else if (strip == RIGHT_KERB)
 		{
 		  increment_kerb_distance (*mp_right_kerb);
 		}
	  else
		{
		  mp_iterator->distance += mp_iterator->resolution;
		}
	}

  size_t substrips = 1;
  if (strip == LEFT_KERB)
	{
	  mp_left_kerb->length (m_length);
	  substrips = mp_left_kerb->substrips ();
	  if (mp_iterator->distance < mp_left_kerb->start ())
		{
		  left_start_transition = true;
		}
	  else if (mp_iterator->distance > mp_left_kerb->end ())
		{
		  left_end_transition = true;
		}
	}
  else if (strip == RIGHT_KERB)
	{
	  mp_right_kerb->length (m_length);
	  substrips = mp_right_kerb->substrips ();
	  if (mp_iterator->distance < mp_right_kerb->start ())
		{
		  right_start_transition = true;
		}
	  else if (mp_iterator->distance > mp_right_kerb->end ())
		{
		  right_end_transition = true;
		}
	}

  // Calculate the geometry of the strip.
  double across = 0.0;
  double up = 0.0;
  switch (strip)
	{
	case LEFT_BARRIER:
	  across = -left_width (mp_iterator->distance); 
	  if (mp_iterator->left_side)
		{
		  up = m_left_wall_height;
		}
	  else
		{
		  up = 0.0;
		}
	  mp_iterator->texture_coordinates.x = up;
	  mp_iterator->normal = barrier_normal (mp_iterator->distance, across);
	  break;
	case LEFT_SHOULDER:
	  up = 0.0;
	  if (mp_iterator->left_side)
		{
		  across = -left_width (mp_iterator->distance);
		}
	  else
		{
		  across = -left_road_width (mp_iterator->distance);
		}
	  mp_iterator->texture_coordinates.x = across;
	  mp_iterator->normal = normal (mp_iterator->distance, across);
	  break;
	case LEFT_KERB:
	  up = 0.0;
	  across = 0.0;
	  if (!mp_iterator->left_side)
		{
		  up = mp_left_kerb->point (mp_iterator->substrip).y;
		  across = -mp_left_kerb->point (mp_iterator->substrip).x;
		}
	  else
		{
		  up = mp_left_kerb->point (mp_iterator->substrip + 1).y;
		  across = -mp_left_kerb->point (mp_iterator->substrip + 1).x;
		}
	  if (left_start_transition)
		{
		  up = 0.0;
		  across *= (mp_left_kerb->start_transition_width ()
					 / mp_left_kerb->width ());
		}
	  else if (left_end_transition)
		{
		  up = 0.0;
		  across *= (mp_left_kerb->end_transition_width ()
					 / mp_left_kerb->width ());
		}

	  across -= left_road_width (mp_iterator->distance);
	  mp_iterator->texture_coordinates.x = across;
	  break;
	case TRACK:
	  up = 0.0;
	  if (mp_iterator->left_side)
		{
		  across = -left_road_width (mp_iterator->distance);
		}
	  else
		{
		  across = right_road_width (mp_iterator->distance);
		}
	  mp_iterator->texture_coordinates.x 
		= across + left_road_width (mp_iterator->distance);
	  mp_iterator->normal = normal (mp_iterator->distance, across);
	  break;
	case RIGHT_KERB:
	  up = 0.0;
	  across = 0.0;
	  if (mp_iterator->left_side)
		{
		  up = mp_right_kerb->point (mp_iterator->substrip).y;
		  across = mp_right_kerb->point (mp_iterator->substrip).x;
		}
	  else
		{
		  up = mp_right_kerb->point (mp_iterator->substrip + 1).y;
		  across = mp_right_kerb->point (mp_iterator->substrip + 1).x;
		}
	  if (right_start_transition)
		{
		  up = 0.0;
		  across *= (mp_right_kerb->start_transition_width ()
					 / mp_right_kerb->width ());
		}
	  else if (right_end_transition)
		{
		  up = 0.0;
		  across *= (mp_right_kerb->end_transition_width ()
					 / mp_right_kerb->width ());
		}

	  across += right_road_width (mp_iterator->distance);
	  mp_iterator->texture_coordinates.x = across;
	  break;
	case RIGHT_SHOULDER:
	  up = 0.0;
	  if (mp_iterator->left_side)
		{
		  across = right_road_width (mp_iterator->distance);
		}
	  else
		{
		  across = right_width (mp_iterator->distance);
		}
	  mp_iterator->texture_coordinates.x = across;
	  mp_iterator->normal = normal (mp_iterator->distance, across);
	  break;
	case RIGHT_BARRIER:
	  across = right_width (mp_iterator->distance);
	  if (mp_iterator->left_side)
		{
		  up = 0.0;
		}
	  else
		{
		  up = m_right_wall_height;
		}
	  mp_iterator->texture_coordinates.x = up;
	  mp_iterator->normal = barrier_normal (mp_iterator->distance, across);
	  break;
	default:
	  assert (false);
	}

  if ((mp_iterator->last && m_last_segment)
	  && (strip != LEFT_KERB)
	  && (strip != RIGHT_KERB))
	{
	  // Return to the origin if we're at the end of the track.
	  mp_iterator->coordinates = Three_Vector (0.0, -across, up);
		double elev = elevationxy (mp_iterator->coordinates[0], mp_iterator->coordinates[1]);
		mp_iterator->coordinates [2] = elev+up;
	}
  else
	{
	  double x;
	  double y;
      double extra = across 
        * (m_start_skew
           + ((m_end_skew - m_start_skew) * mp_iterator->distance 
              / m_length)); 
	  if (m_radius == 0.0)
		{
		  x = m_start_coords [0] 
			+ (mp_iterator->distance + extra) * cos (mp_iterator->angle);
		  y = m_start_coords [1] 
			+ (mp_iterator->distance + extra) * sin (mp_iterator->angle);

          mp_iterator->coordinates [0] = x + across * sin (mp_iterator->angle);
          mp_iterator->coordinates [1] = y - across * cos (mp_iterator->angle);
		}
	  else
		{
          double skew_angle = atan (m_start_skew);
          double radius = m_radius 
            + across * (1.0 - tan (skew_angle) / tan (arc() / 2.0));

          double center = across * tan (skew_angle) / sin (arc() / 2.0);
          double mid_angle = m_start_angle + arc() / 2.0;
          double center_x = m_center[0] + center * sin (mid_angle);
          double center_y = m_center[1] - center * cos (mid_angle);

		  mp_iterator->angle = 
			m_start_angle + mp_iterator->distance / m_radius;
		  x = center_x + radius * sin (mp_iterator->angle);
		  y = center_y - radius * cos (mp_iterator->angle);

          mp_iterator->coordinates [0] = x;
          mp_iterator->coordinates [1] = y;
		}
      /*mp_iterator->coordinates [2] = mp_elevation_curve->
        interpolate (mp_iterator->distance + m_start_distance)
        + bank_height (mp_iterator->distance, -across)
        + up;*/
		//mp_iterator->coordinates [2] = bank_height (mp_iterator->distance, -across) + up;
		//double elev = 0;
		double elev = elevationxy (mp_iterator->coordinates[0], mp_iterator->coordinates[1]);
		mp_iterator->coordinates [2] = elev+up;
	}

  if (!mp_iterator->first
	  && ((strip == LEFT_BARRIER) || (strip == RIGHT_BARRIER)))
	{
	  if (mp_iterator->left_side)
		{
		  double dx = mp_iterator->coordinates [0] - last_coords [0];
		  double dy = mp_iterator->coordinates [1] - last_coords [1];
		  double dist = std::sqrt (dx*dx + dy*dy);
		  mp_iterator->texture_coordinates.y = last_tex_dist + dist; 
		}
	}
  else
	{
	  mp_iterator->texture_coordinates.y = mp_iterator->distance;
	}

  if (!mp_iterator->left_side && mp_iterator->last)
	{
	  mp_iterator->distance = 0.0;
	}
  
  mp_iterator->first = false;

  if (mp_iterator->last && !mp_iterator->left_side)
	{
	  mp_iterator->substrip++;
	  mp_iterator->last = mp_iterator->substrip == substrips;
	  mp_iterator->first = !mp_iterator->last;
	}

  // Set mp_iterator->left_side to the correct value for the next time
  // next_vertex() is called.
  mp_iterator->left_side = !mp_iterator->left_side;

  // Update the track's bounds.
  if (mp_iterator->coordinates [0] < m_min_x)
	m_min_x = mp_iterator->coordinates [0];
  else if (mp_iterator->coordinates [0] > m_max_x)
	m_max_x = mp_iterator->coordinates [0];
  if (mp_iterator->coordinates [1] < m_min_y)
	m_min_y = mp_iterator->coordinates [1];
  else if (mp_iterator->coordinates [1] > m_max_y)
	m_max_y = mp_iterator->coordinates [1];

  return mp_iterator->coordinates;
}

double Vamos_Track::
Road_Segment::elevationxy (double x, double y) const
{
	//return 0.0;
	
	//return (x+y)/10.0;
	
	//return terrain.GetHeight(x,-y) + 0.5;
	//double elev = terrain.GetHeight(x,-y);
	double elev = terrain.GetHeight(x,-y) + 0.5;
	
	Three_Vector wc, tc;
	wc[0] = x;
	wc[1] = y;
	wc[2] = 0;
	coordinates(wc, tc);
	double diff = tc[1] - left_road_width (tc[0]);
	elev += mp_left_kerb->elevation (tc[0], diff);
	diff = -tc[1] - right_road_width (tc[0]);
	elev += mp_right_kerb->elevation (tc[0], diff);
	
	return elev;
}

double Vamos_Track::
Road_Segment::left_width (double dist) const
{
  return mp_left_width->interpolate (dist);
}

double Vamos_Track::
Road_Segment::right_width (double dist) const
{
  return mp_right_width->interpolate (dist);
}

double Vamos_Track::
Road_Segment::left_road_width (double dist) const
{
  return mp_left_road_width->interpolate (dist);
}

double Vamos_Track::
Road_Segment::right_road_width (double dist) const
{
  return mp_right_road_width->interpolate (dist);
}

void Vamos_Track::
Road_Segment::set_length (double length)
{
	scale_widths (length / m_length);
  	m_length = length;
	
  /*double factor = length / m_length;
  m_length = length;
  mp_left_width->scale (factor);
  mp_right_width->scale (factor);
  mp_left_road_width->scale (factor);
  mp_right_road_width->scale (factor);

  m_arc = (m_radius == 0.0) ? 0.0 : m_length / m_radius;*/
}

/*void Vamos_Track::
Road_Segment::set_arc (double arc)
{
  set_length (m_radius * arc);
}*/

void Vamos_Track::
Road_Segment::scale (double factor)
{
  assert (factor != 0);
  m_length *= factor;
  m_radius *= factor;
  mp_left_width->scale (factor);
  mp_right_width->scale (factor);
  mp_left_road_width->scale (factor);
  mp_right_road_width->scale (factor);
}

void Vamos_Track::
Road_Segment::scale_widths (double factor)
{
  mp_left_width->scale (factor);
  mp_right_width->scale (factor);
  mp_left_road_width->scale (factor);
  mp_right_road_width->scale (factor);
}

double Vamos_Track::
Road_Segment::skew_slope () const
{
  // theta = +/- 90 degrees, and skew == 0 must be handled specially.
  const double sign = Vamos_Geometry::sign (m_radius);
  double theta = sign * arc () / 2.0;
  assert (cos (theta) != 0.0);
  assert (m_start_skew != 0.0);

  double phi = theta - sign * atan (m_start_skew);
  return (cos (phi) != 0.0)
    ? 1.0 / (sin (theta) / tan (phi) - cos (theta))
    : -1.0 / cos (theta);
}

double solve_quadratic (double a, double b, double c, double solution)
{
	assert(a!= 0);
	if (b*b < 4*a*c)
	{
		return (-b) / (2*a);
	}
  return (-b + Vamos_Geometry::sign (solution) * sqrt (b*b - 4*a*c)) / (2*a);
}

double Vamos_Track::
Road_Segment::skew_offset (double x, double y) const
{
  if (m_start_skew == 0.0)
    return 0.0;
  if (cos (arc () / 2.0) == 0.0)
    return x - sqrt (m_radius * m_radius - y * y);

  double slope = skew_slope ();

  return solve_quadratic (slope * slope - 1.0,
                          -2.0 * (slope * std::abs (m_radius) + x),
                          m_radius * m_radius - x * x - y * y,
                          -m_start_skew * m_radius);
}

double Vamos_Track::
Road_Segment::skewed_radius (double x, double y, double offset) const
{
  x -= offset;
  return sqrt (x*x + y*y);
}

double Vamos_Track::
Road_Segment::coordinates (const Three_Vector& world_pos_in,
						   Three_Vector& track_pos) const
{
	Three_Vector world_pos = world_pos_in;
	//world_pos[2] = 0;
	//track_pos[2] = 0;
	//Three_Vector m_center_old = m_center;
	//m_center[2] = 0;
	
	//cout << world_pos_in.magnitude() << track_pos.magnitude() << endl;
	
  if (m_radius < 0.0 || m_radius > 0.0)
    {
      const double sign = Vamos_Geometry::sign (m_radius);
      const double r0 = sign * m_radius;
      const double half_angle = sign * arc () / 2.0;

      track_pos = (world_pos - m_center).
        rotate (0.0, 0.0, (sign * pi - arc ()) / 2.0 - m_start_angle);

      const double offset = -skew_offset (track_pos[0], track_pos[1]);
      const double r1 = skewed_radius (track_pos[0], track_pos[1], offset);
      const double theta = 
        sign * (atan2 (track_pos[1], track_pos[0] - offset)) + half_angle;

      track_pos[0] = r0 * theta;
      const double delta_x = offset + r1 * cos (half_angle)
        - r0 * cos (half_angle);
      const double delta_y = (r1 - r0) * sin (half_angle);
      track_pos[1] = sign * Three_Vector (delta_x, delta_y, 0.0).abs () 
        * cos (atan (m_start_skew));
      if (delta_x > 0.0)
        track_pos[1] *= -1.0;
      track_pos[2] = 0.0;
    }
  else
    {
      track_pos = (world_pos - m_center).rotate (0.0, 0.0, -m_start_angle);
	//if (m_start_angle == 6.21652) 
      double extra = track_pos[1] * (m_start_skew - m_end_skew);
      double slope = m_length / (m_length + extra);
      double offset = slope * m_start_skew * track_pos[1];
      track_pos[0] = slope * track_pos[0] + offset;
    }
	//cout << m_start_angle << endl;
  //track_pos.z = elevation (track_pos.x, track_pos.y);
	//cout << m_radius << ":" << track_pos[0] << "," << track_pos[1] << endl;
	//m_center = m_center_old;
	/*if (track_pos[0] < -10000 || track_pos[0] > 10000) 
	{
		cout << m_start_skew << "," << m_end_skew << "," << world_pos[0] << "," << world_pos[1] << endl;
		cout << m_radius << ":" << track_pos[0] << "," << track_pos[1] << "," << arc() << "," << length() << "," << m_start_angle << endl;
		cout << (world_pos - m_center).rotate (0.0, 0.0, -m_start_angle)[0] << "," << (world_pos - m_center).rotate (0.0, 0.0, -m_start_angle)[1] << endl;
		cout << m_center[0] << "," << m_center[1] << endl << endl;
	}*/
	
	if (!(track_pos[0] >= 0 || track_pos[0] < 0))
	{
		/*cout << m_start_skew << "," << m_end_skew << "," << world_pos[0] << "," << world_pos[1] << endl;
		cout << m_radius << ":" << track_pos[0] << "," << track_pos[1] << "," << arc() << "," << length() << "," << m_start_angle << endl;
		const double sign = Vamos_Geometry::sign (m_radius);
		cout << (world_pos - m_center).rotate (0.0, 0.0, (sign * pi - arc ()) / 2.0 - m_start_angle)[0] << "," << (world_pos - m_center).rotate (0.0, 0.0, (sign * pi - arc ()) / 2.0 - m_start_angle)[1] << endl;
		cout << m_center[0] << "," << m_center[1] << endl;
		cout << endl;*/
	}
  //track_pos [2] = elevationxy (world_pos[0], world_pos[1]);
	track_pos[2] = 0;
  if (track_pos[0] < 0.0)
    return track_pos[0];
  if (track_pos[0] > m_length)
    return track_pos[0] - m_length;

  //cout << m_radius << endl;
  
  //track_pos [0] = along;
  //track_pos [1] = from_center;
  //track_pos [2] = elevation (along, from_center);
  return 0.0;
}

Vamos_Geometry::Three_Vector Vamos_Track::
Road_Segment::position (double along, double from_center) const
{
  double x = m_center[0];
  double y = m_center[1];
  if (m_radius == 0.0)
    {
      x += along * cos (m_start_angle) - from_center * sin (m_start_angle);
      y += along * sin (m_start_angle) + from_center * cos (m_start_angle);
    }
  else
    {
      double angle = m_start_angle + along / m_radius;
      x += (m_radius - from_center) * sin (angle);
      y -= (m_radius - from_center) * cos (angle);
    }

  //return Three_Vector (x, y, elevation (along, from_center));
	return Three_Vector (x, y, elevationxy (x,y));
}
