//  Rigid_Body.cc - a rigid body.
//
//  Copyright (C) 2001--2004 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  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/body/Rigid_Body.h>
#include <vamos/body/Aerodynamic_Device.h>
#include <vamos/body/Contact_Point.h>

#include <cassert>

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

//* Struct Contact_Parameters

//** Constructor

Vamos_Body::
Contact_Parameters::Contact_Parameters () 
  : m_distance (0.0)
{
}

//* Class Body

//** Constructors

// Specify the position and orientation of the body.
Vamos_Body::
Rigid_Body::Rigid_Body (const Three_Vector& pos, const Three_Matrix& orient) 
  : Frame (pos, orient),
    m_initial_position (pos),
    m_delta_time (0.0),
    m_mass (0.0)
{
	valid = false;
}

// Specify the position, the orientation is the same as the parent.
Vamos_Body::
Rigid_Body::Rigid_Body (const Three_Vector& pos) 
  : Frame (pos),
    m_initial_position (pos),
    m_delta_time (0.0),
    m_mass (0.0)
{
	valid = false;
}

// Specify the position, the orientation is the same as the parent.
Vamos_Body::
Rigid_Body::Rigid_Body () 
  : Frame (),
    m_delta_time (0.0),
    m_mass (0.0)
{
	valid = false;
}


//** Destuctor
Vamos_Body::Rigid_Body::
~Rigid_Body ()
{
  // The body is responsible for deleting the partiles even though
  // they were constructed elsewhere
  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      	//delete *it;
    }
}

// Return the position of the center of mass of the body with respect
// to the world.
Three_Vector Vamos_Body::
Rigid_Body::cm_position ()
{
  return transform_out (m_body_cm, m_body_cm);
}

// Return the contact position of the INDEXth particle of
// `m_particles' with respect to the world.
Three_Vector Vamos_Body::
Rigid_Body::contact_position (Particle* contact_point)
{
  return transform_out (contact_point->contact_position (), m_body_cm);
}

Three_Vector Vamos_Body::
Rigid_Body::last_contact_position (Particle* contact_point)
{
	Three_Vector temp_position;
	Three_Matrix temp_orientation;
	
	temp_position = m_position;
	temp_orientation = m_orientation;
	
	m_position = m_last_position;
	m_orientation = m_last_orientation;
	
  Three_Vector out;
	
	out = transform_out (contact_point->contact_position (), m_last_body_cm);
	
	m_position = temp_position;
	m_orientation = temp_orientation;
	
	return out;
}

//set current state to last state
void Vamos_Body::
Rigid_Body::roll_back()
{
	m_position = m_last_position;
	m_orientation = m_last_orientation;
	m_body_cm = m_last_body_cm;
	m_cm_velocity = m_last_cm_velocity;
	m_ang_velocity = m_last_ang_velocity;
}

void Vamos_Body::
Rigid_Body::kill_vel()
{
	m_velocity = m_velocity * 0; 
	m_ang_velocity = m_ang_velocity * 0;
	
	std::vector <Particle*>::const_iterator it = m_particles.begin ();

	for (it++; it != m_particles.end (); it++)
    {
		(*it)->set_ang_velocity(m_ang_velocity);
		(*it)->set_velocity(m_velocity);
	}
}

// Return the smallest contact position z-value of the particles.
double Vamos_Body::
Rigid_Body::lowest_contact_position () const
{
  std::vector <Particle*>::const_iterator it = m_particles.begin ();
  double pos = transform_out ((*it)->contact_position ()) [2];
  double lowest = pos;

  for (it++; it != m_particles.end (); it++)
    {
      pos = transform_out ((*it)->contact_position ()) [2];
      if (pos < lowest)
        {
          lowest = pos;
        }
    }

  return lowest;
}

