// K-3D
// Copyright (c) 1995-2004, Timothy M. Shead
//
// Contact: tshead@k-3d.com
//
// 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

/** \file
		\brief Implements a body that has mass / velocity within an ODE world
		\author Timothy M. Shead (tshead@k-3d.com)
*/

#include "ibody.h"

#include <k3dsdk/persistence.h>
#include <k3dsdk/measurement.h>
#include <k3dsdk/module.h>
#include <k3dsdk/renderman.h>
#include <k3dsdk/transformable.h>

#include <sdpgl/sdpgl.h>

namespace
{

class body_implementation :
	public k3d::animate<k3d::editor::renderable<k3d::ri::renderable<k3d::transformable<k3d::persistent<k3d::object> > > > >,
	public libk3dode::ibody
{
	typedef k3d::animate<k3d::editor::renderable<k3d::ri::renderable<k3d::transformable<k3d::persistent<k3d::object> > > > > base;
		
public:
	body_implementation(k3d::idocument& Document) :
		base(Document),
		m_mass(k3d::init_name("mass") + k3d::init_description("Mass [number]") + k3d::init_value(1.0) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::mass))),
		m_forcex(k3d::init_name("forcex") + k3d::init_description("Force X [number]") + k3d::init_value(0.0) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_forcey(k3d::init_name("forcey") + k3d::init_description("Force Y [number]") + k3d::init_value(0.0) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_forcez(k3d::init_name("forcez") + k3d::init_description("Force Z [number]") + k3d::init_value(0.0) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_torquex(k3d::init_name("torquex") + k3d::init_description("Torque X [number]") + k3d::init_value(0.0) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_torquey(k3d::init_name("torquey") + k3d::init_description("Torque Y [number]") + k3d::init_value(0.0) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_torquez(k3d::init_name("torquez") + k3d::init_description("Torque Z [number]") + k3d::init_value(0.0) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_body(0)
	{
		enable_serialization(k3d::persistence::proxy(m_mass));
		enable_serialization(k3d::persistence::proxy(m_forcex));
		enable_serialization(k3d::persistence::proxy(m_forcey));
		enable_serialization(k3d::persistence::proxy(m_forcez));
		enable_serialization(k3d::persistence::proxy(m_torquex));
		enable_serialization(k3d::persistence::proxy(m_torquey));
		enable_serialization(k3d::persistence::proxy(m_torquez));

		register_property(m_mass);
		register_property(m_forcex);
		register_property(m_forcey);
		register_property(m_forcez);
		register_property(m_torquex);
		register_property(m_torquey);
		register_property(m_torquez);
	}

	const dBodyID create(const dWorldID World)
	{
		return_val_if_fail(!m_body, m_body);

		m_body = dBodyCreate(World);
		return m_body;
	}
	
	void reset()
	{
		return_if_fail(m_body);
		
		static const dReal resting_orientation[] = { 1, 0, 0, 0 };
		m_position = k3d::vector3(0, 0, 0);
		m_orientation = k3d::angle_axis(0, k3d::vector3(0, 0, 1));
				
		dBodySetPosition(m_body, 0, 0, 0);
		dBodySetQuaternion(m_body, resting_orientation);
		dBodySetLinearVel(m_body, 0, 0, 0);
		dBodySetAngularVel(m_body, 0, 0, 0);
	}
	
	void pre_step()
	{
		return_if_fail(m_body);
		
		dMass mass;
		dMassSetSphereTotal(&mass, m_mass.property_value(), 1.0);
		dBodySetMass(m_body, &mass);

		dBodyAddForce(m_body, m_forcex.property_value(), m_forcey.property_value(), m_forcez.property_value());
		
		dBodyAddTorque(m_body, m_torquex.property_value(), m_torquey.property_value(), m_torquez.property_value());
	}
	
	void post_step()
	{
		return_if_fail(m_body);
		
		// Get the new position and orientation of the body ...
		const k3d::vector3 position = dBodyGetPosition(m_body);

		k3d::quaternion orientation;
		orientation.w = dBodyGetQuaternion(m_body)[0];
		orientation.v[0] = dBodyGetQuaternion(m_body)[1];
		orientation.v[1] = dBodyGetQuaternion(m_body)[2];
		orientation.v[2] = dBodyGetQuaternion(m_body)[3];

		// Update it ...
		m_position = position;
		m_orientation = k3d::angle_axis(orientation);
		
		set_animated_child_position(m_position);
		set_animated_child_orientation(m_orientation);

	}
	
	void destroy()
	{
		return_if_fail(m_body);
		
		dBodyDestroy(m_body);
		m_body = 0;
	}

	void pop_animate()
	{
		set_animated_child_position(m_position);
		set_animated_child_orientation(m_orientation);
	}
	
	k3d::iplugin_factory& factory()
	{
		return get_factory();
	}
	
	static k3d::iplugin_factory& get_factory()
	{
		static k3d::plugin_factory<k3d::document_plugin<body_implementation> > factory(
			k3d::uuid(0x34248c5b, 0x97324a6b, 0x9335e820, 0x2934fcd5),
			"ODEBody",
			"Experimental ODE dynamics library plugin",
			"Objects",
			k3d::iplugin_factory::EXPERIMENTAL);
		
		return factory;
	}

private:
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_mass;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_forcex;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_forcey;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_forcez;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_torquex;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_torquey;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_torquez;

	dBodyID m_body;

	k3d::vector3 m_position;
	k3d::angle_axis m_orientation;	
};

} // namespace

namespace libk3dode
{

//////////////////////////////////////////////////////////////////////////////////////////
// body_factory

k3d::iplugin_factory& body_factory()
{
	return ::body_implementation::get_factory();
}

} // namespace libk3dode

