// 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 physical world through the ODE library
		\author Timothy M. Shead (tshead@k-3d.com)
		\author Brett W. McCoy(bmccoy@chapelperilous.net)
*/

#include "ibody.h"
#include "icollidable.h"

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

#include <boost/bind.hpp>
#include <boost/mem_fn.hpp>

#include <iomanip>
#include <vector>

namespace
{

typedef std::vector<libk3dode::ibody*> bodies_t;
typedef std::vector<libk3dode::icollidable*> collidables_t;

/////////////////////////////////////////////////////////////////////////////
// add_bodies

class add_bodies
{
public:
	add_bodies(bodies_t& Bodies) :
		m_bodies(Bodies)
	{
	}

	void operator()(k3d::iobject& Object)
	{
		libk3dode::ibody* const body = dynamic_cast<libk3dode::ibody*>(&Object);
		if(body)
			m_bodies.push_back(body);
	}

private:
	bodies_t& m_bodies;
};

/////////////////////////////////////////////////////////////////////////////
// add_collidables

class add_collidables
{
public:

  add_collidables(collidables_t Collidables) :
    m_collidables(Collidables)
  {
  }

  void operator()(k3d::iobject& Object)
  {
    libk3dode::icollidable* const collidable = dynamic_cast<libk3dode::icollidable*>(&Object);
    if(collidable)
      m_collidables.push_back(collidable);
  }

private:
  collidables_t& m_collidables;
};

/////////////////////////////////////////////////////////////////////////////
// world_implementation

class world_implementation :
	public k3d::editor::renderable<k3d::ri::renderable<k3d::transformable<k3d::persistent<k3d::object> > > > 
{
	typedef k3d::editor::renderable<k3d::ri::renderable<k3d::transformable<k3d::persistent<k3d::object> > > > base;

public:
	world_implementation(k3d::idocument& Document) :
		base(Document),
		m_world(dWorldCreate()),
		m_collision_joints(dJointGroupCreate(0)),
		m_gravityx(k3d::init_name("gravityx") + k3d::init_description("Gravity 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_gravityy(k3d::init_name("gravityy") + k3d::init_description("Gravity Y [number]") + k3d::init_value(-9.7) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_gravityz(k3d::init_name("gravityz") + k3d::init_description("Gravity 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_erp(k3d::init_name("erp") + k3d::init_description("Error Reduction Parameter [number]") + k3d::init_value(0.2) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_cfm(k3d::init_name("cfm") + k3d::init_description("Constraint Force Mixing [number]") + k3d::init_value(1.0E-10) + k3d::init_document(Document) + k3d::init_precision(2) + k3d::init_step_increment(0.1) + k3d::init_units(typeid(k3d::measurement::distance))),
		m_last_time(Document.time().view_time())
	{
		enable_serialization(k3d::persistence::proxy(m_gravityx));
		enable_serialization(k3d::persistence::proxy(m_gravityy));
		enable_serialization(k3d::persistence::proxy(m_gravityz));
		enable_serialization(k3d::persistence::proxy(m_cfm));
		enable_serialization(k3d::persistence::proxy(m_erp));

		register_property(m_gravityx);
		register_property(m_gravityy);
		register_property(m_gravityz);
		register_property(m_cfm);
		register_property(m_erp);

		Document.hierarchy().changed_signal().connect(SigC::slot(*this, &world_implementation::on_hierarchy_changed));
	}

	~world_implementation()
	{
		destroy_bodies();

		dJointGroupDestroy(m_collision_joints);
		dWorldDestroy(m_world);
	}

	void on_hierarchy_changed()
	{
		destroy_bodies();
		create_bodies();
	}

	void observe_time(double& Time, double& DeltaTime)
	{
		if(Time < time())
			{
				reset_bodies();
				m_last_time = Time;
			}

		base::observe_time(Time, DeltaTime);
	}

	void create_bodies()
	{
		k3d::for_each_descendant(document().hierarchy(), *this, ::add_bodies(m_bodies));
		std::for_each(m_bodies.begin(), m_bodies.end(), boost::bind(&libk3dode::ibody::create, _1, m_world));
	}

	void destroy_bodies()
	{
		std::for_each(m_bodies.begin(), m_bodies.end(), boost::mem_fn(&libk3dode::ibody::destroy));
		m_bodies.clear();
	}

	void reset_bodies()
	{
		std::for_each(m_bodies.begin(), m_bodies.end(), boost::mem_fn(&libk3dode::ibody::reset));
	}

	void post_animate()
	{
		if(time() > m_last_time)
			{
				// Setup the state of each body (accumulate forces, etc) ...
				std::for_each(m_bodies.begin(), m_bodies.end(), boost::mem_fn(&libk3dode::ibody::pre_step));

				// Do collision-detection ...

				// Increment the simulation ...
				dWorldSetGravity(m_world, m_gravityx.property_value(), m_gravityy.property_value(), m_gravityz.property_value());
				dWorldSetERP(m_world, m_erp.property_value());
				dWorldSetCFM(m_world, m_cfm.property_value());
				dWorldStep(m_world, time() - m_last_time);

				// Cleanup collision-detection data ...
				dJointGroupEmpty(m_collision_joints);

				// Extract the new state for each body ...
				std::for_each(m_bodies.begin(), m_bodies.end(), boost::mem_fn(&libk3dode::ibody::post_step));
			}

		m_last_time = time();
	}

	k3d::iplugin_factory& factory()
	{
		return get_factory();
	}

	static k3d::iplugin_factory& get_factory()
	{
		static k3d::plugin_factory<k3d::document_plugin<world_implementation> > factory(
			k3d::uuid(0x8e64a6c1, 0xf5c34a5f, 0xab0e3993, 0xc6835338),
			"ODEWorld",
			"Experimental ODE dynamics library plugin",
			"Objects",
			k3d::iplugin_factory::EXPERIMENTAL);

		return factory;
	}

private:
	const dWorldID m_world;
	const dJointGroupID m_collision_joints;

	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_gravityx;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_gravityy;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_gravityz;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_erp;
	k3d_measurement_property(double, k3d::immutable_name, k3d::change_signal, k3d::with_undo, k3d::local_storage, k3d::no_constraint) m_cfm;

	double m_last_time;

	bodies_t m_bodies;
  collidables_t m_collidables;
};

} // namespace

namespace libk3dode
{

/////////////////////////////////////////////////////////////////////////////
// world_factory

k3d::iplugin_factory& world_factory()
{
	return ::world_implementation::get_factory();
}

} // namespace libk3dode