// Calculate the center of mass, the ineritia tensor, and its inverse.
void Vamos_Body::
Rigid_Body::update_center_of_mass ()
{
	if (m_contact_parameters.m_distance > 0.0)
    {
	}
	else
		m_last_body_cm = m_body_cm;
	
  // Find the center of mass in the body frame.
  m_body_cm = Three_Vector (0.0, 0.0, 0.0);
  m_mass = 0.0;
  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      m_mass += (*it)->mass ();
      // The particle reports its position in the body frame.
      m_body_cm += (*it)->mass_position () * (*it)->mass ();
    }
  m_body_cm /= m_mass;

  // Initialize the inertia tensor.
  m_inertia.zero ();
  // Inertia tensor for rotations about the center of mass.
  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {  
      m_inertia.add ((*it)->mass (),
                     (*it)->mass_position () - m_body_cm);
    }
  m_inertia.update ();
}

void Vamos_Body::
Rigid_Body::find_forces ()
{
  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->find_forces ();
    }
}

void Vamos_Body::
Rigid_Body::propagate_contact()
{
	// Process single-collision contact.
  	if (m_contact_parameters.m_distance > 0.0)
    {
		Particle* point = m_contact_parameters.mp_contact_point;
		Three_Vector position = rotate_out (point->contact_position () 
										  - m_body_cm);
		Three_Vector velocity = m_cm_velocity + rotate_out (m_ang_velocity).
		cross (rotate_out (point->position () - m_body_cm));
		Three_Vector ang_vel = 
		m_ang_velocity.project (m_contact_parameters.m_normal);
		point->contact (position,
					  m_inertia, 
					  rotate_in (velocity),
					  m_contact_parameters.m_distance, 
					  rotate_in (m_contact_parameters.m_normal), 
					  rotate_in (ang_vel), 
					  m_contact_parameters.m_material);
		
		translate ((m_contact_parameters.m_distance + 0.00) 
				 * m_contact_parameters.m_normal);
		
		m_contact_parameters.m_distance = 0.0;
    }
}

void Vamos_Body::
Rigid_Body::single_point_contact(Vamos_Geometry::Three_Vector worldposition,
		double distance,
		Vamos_Geometry::Three_Vector normal,
		Vamos_Geometry::Material_Handle material, double time)
{
	if (distance > 0.0)
    {
		Three_Vector contactposition = transform_in(worldposition);
		Three_Vector position = rotate_out (contactposition - m_body_cm);
		Three_Vector velocity = m_cm_velocity + rotate_out (m_ang_velocity).
			cross (position);
		Three_Vector ang_vel = 
			m_ang_velocity.project (normal);
				
		Vamos_Geometry::Three_Vector v_perp = rotate_in (velocity.project (normal));

		
		
		// Find the effective mass.
		double meff = m_inertia.inertia (position, normal);
		Three_Vector impulse = -(1.0 + (material->restitution_factor ())) * meff * v_perp;
		
		Vamos_Geometry::Three_Vector v_par = rotate_in (velocity) - v_perp;
		impulse -= v_par.unit () * material->friction_factor () * impulse.abs ();
		
		/*double impmax = 1;
		for (int i = 0; i < 3; i++)
		{
			if (impulse[i] > impmax)
				impulse[i] = impmax;
			if (impulse[i] < -impmax)
				impulse[i] = -impmax;
		}*/
		
		/*double impscale = 0.1;
		for (int i = 0; i < 3; i++)
			impulse[i] *= impscale;*/
		
		/*double maxvimpulse = 0;
		if (impulse[1] > maxvimpulse)
			impulse[1] = maxvimpulse;
		if (impulse[1] < -maxvimpulse)
			impulse[1] = -maxvimpulse;*/
		
		//cout << impulse[0] << "," << impulse[1] << "," << impulse[2] << endl;
		
		
		
		
		translate ((distance + 0.00) 
				 * normal);



		// Find the force that the particle exerts on the rest of the system.
		// The particle reports its force in the Body frame.
		Three_Vector total_force;
  		Three_Vector total_torque;
		
		Three_Vector body_force = impulse / time;
		total_force += body_force;
		
		// Find the force and torque that the particle exerts on the Body.
		// Find the vector from the cm to the particle in the world frame.
		//Three_Vector torque_dist = m_body_cm - (*it)->torque_position ();
		//Three_Vector torque = (*it)->torque ();
		//double I = (m_inertia * torque.unit ()).abs ();
		//torque *= I / (I + m_mass * torque_dist.dot (torque_dist));
		Three_Vector force_dist = m_body_cm - contactposition;
		Three_Vector zero(0,0,0);
		total_torque += zero - force_dist.cross (body_force);
		
		// Transform the forces to the parent's coordinates so we can find
		// out how the Body moves w.r.t its parent.
		total_force = rotate_out (total_force) + m_gravity * m_mass;
		
		Three_Vector delta_omega = time * total_torque * m_inertia.inverse ();
		Three_Vector delta_theta = (m_ang_velocity + delta_omega) * time;
		//m_last_ang_velocity = m_ang_velocity;
		m_ang_velocity += delta_omega;
		
		Three_Vector delta_v = time * total_force / m_mass;
		Three_Vector delta_r = (m_cm_velocity + delta_v) * time;
		//m_last_cm_velocity = m_cm_velocity;
		m_cm_velocity += delta_v;
		
		// Because the body's origin is not necessarily coincident with the
		// center of mass, the body's translation has a component that
		// depends on the orientation.  Place the Body by translating to the
		// cm, rotating and then translating back.
		//m_last_position = m_position;
		translate (m_body_cm);
		
		// rotate() acts in the body frame.
		//m_last_orientation = m_orientation;
		rotate (delta_theta);
		translate (delta_r - m_body_cm);
		
		// Determine the velocity of the origin.
		//m_last_velocity = m_velocity;
		m_velocity = (m_position - m_last_position) / time;
    }
}

