//  Vamos Automotive Simulator
//  Copyright (C) 2001--2004 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/world/World.h>

#include <cassert>

using Vamos_Geometry::Three_Vector;
using Vamos_Geometry::Three_Matrix;

//-----------------------------------------------------------------------------
Vamos_World::
Times::Times () :
  m_current (0.0),
  m_previous (0.0),
  m_best (0.0),
  m_difference (0.0)
{
}

void Vamos_World::
Times::update (double time_step)
{
  m_current += time_step;
}

void Vamos_World::
Times::finalize ()
{
  if (m_best != 0.0)
    {
      m_difference = m_current - m_best;
    }

  if ((m_current < m_best) || (m_best == 0.0))
    {
      m_best = m_current;
    }

  m_previous = m_current;
  m_current = 0.0;
}

void Vamos_World::
Times::reset ()
{
  m_current = 0.0;
}

//-----------------------------------------------------------------------------
Vamos_World::
Timing_Info::Timing_Info () :
  m_sector (-1),
  m_previous_sector (-1),
  m_distance (0.0)
{
}

void Vamos_World::
Timing_Info::update_sector_info (int sector)
{
  if (sector == int (ma_sector_times.size ()))
    { // We're entering a sector for the first time.
      ma_sector_times.push_back (Times ());
    }

  if ((sector != m_sector) && (sector != -1))
    { // We have left a sector.
      if (m_sector != -1)
        {
          ma_sector_times [m_sector].finalize ();
          
          if (sector == 0)
            { // We have finished a lap.
              m_lap_times.finalize ();
            }
        }
      m_previous_sector = m_sector;
      m_sector = sector;
    }
}

void Vamos_World::
Timing_Info::update_times (int sector, double time_step)
{
  m_lap_times.update (time_step);
  if (sector != -1)
    {
      ma_sector_times [sector].update (time_step);
    }
}

void Vamos_World::
Timing_Info::update (double time_step, double distance, int sector)
{
  update_sector_info (sector);
  update_times (sector, time_step);
  m_distance = distance;
}

void Vamos_World::
Timing_Info::reset ()
{
  m_lap_times.reset ();
  ma_sector_times [m_sector].reset ();

  m_sector = -1;
  m_previous_sector = -1;
  m_distance = 0.0;
}

//-----------------------------------------------------------------------------
void Vamos_World::
Car_Information::reset ()
{
  timing.reset ();
  segment_index = 0;
  car->reset ();
}

//-----------------------------------------------------------------------------
Vamos_World::
World::World (TRACK * track) 
  : m_focused_car_index (0),
    p_track (track),
    m_gravity (-9.8)
{
}

extern bool verbose_output;

Vamos_World::
World::~World ()
{
	if (verbose_output)
		std::cout << "world deinit" << std::endl;
  for (std::vector <Car_Information>::iterator it = m_cars.begin ();
       it != m_cars.end ();
       it++)
    {
      delete it->car;
    }
	//std::cout << (void*)mp_track << std::endl;
	//delete mp_track;
	
	if (verbose_output)
		std::cout << "world deinit done" << std::endl;
}

extern float FrameTime();

