/*
 * UsePhysicsEngine.cpp
 *
 * Copyright (C) 2007 J. "MUFTI" Scheurich
 * 
 * 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; version 2 of the License
 *
 * 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 (see the file "COPYING" for details); if 
 * not, write to the Free Software Foundation, Inc., 675 Mass Ave, 
 * Cambridge, MA 02139, USA.
 */

#include "UsePhysicsEngine.h"

#ifdef HAVE_LIBODE

#define DELTA_T 0.02f

// license from the Xj3D project
/*****************************************************************************
 *                        Web3d.org Copyright (c) 2001 - 2006
 *                               Java Source
 *
 * This source is licensed under the GNU LGPL v2.1
 * Please read http://www.gnu.org/copyleft/lgpl.html for more information
 *
 * This software comes with the standard NO WARRANTY disclaimer for any
 * purpose. Use it at your own risk. If there's a problem you get to fix it.
 *
 ****************************************************************************/

#include <math.h>
#include <ode/ode.h>
#include "Scene.h"
#include "RigidBodyPhysicsNode.h"
#include "X3DRigidJointNode.h"
#include "X3DNBodyCollidableNode.h"
#include "NodeRigidBodyCollection.h"

UsePhysicsEngine::UsePhysicsEngine() 
{
    m_countTick = 0;
    m_elapsedTime = 0;
    m_deltaT = 0.02f;
    m_managedNodeTypes.append(X3D_RIGID_BODY_COLLECTION);
    m_managedNodeTypes.append(X3D_RIGID_JOINT_NODE);
    m_managedNodeTypes.append(X3D_RIGID_BODY);
    m_managedNodeTypes.append(X3D_COLLISION_COLLECTION);
    m_managedNodeTypes.append(X3D_COLLISION_SENSOR);
    m_managedNodeTypes.append(BODY_COLLIDABLE_OR_COLLISION_SPACE_NODE);

/*
    _odeWorld = dWorldCreate();
    _collisionSpace = dSimpleSpaceCreate(NULL);
    _gravity = -9.81;
    dWorldSetGravity(_odeWorld, 0.0, 0.0, _gravity);
    _constraintForceMixing = 0.1;
    _errorReductionParameter = 0.9;
    dWorldSetERP(_odeWorld, _errorReductionParameter);
*/
}

/**
 * Set the VRMLClock instance in use by this manager. Ignored for this
 * manager.
 *
 * @param clk A reference to the clock to use
 */
void 
UsePhysicsEngine::setVRMLClock(double clk) {
    m_vrmlClock = clk;
    m_lastTime = clk;
}

/**
 * Reset the local time zero for the manager. This is called when a new
 * root world has been loaded and any manager that needs to rely on delta
 * time from the start of the world loading can reset it's local reference
 * from the passed in {@link VRMLClock} instance.
 */
void 
UsePhysicsEngine::resetTimeZero() {
    m_lastTime = m_vrmlClock;
    m_countTick = 0;
    m_elapsedTime = 0;
    m_deltaT = DELTA_T;
}

/**
 * Add or remove a node of the require type to be managed.
 *
 * @param node The node instance to add for management
 */
void 
UsePhysicsEngine::addOrRemoveManagedNode(Node* node, bool add) {
    int type = node->getType();
    bool isType = true;

    switch(type) {
        case X3D_RIGID_BODY_COLLECTION:
            if(!m_updatedCollections.contains(node)) {
                if (add) {
                    m_collections.append(node);
                    m_updatedCollections.append(node);
                    ((NodeRigidBodyCollection *)node)->setTimestep(m_deltaT);
                } else {
                    m_collections.eliminate(node);
                    m_updatedCollections.eliminate(node);
                }
            }
            break;

        case X3D_RIGID_BODY:
            if(!m_updatedBodies.contains(node)) {
                if (add) {
                    m_bodies.append(node);
                    m_updatedBodies.append(node);
                } else {
                    m_bodies.eliminate(node);
                    m_updatedBodies.eliminate(node);
                }
            }
            break;

        case X3D_COLLISION_SENSOR:
            if(!m_updatedSensors.contains(node)) {
                if (add) {
                    m_sensors.append(node);
                    m_updatedSensors.append(node);
                } else {
                    m_sensors.eliminate(node);
                    m_updatedSensors.eliminate(node);
                }
            }
            break;

        case X3D_COLLISION_COLLECTION:
            if(!m_updatedCollisionSpaces.contains(node)) {
                if (add) {
                    m_collisionSpaces.append(node);
                    m_updatedCollisionSpaces.append(node);
                } else {
                    m_collisionSpaces.eliminate(node);
                    m_updatedCollisionSpaces.eliminate(node);
                }
            }
            break;

        default:
            isType = false;
    }
    if (isType)
        return;

    if (node->matchNodeClass(RIGID_JOINT_NODE)) {
        if(!m_updatedJoints.contains(node)) {
            if (add) {
                m_joints.append(node);
                m_updatedJoints.append(node);
            } else {
                m_joints.eliminate(node);
                m_updatedJoints.eliminate(node);
            }
        }
    } else if (node->matchNodeClass(BODY_COLLIDABLE_NODE)) {
        if(!m_updatedCollidables.contains(node)) {
            if (add) {
                m_collidables.append(node);
                m_updatedCollidables.append(node);
            } else {
                m_collidables.eliminate(node);
                m_updatedCollidables.eliminate(node);
            }
        }
    } else { 
        node->getScene()->errorf("Non-Physics node added to the manager");
    }
}