// Advance the body in time by TIME.
void Vamos_Body::
Rigid_Body::propagate (double time)
{
	valid = true;
	
	bool contact = false;
	
  // Re-calculate the inertia tensor and center of mass.
  update_center_of_mass ();

  //VENZON:  the stuff in propagate_contact used to be here
	/*if (m_contact_parameters.m_distance > 0.0)
    {
		contact = true;
	}*/
	//propagate_contact();

  // Propagate the particles
  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->propagate (time);
    }
 
  // Move the body and the particles in response to forces applied to
  // them and their momenta, while keeping their relative positions
  // fixed.
  m_delta_time = time;
  Three_Vector total_force;
  Three_Vector total_torque;

  for (std::vector <Particle*>::const_iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      // Find the force that the particle exerts on the rest of the system.
      // The particle reports its force in the Body frame.
      Three_Vector body_force = (*it)->force () + (*it)->impulse () / time;
		//Three_Vector body_force = (*it)->force ();
      total_force += body_force;

      // Find the force and torque that the particle exerts on the Body.
      // Find the vector from the cm to the particle in the world frame.
      Three_Vector torque_dist = m_body_cm - (*it)->torque_position ();
      Three_Vector torque = (*it)->torque ();
      double I = (m_inertia * torque.unit ()).abs ();
      torque *= I / (I + m_mass * torque_dist.dot (torque_dist));
      Three_Vector force_dist = m_body_cm - (*it)->force_position ();
      total_torque += torque - force_dist.cross (body_force);
    }

  // Transform the forces to the parent's coordinates so we can find
  // out how the Body moves w.r.t its parent.
  total_force = rotate_out (total_force) + m_gravity * m_mass;

  Three_Vector delta_omega = time * total_torque * m_inertia.inverse ();
  Three_Vector delta_theta = (m_ang_velocity + delta_omega) * time;
  if (!contact) 
	  m_last_ang_velocity = m_ang_velocity;
  m_ang_velocity += delta_omega;

  Three_Vector delta_v = time * total_force / m_mass;
  Three_Vector delta_r = (m_cm_velocity + delta_v) * time;
  if (!contact) 
	  m_last_cm_velocity = m_cm_velocity;
  m_cm_velocity += delta_v;

  // Because the body's origin is not necessarily coincident with the
  // center of mass, the body's translation has a component that
  // depends on the orientation.  Place the Body by translating to the
  // cm, rotating and then translating back.
  if (!contact)
	  m_last_position = m_position;
  translate (m_body_cm);

  // rotate() acts in the body frame.
  if (!contact)
	  m_last_orientation = m_orientation;
  rotate (delta_theta);
  translate (delta_r - m_body_cm);

  // Determine the velocity of the origin.
  if (!contact) 
	  m_last_velocity = m_velocity;
  m_velocity = (m_position - m_last_position) / time;
}

