//  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 "World.h"

#include <cassert>

using namespace Vamos_Geometry;

//-----------------------------------------------------------------------------
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.resize (sector + 1);
    }

  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)
    {
      assert (sector < int (ma_sector_times.size ()));
      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 ();

  if (m_sector != -1)
    {
      assert (m_sector < int (ma_sector_times.size ()));
      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 ();
  road_index = 0;
  segment_index = 0;
}

//-----------------------------------------------------------------------------
Vamos_World::
World::World (Vamos_Track::Strip_Track* track, Atmosphere* atmosphere) 
  : m_focused_car_index (0),
    mp_track (track),
    mp_atmosphere (atmosphere),
    m_gravity (-9.8)
{
}

Vamos_World::
World::~World ()
{
  delete mp_atmosphere;
  delete mp_track;
  for (std::vector <Car_Information>::iterator it = m_cars.begin ();
       it != m_cars.end ();
       it++)
    {
      delete it->car;
    }
}

inline Three_Vector
rotation_term (const Inertia_Tensor& I,
               const Three_Vector& r,
               const Three_Vector& n)
{
  return (I.inverse () * (r.cross (n))).cross (r);
}

Three_Vector
impulse (const Three_Vector& r1,
         const Three_Vector& v1,
         double m1,
         const Inertia_Tensor& I1,
         double restitution1,
         const Three_Vector& r2,
         const Three_Vector& v2,
         double m2,
         const Inertia_Tensor& I2,
         double restitution2,
         const Three_Vector& normal)
{
  return -normal.unit () * (1.0 + restitution1 * restitution2) * (v1 - v2).dot (normal)
    / (normal.dot (normal) * (1.0 / m1 + 1.0 / m2)
       + (rotation_term (I1, r1, normal) 
          + rotation_term (I2, r2, normal)).dot (normal));
}

Three_Vector
impulse (const Three_Vector& r,
         const Three_Vector& v,
         double m,
         const Inertia_Tensor& I,
         double restitution1,
         double restitution2,
         const Three_Vector& normal)
{
  Three_Vector j = -normal.unit () * (1.0 + restitution1 * restitution2) * v.dot (normal)
    / (normal.dot (normal) / m + rotation_term (I, r, normal).dot (normal));
#warning fix
  //!    if (j.magnitude () > 1.0)

  return -normal.unit () * (1.0 + restitution1 * restitution2) * v.dot (normal)
    / (normal.dot (normal) / m + rotation_term (I, r, normal).dot (normal));
}

void Vamos_World::
World::interact (Vamos_Body::Car* car, 
                 size_t road_index,
                 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 ().x;
      const Vamos_Geometry::Contact_Info info = 
        mp_track->test_for_contact (pos, 
                                    bump_parameter, 
                                    road_index,
                                    segment_index);

      if (info.contact)
        {
          const Three_Vector& velocity = car->chassis ().velocity (*it);

          car->chassis ().contact (*it, 
                                   impulse (car->chassis ().moment (pos),
                                            velocity,
                                            car->chassis ().mass (),
                                            car->chassis ().inertia (),
                                            (*it)->material ().
                                            restitution_factor (),
                                            info.material.
                                            restitution_factor (),
                                            info.normal),
                                   velocity,
                                   info.depth, 
                                   info.normal, 
                                   info.material);

          Three_Vector v_perp = velocity.project (info.normal);
          Three_Vector v_par = velocity - v_perp;
          m_interaction_info.
            push_back (Interaction_Info (car,
                                         (*it)->material ().type (), 
                                         info.material.type (),
                                         v_par.magnitude (), 
                                         v_perp.magnitude ()));
        }

      // Handle air resistance.
      car->chassis ().wind (*it, mp_atmosphere->velocity () 
                            - car->chassis ().velocity (*it),
                    mp_atmosphere->density ());
    }
}

void Vamos_World::
World::collide (Vamos_Body::Car* car1, Vamos_Body::Car* car2)
{
  // Handle collisions between the contact points of car 1 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++)
    {
      const Vamos_Geometry::Contact_Info info = 
        car2->collision (car1->chassis ().contact_position (*it),
                         car1->chassis ().
                         transform_velocity_out ((*it)->velocity ()));

      if (info.contact)
        {
          Three_Vector v1 = 
            car1->chassis ().transform_velocity_out ((*it)->velocity ());

          Three_Vector system_velocity = 
            (v1 + car2->chassis ().cm_velocity ()) / 2.0;
          //!! not quite

          if (info.normal.dot (v1) < 0.0)
            {
#warning fix
//               car1->chassis ().contact (*it, info.depth / 2.0, 
//                                         info.normal, info.material,
//                                         system_velocity);
              
              car2->chassis ().
                temporary_contact (**it,
                                   car1->chassis ().
                                   transform_out ((*it)->position ()),
                                   v1 - system_velocity,
                                   info.depth / 2.0,
                                   -info.normal,
                                   info.material);

              const Three_Vector& velocity = car1->chassis ().velocity (*it);
              Three_Vector v_perp = velocity.project (info.normal);
              Three_Vector v_par = velocity - v_perp;
              m_interaction_info.
                push_back (Interaction_Info (car1,
                                             info.material.type (),
                                             info.material.type (),
                                             v_par.magnitude (), 
                                             v_perp.magnitude ()));
            }
        }
    }
}

// Place the car back on the track at its current position.
void Vamos_World::
World::reset ()
{
  size_t& segment_index = focused_car ()->segment_index;
  size_t& road_index = focused_car ()->road_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, road_index, segment_index);
  const Three_Matrix& orientation = 
    mp_track->reset_orientation (car_pos, road_index, segment_index);

  double z = car_reset_elevation (focused_car ()->car, segment_index, road_index);

  // Set the car's position and orientation.
  focused_car ()->car->reset (position + Three_Vector (0.0, 0.0, z), 
                              orientation);
}

double Vamos_World::
World::car_reset_elevation (Vamos_Body::Car* car, 
                            size_t& segment_index, 
                            size_t& road_index)
{
  std::vector <Vamos_Body::Particle*>::iterator 
    it = car->chassis ().particles ().begin (); 

  const Three_Vector& pos = car->chassis ().contact_position (*it);
  double dist = car->distance_traveled () + (*it)->position ().x;
  double elevation = mp_track->elevation (pos, dist, road_index, segment_index);
  double max_diff = elevation - pos.z;

  for (size_t i = 0; it != car->chassis ().particles ().end (); it++, i++)
    {
      const Three_Vector& pos = car->chassis ().contact_position (*it);
      dist = car->distance_traveled () + (*it)->position ().x;
      elevation = 
        mp_track->elevation (pos, dist, road_index, segment_index);

      max_diff = std::max (max_diff, elevation - pos.z);
    }
  return max_diff;
}

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

  size_t road_index = 0;
  size_t segment_index = 0;
  double z = car_reset_elevation (focused_car ()->car, segment_index, road_index);

  focused_car ()->car->
    reset (Three_Vector (0.0, 0.0, z), 
           mp_track->
           reset_orientation (Three_Vector (), road_index, segment_index));
}

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