void Vamos_World::
World::interact (Vamos_Body::Car* car, size_t segment_index)
{
	bool verbose = false;
	
	Vamos_Geometry::Material * tmatroad = new Vamos_Geometry::Material(
		Vamos_Geometry::Material::ASPHALT, 1.0, 1.0,
		1.0, 0.0, 
		0.0, 0.0, 
		0);
	Vamos_Geometry::Material * tmatwall = new Vamos_Geometry::Material(
		Vamos_Geometry::Material::CONCRETE, 1.0, -0.8,
		1.0, 0.0, 
		0.0, 0.0, 
		0);
	Vamos_Geometry::Material_Handle material = tmatroad;
	Vamos_Geometry::Material_Handle materialwall = tmatwall;
	
	size_t i;
	
	//do additional collisions for walls and stuff
	int loopcount = 0;
	bool col = true;
	const int maxloops = 4;
	while (col && loopcount < maxloops)
	{
		loopcount++;
		col = false;
		//find the orientation of the car
		QUATERNION rot;		
		double angle;
		Three_Vector aa = car->chassis().axis_angle(&angle);
		//rot.SetAxisAngle(angle*(3.141593/180.0),aa[0],aa[1],aa[2]);
		//rot.SetAxisAngle(angle*(3.141593/180), -aa[0], -aa[2], aa[1]);
		//rot.Rotate(3.141593/2.0,0,1,0);
		rot.SetAxisAngle(angle*(3.141593/180), aa[0], aa[2], -aa[1]);
		
		//find the position of the car
		Three_Vector chassispos = car->chassis().position();
		VERTEX cp;
		cp.Set(chassispos[0],chassispos[2],-chassispos[1]);
		
		VERTEX cmv;
		Vamos_Geometry::Three_Vector cm = car->chassis().center_of_mass();
		cmv.Set(cm[0], cm[2], -cm[1]);
		
		//get the collision mesh
		JOEMODEL * mod = car->GetCollisionModel();
		if (mod != NULL)
		{	
			unsigned int nfaces = mod->GetFaces();
			
			//for the AABB calculation
			float maxv[3];
			float minv[3];
			bool havevals[6];
			int n = 0;
			for (n = 0; n < 6; n++)
				havevals[n] = false;
			
			unsigned int f;
			unsigned int v;
			VERTEX modelfaces[nfaces*3];
			for (f = 0; f < nfaces; f++)
			{
				for (v = 0; v < 3; v++)
				{
					//transform the mesh faces to the world frame
					modelfaces[f*3+v].Set(mod->GetVert(mod->GetFace(f)[v]));
					//modelfaces[f*3+v].z = -modelfaces[f*3+v].z;
					modelfaces[f*3+v] = cp + rot.RotateVec(cmv.ScaleR(-1.0)) + 
										rot.RotateVec(modelfaces[f*3+v]);
					
					//z mirror due to weirdness in the object code
					modelfaces[f*3+v].z = -modelfaces[f*3+v].z;
					
					//cache for bbox stuff
					for (n = 0; n < 3; n++)
					{
						if (modelfaces[f*3+v].v3()[n] > maxv[n] || !havevals[n])
						{
							maxv[n] = modelfaces[f*3+v].v3()[n];
							havevals[n] = true;
						}
						if (modelfaces[f*3+v].v3()[n] < minv[n] || !havevals[n+3])
						{
							minv[n] = modelfaces[f*3+v].v3()[n];
							havevals[n+3] = true;
						}
					}
				}
			}
			
			//assign the world space AABB
			AABB modelbbox;
			VERTEX minvals, maxvals;
			minvals.Set(minv);
			maxvals.Set(maxv);
			modelbbox.SetFromCorners(minvals, maxvals);
			
			VERTEX normal, colpt;
			float depth;
			
			col = p_track->CollideModel(modelfaces, nfaces, modelbbox, colpt, false, normal, depth);
			
			if (verbose && col)
			{
				float miny = 10000;
				for (f = 0; f < nfaces; f++)
				{
					for (v = 0; v < 3; v++)
					{
						if (modelfaces[f*3+v].y < miny)
							miny = modelfaces[f*3+v].y;
					}
				}
				
				cout << "Loop " << loopcount << endl;
				colpt.DebugPrint();
				cout << miny << endl;
				normal.DebugPrint();
				cout << "depth: " << depth << endl << endl;
			}
			
			if (col && !colpt.nan() && !normal.nan() && (depth >= 0 || depth < 0))
			{
				Three_Vector colpos;
				Three_Vector norm;
				
				//normal.y = 0;
				//normal = normal.normalize();
				
				norm.m_vec[0] = normal.x;
				norm.m_vec[1] = -normal.z;
				norm.m_vec[2] = normal.y;
				
				colpos.m_vec[0] = colpt.x;
				colpos.m_vec[1] = -colpt.z;
				colpos.m_vec[2] = colpt.y;
				
				car->chassis().single_point_contact(colpos, depth+0.001, norm, materialwall, FrameTime());
			}
		}
	}
	
	//keep the wheels on the track
	i = 0;
	int wheelcount = 0;
	for (i = 0; i < 4; i++)
		car->SetColPatch(i, NULL);
	for (std::vector <Vamos_Body::Particle*>::iterator 
		 it = car->chassis ().particles ().begin ();
	   it != car->chassis ().particles ().end ();
	   it++, i++)
    {
    	if (!(*it)->single_contact ())
		{
			const Three_Vector& pos = car->chassis ().contact_position (*it);
			//const Three_Vector& lastpos = car->chassis ().last_contact_position (*it);
			//double bump_parameter = car->distance_traveled () + (*it)->position () [0];
			
			VERTEX particlepos;
			particlepos.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
			
			bool con = false;
			Three_Vector norm;
			double depth = 0;
			VERTEX normal;
			VERTEX origin = particlepos;
			origin.y += 0.5;
			BEZIER * tcolpatch = NULL;
			OBJECTNODE * tcolnode = NULL;
			depth = p_track->ElevationSeg(origin, normal, 10.0, tcolpatch, tcolnode) - particlepos.y;
			
			car->SetColPatch(wheelcount, tcolpatch);
			
			//calculate friction coefficients from collision data and track data
			float f1 = 1.0;
			float f2 = 1.0;
			float rr = 1.0;
			float rd = 0.0;
			if (tcolpatch != NULL)
			{
				p_track->GetFrictionParams(f1, f2);
			}
			else if (tcolnode != NULL)
			{
				f1 = tcolnode->friction1;
				f2 = tcolnode->friction2;
				//cout << "Setting friction wheel " << wheelcount << " to " << f1 << "," << f2 << endl;
			
				//add in bump effects				
				float posx = origin.x;
				float posz = origin.z;
				float phase = 2*3.141593*(posx+posz) / tcolnode->bumplength;
				float shift = 2.0*sin(phase*1.414214);
				float amplitude = 0.25*tcolnode->bumpmag;
				float bumpoffset = amplitude * (sin (phase + shift) + sin (1.414214*phase) - 2.0);
				depth += bumpoffset;
				
				rr = tcolnode->rolling_resistance_factor;
				rd = tcolnode->rolling_drag;
			}
			car->SetColParams(wheelcount, f1, f2, rr, rd);
			
			wheelcount++;

			//depth = p_track->Elevation(particlepos, normal) - particlepos.y;
			
			if (depth < 0 || depth >= 0)
			{
				//depth = -particlepos.y;
				//normal.Set(0,1,0);
				if (depth > 0)
					con = true;
				norm.m_vec[0] = normal.x;
				norm.m_vec[1] = -normal.z;
				norm.m_vec[2] = normal.y;
				if (con)
				{
					if (depth > 1.0)
					{
						cout << "Large suspension displacement detected: " << depth << endl;
					}
					
					//cout << p_track->Elevation(particlepos, normal) << endl;
					//cout << depth << endl;
					//particlepos.DebugPrint();	cout << endl;
					//Vamos_Geometry::Material_Handle mat = &info.material;
					car->chassis ().contact (*it, depth, 
										   norm, material);
					//car->chassis().propagate_contact();
				}
			}
			else
				cout << "Not contacting with NaN surface" << endl;
		}
	}
	
	/*int loopcount = 0;
	int numcols = 1;
	const int maxloops = 50;
	while (numcols > 0 && loopcount < maxloops)
	{
		loopcount++;
		numcols = 0;
		i = 0;
		for (std::vector <Vamos_Body::Particle*>::iterator 
			 it = car->chassis ().particles ().begin ();
		   it != car->chassis ().particles ().end ();
		   it++, i++)
		{	
			if ((*it)->single_contact ())
			{
				bool con = false;
				
				const Three_Vector& pos = car->chassis ().contact_position (*it);
				const Three_Vector& lastpos = car->chassis ().last_contact_position (*it);
				
				Three_Vector norm;
				double depth = 0;
				
				VERTEXD lastparticlepos;
				lastparticlepos.Set(lastpos.m_vec[0],lastpos.m_vec[2],-lastpos.m_vec[1]);
				
				VERTEXD particleposd;
				particleposd.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
				
				VERTEXD colpt;
				double coldist = 0;
				VERTEXD coldir;
				VERTEXD normald;
				coldir.Set(pos.m_vec[0]-lastpos.m_vec[0],pos.m_vec[2]-lastpos.m_vec[2],-(pos.m_vec[1]-lastpos.m_vec[1]));
				double seglen = coldir.len();
				//const float fluff = 0.5;
				const double fluff = 0.00001;
				if (seglen < fluff)
					seglen = fluff;
				//lastparticlepos = lastparticlepos - coldir.normalize().ScaleR(fluff/2.0);
				con = p_track->CollideD(lastparticlepos, coldir, seglen, colpt, true, normald, coldist);
				//con = p_track->Collide(lastparticlepos, particlepos, colpt, true, normal, coldist);
				//if (particleposd.y < 0)
				if (con)
				//while (con && numcols < 5)
				{
					//car->chassis ().roll_back();
					
					//normal.Scale(100.0);
					
					norm.m_vec[0] = normald.x;
					norm.m_vec[1] = -normald.z;
					norm.m_vec[2] = normald.y;
					
					depth = (seglen - coldist)*coldir.normalize().dot(normald.ScaleR(-1.0));
					//depth *= 2.0;
					//depth = seglen - coldist;
					//depth = - ((particleposd - colpt).dot(normald));
					depth += fluff;
					
					//depth = -particleposd.y;
					//normald.Set(0,1,0);
					
					if (verbose)
					{
						cout << "Loop " << loopcount << endl;
						cout << "Collision " << numcols << endl;
						cout << "Particle " << i << endl;
						particleposd.DebugPrint();
						lastparticlepos.DebugPrint();
						colpt.DebugPrint();
						coldir.DebugPrint();
						coldir.normalize().DebugPrint();
						normald.DebugPrint();
						cout << coldir.len() << endl;
						cout << seglen << endl;
						cout << (colpt - lastparticlepos).len() << endl;
						cout << coldist << endl;
						cout << "depth: " << depth << endl << endl;
					}
					
					if (depth >= 0)
					{
						//roll back chassis state
						//car->chassis ().roll_back();
						
						//depth = 1.0;
						
						numcols++;
						
						car->chassis ().contact (*it, depth, norm, materialwall);
						car->chassis().propagate_contact();
						//Three_Vector colpos;
						//colpos.m_vec[0] = colpt.x;
						//colpos.m_vec[1] = -colpt.z;
						//colpos.m_vec[2] = colpt.y;
						//car->chassis().single_point_contact(colpos, depth, norm, materialwall, FrameTime());
						
						//car->chassis().kill_vel();

					}
					else 
					{
						con = false;
						if (verbose)
							cout << "weird collision depth" << endl << endl;
					}
				}
			}
		}
	}
	
	if (loopcount == maxloops)
	{
		car->chassis().roll_back();
		car->chassis().kill_vel();
	}*/

	// Handle air resistance.
	i = 0;
	for (std::vector <Vamos_Body::Particle*>::iterator 
		 it = car->chassis ().particles ().begin ();
	   it != car->chassis ().particles ().end ();
	   it++, i++)
	{
		car->chassis ().wind (*it, Three_Vector(0,0,0)
							- car->chassis ().velocity (*it),
					1.2);
	}
	
	/*for (std::vector <Vamos_Body::Particle*>::iterator 
		 it = car->chassis ().particles ().begin ();
	   it != car->chassis ().particles ().end ();
	   it++, i++)
    {
    	const Three_Vector& pos = car->chassis ().contact_position (*it);
		const Three_Vector& lastpos = car->chassis ().last_contact_position (*it);
      	//double bump_parameter = car->distance_traveled () + (*it)->position () [0];
		
      	//Vamos_Track::Track_Contact_Info info = mp_track->test_for_contact (pos, bump_parameter, segment_index);
		
		VERTEX particlepos;
		particlepos.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
		
		//keep the tires on the track
		bool con = false;
		Three_Vector norm;
		Vamos_Geometry::Material * tmat = new Vamos_Geometry::Material;
		Vamos_Geometry::Material_Handle material = tmat;
		double depth = 0;
		
		//do additional collisions for walls and stuff
		VERTEXD lastparticlepos;
		lastparticlepos.Set(lastpos.m_vec[0],lastpos.m_vec[2],-lastpos.m_vec[1]);
		
		VERTEXD particleposd;
		particleposd.Set(pos.m_vec[0],pos.m_vec[2],-pos.m_vec[1]);
		
		VERTEXD colpt;
		double coldist;
		VERTEXD coldir;
		VERTEXD normald;
		coldir.Set(pos.m_vec[0]-lastpos.m_vec[0],pos.m_vec[2]-lastpos.m_vec[2],-(pos.m_vec[1]-lastpos.m_vec[1]));
		double seglen = coldir.len();
		//const float fluff = 0.5;
		const double fluff = 0.00001;
		if (seglen < fluff)
			seglen = fluff;
		//lastparticlepos = lastparticlepos - coldir.normalize().ScaleR(fluff/2.0);
		con = p_track->CollideD(lastparticlepos, coldir, seglen, colpt, false, normald, coldist);
		//OLD con = p_track->Collide(lastparticlepos, particlepos, colpt, true, normal, coldist);
		int numcols = 0;
		//while (con && numcols < 5)
		if (con)
		{
			//car->chassis ().roll_back();
			
			//normal.Scale(100.0);
			
			norm.m_vec[0] = normald.x;
			norm.m_vec[1] = -normald.z;
			norm.m_vec[2] = normald.y;
			
			depth = (seglen - coldist)*coldir.normalize().dot(normald.ScaleR(-1.0));
			//depth *= 10.0;
			//depth = seglen - coldist;
			//depth = - ((particleposd - colpt).dot(normald));
			depth += fluff;
			
			if (verbose)
			//if (0)
			{
				cout << "Second pass:  Collision " << count << " particle " << i << endl;
				if (numcols != 0)
					cout << "Sub-collision " << numcols << endl;
				particleposd.DebugPrint();
				lastparticlepos.DebugPrint();
				colpt.DebugPrint();
				coldir.DebugPrint();
				coldir.normalize().DebugPrint();
				normald.DebugPrint();
				cout << coldir.len() << endl;
				cout << seglen << endl;
				cout << (colpt - lastparticlepos).len() << endl;
				cout << coldist << endl;
				cout << "depth: " << depth << endl << endl;
			}
			
			count++;
			
			if (depth >= 0)
			{
				//roll back chassis state
				//car->chassis ().roll_back();
				
				//depth = 1.0;
				
				numcols++;
				
				car->chassis ().contact (*it, depth, 
								   norm, materialwall);
				
				//car->chassis().propagate_contact();
			}
			//else if (verbose)	cout << "weird collision depth" << endl << endl;
		}
	}*/
}