// Undo the last propagation.
void Vamos_Body::
Rigid_Body::rewind ()
{
  m_position = m_last_position;
  m_velocity = m_last_velocity;
  m_cm_velocity = m_last_cm_velocity;

  m_orientation = m_last_orientation;
  m_ang_velocity = m_last_ang_velocity;
}

// Finish the timestep.
void Vamos_Body::
Rigid_Body::end_timestep ()
{
  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->end_timestep ();
    }
}

// Return the velocity of the particle in the parent frame.
Three_Vector Vamos_Body::
Rigid_Body::velocity (Particle* particle)
{
  Three_Vector r_vec = particle->position ();
  return m_cm_velocity + rotate_out (m_ang_velocity).
    cross (rotate_out (r_vec - m_body_cm));
}

// Handle a collision.
void Vamos_Body::
Rigid_Body::contact (Particle* contact_point, 
               double distance,
               const Three_Vector& normal,
               const Vamos_Geometry::Material_Handle material)
{
  if (contact_point->single_contact ())
    {
      if (distance > m_contact_parameters.m_distance)
        {
          m_contact_parameters.mp_contact_point = contact_point;
          m_contact_parameters.m_distance = distance;
          m_contact_parameters.m_normal = normal;
          m_contact_parameters.m_material = material;
        }
    }
  else
    {
      Three_Vector position = 
        rotate_out (contact_point->contact_position () - m_body_cm);
      Three_Vector ang_vel = m_ang_velocity.project (normal);
      Three_Vector velocity = m_cm_velocity + rotate_out (m_ang_velocity).
        cross (rotate_out (contact_point->position () - m_body_cm));
      contact_point->contact (position,
                              m_inertia,
                              rotate_in (velocity),
                              distance,
                              rotate_in (normal),
                              rotate_in (ang_vel),
                              material);
    }
}

// Transform the wind into the body frame and send it to the INDEXth
// particle which must be an Aerodynamic_Device.
void Vamos_Body::
Rigid_Body::wind (Particle* aero_device, 
                  const Three_Vector& wind_vector, 
                  double density)
{
  aero_device->wind (rotate_in (wind_vector), density);
}

// Return the body to its initial state at its initial position.
void Vamos_Body::
Rigid_Body::reset ()
{
  m_position = m_initial_position;
  m_orientation.identity ();

  private_reset ();
}

// Return the body to its initial state at a particular position and
// orientation.
void Vamos_Body::
Rigid_Body::reset (const Three_Vector& position, 
                   const Three_Matrix& orientation)
{
  m_position = position;
  m_orientation = orientation;

  private_reset ();
}

// Common code for the two reset () methods.
void Vamos_Body::
Rigid_Body::private_reset ()
{
  m_velocity.zero ();
  m_cm_velocity.zero ();
  m_ang_velocity.zero ();

  for (std::vector <Particle*>::iterator it = m_particles.begin ();
       it != m_particles.end ();
       it++)
    {
      (*it)->reset ();
    }
}