/**
 * Add a node of the require type to be managed.
 *
 * @param node The node instance to add for management
 */
void 
UsePhysicsEngine::addManagedNode(Node* node) {
    addOrRemoveManagedNode(node, true);
}

/**
 * Remove a node of the require type to be managed.
 *
 * @param node The node instance to add for management
 */
void 
UsePhysicsEngine::removeManagedNode(Node* node) {
    addOrRemoveManagedNode(node, false);
}


/**
 * Run the pre-event modelling for this frame now. This is a blocking call
 * and does not return until the event model is complete for this frame.
 * The time should be system clock time, not VRML time.
 *
 * @param time The timestamp of this frame to evaluate
 */
void 
UsePhysicsEngine::executePreEventModel(double time) {

    double dt = (time - m_lastTime);

    // Avoid div-zero issues in the physics model.
    if(dt == 0)
        return;

    int i;
    // First evaluate collision detection. Use the values from here to
    // feed values into the physics model.
    
    for(i = 0; i < m_collisionSpaces.size(); i++) {
        RigidBodyPhysicsNode *node = (RigidBodyPhysicsNode *)
                                     m_collisionSpaces.get(i);
        if(node->isEnabled())
           node->evaluateCollisions();
    }

    // Update the joint outputs
    for(i = 0; i < m_joints.size(); i++) {
        X3DRigidJointNode *joint = (X3DRigidJointNode *)m_joints.get(i);

        if(joint->numOutputs() != 0)
            joint->updateRequestedOutputs();
    }

    // Finally have the sensors dump their stuff.
    for(i = 0; i < m_sensors.size(); i++)
        ((RigidBodyPhysicsNode *)m_sensors.get(i))->updateContacts();
    

    for(i = 0; i < m_collections.size(); i++) {
        RigidBodyPhysicsNode *node = (RigidBodyPhysicsNode *)
                                      m_collections.get(i);

        if(node->isEnabled())
            node->updatePostSimulation();
    }
}

/**
 * Run the post-event modelling for this frame now. This is a blocking call
 * and does not return until the event model is complete for this frame.
 * The time should be system clock time, not VRML time.
 *
 * @param time The timestamp of this frame to evaluate
 */
void 
UsePhysicsEngine::executePostEventModel(double time) {
    int size = m_collections.size();

    if(++m_countTick == m_recalc_interval) {
        m_deltaT = (m_elapsedTime / (float)m_countTick) * 0.001f;
        m_countTick = 0;
        m_elapsedTime = 0;

        for(int i = 0; i < size; i++)
            ((RigidBodyPhysicsNode *)m_collections.get(i))->
                  setTimestep(m_deltaT);
    } else {
        m_elapsedTime += time - m_lastTime;
    }

    m_lastTime = time;

    // Do stuff here to push the collision stuff over to the physics
    // model.

    for(int i = 0; i < size; i++) {
        RigidBodyPhysicsNode *node = (RigidBodyPhysicsNode *)
                                     m_collections.get(i);

        if(node->isEnabled()) {
            node->processInputContacts();
            node->evaluateModel();
        }
    }

    for(int i = 0; i < m_collidables.size(); i++)
        ((X3DNBodyCollidableNode *)m_collidables.get(i))->updateFromODE();
}

/**
 * Force clearing all currently managed nodes from this manager now. This
 * is used to indicate that a new world is about to be loaded and
 * everything should be cleaned out now.
 */
void
UsePhysicsEngine::clear() {
    int i;
    for(i = 0; i < m_joints.size(); i++)
        ((RigidBodyPhysicsNode *)m_joints.get(i))->deleteFromODE();

    for(i = 0; i < m_collisionSpaces.size(); i++)
        ((RigidBodyPhysicsNode *)m_collisionSpaces.get(i))->deleteFromODE();


    for(i = 0; i < m_collections.size(); i++)
        ((RigidBodyPhysicsNode *)m_collections.get(i))->deleteFromODE();

    m_collections.resize(0);
    m_joints.resize(0);
    m_bodies.resize(0);
    m_collidables.resize(0);
    m_collisionSpaces.resize(0);
    m_sensors.resize(0);

    m_updatedCollections.resize(0);
    m_updatedJoints.resize(0);
    m_updatedBodies.resize(0);
    m_updatedCollidables.resize(0);
    m_updatedCollisionSpaces.resize(0);
    m_updatedSensors.resize(0);
}

#endif

