//  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 (Vamos_Track::Strip_Track* track) 
  : m_focused_car_index (0),
    mp_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;
}

void Vamos_World::
World::interact (Vamos_Body::Car* car, size_t segment_index)
{
  size_t i = 0;
  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);
      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);

      if (info.m_contact)
        {
			//Vamos_Geometry::Material_Handle mat = &info.material;
          car->chassis ().contact (*it, info.m_depth, 
                                   info.m_normal, info.m_material);

          const Three_Vector& velocity = car->chassis ().velocity (*it);
          Three_Vector v_perp = velocity.project (info.m_normal);
          Three_Vector v_par = velocity - v_perp;
          m_contact_info.push_back (Contact_Info (car,
                                                  (*it)->material ()->type (), 
                                                  info.m_material->type (),
                                                  v_par.abs (), 
                                                  v_perp.abs ()));
        }

      // Handle air resistance.
      car->chassis ().wind (*it, Three_Vector(0,0,0)
                            - car->chassis ().velocity (*it),
                    1.2);
    }
}

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++)
		{
			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 ();
			  const Three_Vector& position = 
				mp_track->reset_position (car_pos, segment_index);
			  const Three_Matrix& orientation = 
				mp_track->reset_orientation (car_pos, segment_index);
			
			  // 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);
			  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];
				  elevation = 
					mp_track->elevation (pos, dist, segment_index);
			
				  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 ();
		const Three_Vector& position = 
		mp_track->reset_position (car_pos, segment_index);
		const Three_Matrix& orientation = 
		mp_track->reset_orientation (car_pos, segment_index);
		
		// 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);
		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];
		  elevation = 
			mp_track->elevation (pos, dist, segment_index);
		
		  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);
	
	// 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 = cartoreset->car->chassis ().particles ().begin (); 
	
	const Three_Vector& pos = 
	cartoreset->car->chassis ().contact_position (*it);
	double dist = cartoreset->car->distance_traveled () 
	+ (*it)->position () [0];
	double elevation = mp_track->elevation (pos, dist, segment_index);
	double max_diff = elevation - pos [2];
	
	for (size_t i = 0; 
	   it != cartoreset->car->chassis ().particles ().end (); 
	   it++, i++)
	{
	  const 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);
	
	  double diff = elevation - pos [2];
	  if (diff > max_diff)
		{
		  max_diff = diff;
		}
	}
	
	// Set the car's position and orientation.
	cartoreset->car->reset (position + Three_Vector (0.0, 0.0, max_diff+0.5), orientation);
}

// 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];
}