void Vamos_World::
World::collide (Vamos_Body::Car* car1, Vamos_Body::Car* car2)
{
  // Handle collisions between the contact points of car1 and the
  // crash box of car 2. 
  for (std::vector <Vamos_Body::Particle*>::iterator 
         it = car1->chassis ().particles ().begin ();
       it != car1->chassis ().particles ().end ();
       it++)
    {
      if (car2->collision (car1->chassis ().transform_out ((*it)->position ())))
        {
          std::cout << "boom" << std::endl;
        }
    }
}
void Vamos_World::
World::reset ()
{
	reset(false);
}

// Place the car back on the track at its current position.
void Vamos_World::
World::reset (bool all)
{
	//if (all)
	{
		for (std::vector <Vamos_World::Car_Information>::iterator carit = m_cars.begin ();
			   carit != m_cars.end ();
			   carit++)
			reset(&(*carit));
	}
	//else reset(focused_car());
	
	/*if (all)
	{
		for (std::vector <Vamos_World::Car_Information>::iterator carit = m_cars.begin ();
		   carit != m_cars.end ();
		   carit++)
		{
//			size_t& segment_index = carit->segment_index;
			
			  // Find the position and orientation specified by the track.
//			  const Three_Vector& car_pos = carit->car->chassis ().position ();
			Three_Matrix orientation(1.0);
			orientation.identity();
			VERTEX vpos = p_track->GetStart();
			Three_Vector position(vpos.x, -vpos.z, vpos.y);
			//const Three_Vector& position(p_track->GetStart().v3());
			
			  // Adjust the vertical position so that at least one point on the
			  // car is in contact with the road, and the rest are above it.
			
			  std::vector <Vamos_Body::Particle*>::iterator 
				it = carit->car->chassis ().particles ().begin (); 
			
			  const Three_Vector& pos = 
				carit->car->chassis ().contact_position (*it);
			  double dist = carit->car->distance_traveled () 
				+ (*it)->position () [0];
			  //double elevation = mp_track->elevation (pos, dist, segment_index);
			  VERTEX cp;
			  cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
			  double elevation = p_track->Elevation (cp);
			  double max_diff = elevation - pos [2];
			
			  for (size_t i = 0; 
				   it != carit->car->chassis ().particles ().end (); 
				   it++, i++)
				{
				  const Three_Vector& pos = 
					carit->car->chassis ().contact_position (*it);
				  dist = carit->car->distance_traveled () 
					+ (*it)->position () [0];
					
			  	cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
			  	elevation = p_track->Elevation (cp);
					
			
				  double diff = elevation - pos [2];
				  if (diff > max_diff)
					{
					  max_diff = diff;
					}
				}
			
			  // Set the car's position and orientation.
			  carit->car->reset (position + Three_Vector (0.0, 0.0, max_diff), orientation);
		}
	}
	else
	{
//		size_t& segment_index = focused_car ()->segment_index;
		
		// Find the position and orientation specified by the track.
//		const Three_Vector& car_pos = focused_car ()->car->chassis ().position ();

		Three_Matrix orientation(1.0);
		orientation.identity();
		VERTEX vpos = p_track->GetStart();
		Three_Vector position(vpos.x, -vpos.z, vpos.y);
		
		// Adjust the vertical position so that at least one point on the
		// car is in contact with the road, and the rest are above it.
		
		std::vector <Vamos_Body::Particle*>::iterator 
		it = focused_car ()->car->chassis ().particles ().begin (); 
		
		const Three_Vector& pos = 
		focused_car ()->car->chassis ().contact_position (*it);
		double dist = focused_car ()->car->distance_traveled () 
		+ (*it)->position () [0];
		//double elevation = mp_track->elevation (pos, dist, segment_index);
		
		VERTEX cp;
		cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
		double elevation = p_track->Elevation (cp);
		
		double max_diff = elevation - pos [2];
		
		for (size_t i = 0; 
		   it != focused_car ()->car->chassis ().particles ().end (); 
		   it++, i++)
		{
		  const Three_Vector& pos = 
			focused_car ()->car->chassis ().contact_position (*it);
		  dist = focused_car ()->car->distance_traveled () 
			+ (*it)->position () [0];
			
			cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
			elevation = p_track->Elevation (cp);
		
		  double diff = elevation - pos [2];
		  if (diff > max_diff)
			{
			  max_diff = diff;
			}
		}
		
		// Set the car's position and orientation.
		focused_car ()->car->reset (position + Three_Vector (0.0, 0.0, max_diff), orientation);
	}*/
}

