//  Rigid_Body.h - a rigid body.
//
//  Copyright (C) 2001--2002 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

#ifndef _RIGID_BODY_H_
#define _RIGID_BODY_H_

#include <vamos/geometry/Three_Vector.h>
#include <vamos/geometry/Three_Matrix.h>
#include <vamos/geometry/Material.h>
#include <vamos/geometry/Inertia_Tensor.h>
#include <vamos/body/Frame.h>
#include <vamos/body/Contact_Point.h>

#include <vector>

namespace Vamos_Body
{
  struct Contact_Parameters
  {
	Contact_Parameters ();

	Particle* mp_contact_point;
	double m_distance;
	Vamos_Geometry::Three_Vector m_normal;
	Vamos_Geometry::Material_Handle m_material;
  };

  //* A rigid body.
  class Rigid_Body : public Frame
  {
	// The body's initial position vector, used by reset ().
	Vamos_Geometry::Three_Vector m_initial_position;

	Vamos_Geometry::Three_Vector m_last_position;

	// The velocity of the center of mass.
	Vamos_Geometry::Three_Vector m_cm_velocity;

	Vamos_Geometry::Three_Vector m_last_cm_velocity;

	Vamos_Geometry::Three_Vector m_last_velocity;

	Vamos_Geometry::Three_Matrix m_last_orientation;

	Vamos_Geometry::Three_Vector m_last_ang_velocity;

	// The acceleration due to gravity, distance/time^2.  The units
	// determine the distance and time units used in the rest of the
	// simulation.
	Vamos_Geometry::Three_Vector m_gravity;

	// The elapsed time since the last time step.
	double m_delta_time;

	// The total mass of the body.
	double m_mass;

	// Common code for the two reset () methods.
	void private_reset ();

	Contact_Parameters m_contact_parameters;
	
	bool valid;

  protected:
	// The inertia tensor for the body.
	Vamos_Geometry::Inertia_Tensor m_inertia;

	// A vector of pointers to the force and torque producers that
	// make up the body
	std::vector <Particle*> m_particles;

	// The position of the center of mass of the body relative to the
	// origin of the body.
	Vamos_Geometry::Three_Vector m_body_cm;
  
  	Vamos_Geometry::Three_Vector m_last_body_cm;

  public:
	//** Constructors

	// Specify the position and orientation of the body.
	Rigid_Body (const Vamos_Geometry::Three_Vector& pos, 
                const Vamos_Geometry::Three_Matrix& orient);

	// Specify the position, the orientation is the same as the parent.
	Rigid_Body (const Vamos_Geometry::Three_Vector& pos);

	// The body is coincident with the parent.
	Rigid_Body ();

	//** Destructor
	virtual ~Rigid_Body ();
  	//~Rigid_Body();

    Vamos_Geometry::Three_Vector center_of_mass () const { return m_body_cm; }

	// Return the position of the center of mass of the body with
	// respect to the world.
	Vamos_Geometry::Three_Vector cm_position ();

	// Provide access to the frame's position method.  Return the
	// origin of the body.
	Vamos_Geometry::Three_Vector position () const 
	{ return Frame::position (); }
	
	bool IsValid() {return valid;}

	// Return the contact position of the particle with respect to the
	// world.
	Vamos_Geometry::Three_Vector contact_position (Particle* contact_point);
	Vamos_Geometry::Three_Vector last_contact_position (Particle* contact_point);
	
	// Return the smallest contact position z-value of the particles.
	double lowest_contact_position () const;

	// Add a particle to the body.
	void add_particle (Particle* const comp) 
	{ m_particles.push_back (comp); } 

	// Calculate the center of mass, the ineritia tensor, and its
	// inverse.
	void update_center_of_mass ();

	// Provide access to the particles.
	std::vector <Particle*>& particles () { return m_particles; }

	void find_forces ();

	// Advance the body in time by TIME.
	void propagate (double time);
	
	void propagate_contact ();

	// Undo the last propagation.
	void rewind ();

	// Finish the timestep.
	void end_timestep ();

	// Called by the world to tell the body what the acceleration due
	// to gravity is.
	void gravity (const Vamos_Geometry::Three_Vector& grav) 
	{ m_gravity = grav; }

	// Return the velocity of the particle in the parent frame.
	// With no argument, return the velocity of the origin of the body
	// frame.
	Vamos_Geometry::Three_Vector velocity (Particle* particle);

	// Return the velocity of the center of mass.
	Vamos_Geometry::Three_Vector cm_velocity () const { return m_cm_velocity; }
	Vamos_Geometry::Three_Vector cm_last_velocity () const { return m_last_cm_velocity; }

	// Set the velocity of the center of mass.
	void cm_velocity (Vamos_Geometry::Three_Vector vel) 
	{ m_cm_velocity = vel;}

	// Handle a collision.
	void contact (Particle* contact_point, 
				  double distance,
				  const Vamos_Geometry::Three_Vector& normal,
				  Vamos_Geometry::Material_Handle material);

	// Transform the wind into the body frame and send it to the 
	// aerodynamic device.
	void wind (Particle* aero_device, 
			   const Vamos_Geometry::Three_Vector& wind_vector, 
			   double density);

	// Return the total mass.
	double mass () const { return m_mass; }

	// This function is defined by subclasses that work with a
	// graphics system.
	virtual void draw () {};

	// Return the body to its initial state at its initial position.
	virtual void reset ();

	// Return the body to its initial state at a particular position and
	// orientation.
	virtual void reset (const Vamos_Geometry::Three_Vector& position, 
						const Vamos_Geometry::Three_Matrix& orientation);
	
	void set_position(Vamos_Geometry::Three_Vector np) {place(np);m_last_position = np;}
	void set_orientation(Vamos_Geometry::Three_Matrix no) {orient(no);m_last_orientation = no;}
	void set_angvel(Vamos_Geometry::Three_Vector nav) {set_ang_velocity(nav); m_last_ang_velocity = nav;}
	void set_velocity(Vamos_Geometry::Three_Vector vel) {m_cm_velocity = vel; m_velocity = vel; m_last_velocity = vel; m_last_cm_velocity = vel;}
	
	//set current state to last state
	void roll_back();
	
	//set velocities to zero
	void kill_vel();
	
	//respond to a single point contact
	void single_point_contact(Vamos_Geometry::Three_Vector worldposition,
		double distance,
		Vamos_Geometry::Three_Vector normal,
		Vamos_Geometry::Material_Handle material, double time);
  };
}

#endif // !_BODY_H_