void Vamos_World:: 
World::reset (Car_Information * cartoreset)
{
//	size_t& segment_index = cartoreset->segment_index;
		
	// Find the position and orientation specified by the track.
//	const Three_Vector& car_pos = cartoreset->car->chassis ().position ();
	/*const Three_Vector& position = 
	mp_track->reset_position (car_pos, segment_index);
	const Three_Matrix& orientation = 
	mp_track->reset_orientation (car_pos, segment_index);*/
	
	VERTEX vpos = p_track->GetStart();
	Three_Vector position(vpos.x, -vpos.z, vpos.y);
	
	QUATERNION qstart;
	/*qstart = p_track->GetStartOrientation();
	//qstart = qstart.ReturnConjugate();
	//qstart.Rotate(3.141593/2.0, 0,0,1);
	//qstart.Rotate(3.141593/2.0, 0,1,0);
	//qstart.Rotate(-3.141593/2.0, 1,0,0);
	//qstart.Rotate(3.141593/2.0, 0,1,0);
	qstart.Rotate(-3.141593/2.0, 1,0,0);
	QUATERNION goofyfoot;
	goofyfoot.Rotate(3.141593/2.0, 1,0,0);
	qstart.Multiply(goofyfoot);*/
	
	double o3x3[3][3];
	float orient[16];
	
	qstart = p_track->GetStartOrientation();
	QUATERNION qtemp = qstart;
	qstart.x = qtemp.x;
	qstart.y = qtemp.z;
	qstart.z = -qtemp.y;
	
	qstart.Rotate(-3.141593/2.0, 0,0,1);
	
	qstart.GetMat(orient);
	bool flip = false;
	for (int i = 0; i < 3; i++)
	{
		if (flip)
		o3x3[i][0] = orient[i+0];
		else
		o3x3[0][i] = orient[i+0];
	}
	for (int i = 0; i < 3; i++)
	{
		if (flip)
		o3x3[i][1] = orient[i+4];
		else
		o3x3[1][i] = orient[i+4];
	}
	for (int i = 0; i < 3; i++)
	{
		if (flip)
		o3x3[i][2] = orient[i+8];
		else
		o3x3[2][i] = orient[i+8];
	}
	//Three_Matrix orientation(1.0);
	//orientation.identity();
	Three_Matrix orientation(o3x3);
	
	cartoreset->car->reset (position, orientation);
	
	// Adjust the vertical position so that at least one point on the
	// car is in contact with the road, and the rest are above it.
	
	
	
	//Three_Vector mpos = 
		//cartoreset->car->chassis ().contact_position (*it);
	//double mdist = cartoreset->car->distance_traveled () 
		//+ (*it)->position () [0];
	//double elevation = mp_track->elevation (pos, dist, segment_index);
	
	VERTEX cp;
	//cp.Set(mpos.m_vec[0], mpos.m_vec[2], -mpos.m_vec[1]);
	double elevation;
	//elevation = p_track->Elevation (cp);
		
	//double max_diff = elevation - mpos [2];
	bool have_max = false;
	double max_diff = 0.0;
	
	//const Three_Vector& car_pos = cartoreset->car->chassis ().position ();
	
	for (std::vector <Vamos_Body::Particle*>::iterator it = 
		cartoreset->car->chassis ().particles ().begin (); 
	   it != cartoreset->car->chassis ().particles ().end (); 
	   it++)
	{
	  Three_Vector pos = 
		cartoreset->car->chassis ().contact_position (*it);
	  //dist = cartoreset->car->distance_traveled () 
		//+ (*it)->position () [0];
	  //elevation = mp_track->elevation (pos, dist, segment_index);
		
		cp.Set(pos.m_vec[0], pos.m_vec[2], -pos.m_vec[1]);
		elevation = p_track->Elevation (cp);
	
	  	double diff = elevation - pos [2];
		//double diff = pos[2];
		if (diff > max_diff || !have_max)
		{
			max_diff = diff;
			have_max = true;
		}
		
		//cout << diff << endl;
	}
	
	// Set the car's position and orientation.
	cartoreset->car->reset (position + Three_Vector (0.0, 0.0, max_diff+0.0), orientation);
	//cartoreset->car->reset (position, orientation);
	//cout << max_diff << endl;
}

// Place the car back on the track at the starting line.
void Vamos_World::
World::restart ()
{
  focused_car ()->reset ();
}

// Set the acceleration due to gravity.  Always downward, regardless
// of sign.
void Vamos_World::
World::gravity (double g)
{
  m_gravity = -std::abs (g);
  if (focused_car () != 0)
    {
      focused_car ()->car->chassis ().
        gravity (Three_Vector (0.0, 0.0, m_gravity));
    }
}

void Vamos_World::
World::add_car (Vamos_Body::Car* car)
{
  car->chassis ().gravity (Three_Vector (0.0, 0.0, m_gravity));
  m_cars.push_back (Car_Information (car));
}

void Vamos_World::
World::set_focused_car (size_t index)
{
  assert (index < m_cars.size ());
  m_focused_car_index = index;
}

void Vamos_World::
World::focus_other_car (int delta)
{
  set_focused_car ((m_focused_car_index + delta) % m_cars.size ());
}

Vamos_World::Car_Information* Vamos_World::
World::focused_car ()
{
  if (m_focused_car_index >= m_cars.size ()) return 0;
  return &m_cars [m_focused_car_index];
}
