/*****************************
File:      DampedDynamic.cpp
Language:   C++ (header)
Project:    H3DNetworkingUtils
The contents of this file are subject to the Mozilla Public License
Version 1.1 (the "License"); you may not use this file except in
compliance with the License. You may obtain a copy of the License at
http://www.mozilla.org/MPL/

Software distributed under the License is distributed on an "AS IS"
basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
License for the specific language governing rights and limitations
under the License.

The Original Code is H3DNetworkingUtils v1.0.

The Initial Developer of the Original Code is CSIRO.
Portions created by the Initial Developer are Copyright (C) 1009 CSIRO. All Rights Reserved.
Contributor(s):
    Chris Gunn  <Chris.Gunn@csiro.au> <ChrisJGunn@gmail.com>
***************************/

#include "H3DNetworkingUtils/DampedDynamic.h"

using namespace H3D;
using namespace H3DNetworkingUtils;
///const H3DFloat DampedDynamic::MAX_JUMP = 0.001f;

// -------------------------------------------------
H3DNodeDatabase DampedDynamic::database(
  "DampedDynamic", 
  &(newInstance<DampedDynamic>),
  typeid(DampedDynamic),
  &GrabableDynamic::database);


// -------------------------------------------------
DampedDynamic::DampedDynamic(Inst< AddChildren       > _addChildren,   
                  Inst< RemoveChildren     > _removeChildren,
                  Inst< MFChild            > _children,
                  Inst< SFNode             > _metadata,
                  Inst< SFBound            > _bound,  
                  Inst< SFVec3f            > _bboxCenter,
                  Inst< SFVec3f            > _bboxSize,
                  Inst< SFTransformedBound > _transformedBound,
                  Inst< SFMatrix4f         > _matrix,
                  Inst< SFMatrix4f         > _accumulatedForward,
                  Inst< SFMatrix4f         > _accumulatedInverse,
                  Inst< SFVec3f            > _position,
                  Inst< SFRotation         > _orientation,
                  Inst< SFVelocity         > _velocity,
                  Inst< SFVec3f            > _momentum,
                  Inst< SFVec3f            > _force,
                  Inst< SFAngularVelocity  > _angularVelocity,
                  Inst< SFVec3f            > _angularMomentum,
                  Inst< SFSpin             > _spin,
                  Inst< SFVec3f            > _torque,
                  Inst< SFFloat            > _mass,
                  Inst< SFMatrix3f         > _inertiaTensor,
                  Inst< SFMotion           > _motion,
                  Inst<MFVec3f>         _contactForces,
                  Inst<MFVec3f>         _contactTorques,
                  Inst<SumMFVec3f>      _totalContactForce,
                  Inst<SumMFVec3f>      _totalContactTorque,
                  Inst<SumForces>       _externalForces,
                  Inst<SumTorques>      _externalTorques,
                  Inst<SFVec3f>         _freedom,
                  Inst<SFVec3f>         _angularFreedom,
                  Inst<SFFloat>         _linearDamping,
                  Inst<SFFloat>         _rotationalDamping,
                  Inst<SFFloat>         _linearSpringStiffness,
                  Inst<SFFloat>         _angularSpringStiffness,
                  Inst<SFVec3f>         _globalPosition,

                  Inst< MFCollidableGeometry > _collisionSpheres,
                  Inst< H3D::SFFloat>    _collisionStiffness,
                  Inst< SFFloat >        _collisionResolution,
                  Inst<SFBool             >  _grabbed,
                  Inst<SFVec3f            > _grabPoint,
                  Inst<SFFloat            > _grabStrength,
                  Inst<SFFloat            > _grabSlackRadius,
                  Inst<GrabForce	   		> _grabForce,
						Inst<Inverter           > _grabReactionForce,
                  Inst<MovedPos           > _grabSpringAnchorPt,
                  Inst<GrabTorque         > _grabTorque,
						Inst<H3D::SFInt32       > _grabSmoothing,
						Inst<H3D::SFFloat       > _grabDamping,
                  Inst<SlaveSwitch        > _slaveMode,
                  Inst<SumVec3f          > _appliedForce,
                  Inst<SumVec3f          > _appliedTorque) :
GrabableDynamic (_addChildren, _removeChildren, _children, _metadata, _bound, 
           _bboxCenter, _bboxSize, _transformedBound, _matrix, 
           _accumulatedForward, _accumulatedInverse, _position, _orientation, _velocity, _momentum,
           _force, _angularVelocity, _angularMomentum, _spin, _torque, _mass, _inertiaTensor, _motion,
           _contactForces, _contactTorques, _totalContactForce, _totalContactTorque, _externalForces,
           _externalTorques, _freedom, _angularFreedom, _linearDamping, _rotationalDamping, _linearSpringStiffness, 
           _angularSpringStiffness,
           _globalPosition,
           _collisionSpheres, _collisionStiffness, _collisionResolution, _grabbed, _grabPoint,
           _grabStrength, _grabSlackRadius, _grabForce, _grabReactionForce, _grabSpringAnchorPt, _grabTorque,
			  _grabSmoothing, _grabDamping, _slaveMode, _appliedForce, _appliedTorque) {
///impulse_forces (new SFVec3f),
///impulse_torques (new  SFVec3f) {

   type_name = "DampedDynamic";
   database.initFields( this );

   ((SFMotion*)_motion)->setOwner(this);
   ///impulse_forces->route(externalForces);
   ///impulse_torques->route(externalTorques);

   // It needs a low value of inertia to get torques to have any visible effect. Not sure why.
   inertiaTensor->setValue(Matrix3f(0.002f, 0.0f, 0.0f,
                                    0.0f, 0.002f, 0.0f,
                                    0.0f, 0.0f, 0.002f));
   linearDamping->setValue(0.0f);
   rotationalDamping->setValue(0.0f);
}


// -------------------------------------------------
void DampedDynamic::traverseSG(TraverseInfo & ti) {
   GrabableDynamic::traverseSG(ti);
   // Clear out impulses.
   ///impulse_forces->setValue(Vec3f(0.0f,0.0f,0.0f));
   ///impulse_torques->setValue(Vec3f(0.0f,0.0f,0.0f));
}

// ---------------------------------
void DampedDynamic::SFMotion::update() {
  if ( routes_in.size() != 1 )
    return;
  
  H3DTime  t  = static_cast< SFTime *>(routes_in[0])->getValue();

  if( last_t == 0 ) last_t = t;
  H3DTime dt = t - last_t;
  last_t = t;
  if ( dt > 0.2 || dt <= 0 ) return; // ignore large dt's

  // iteratively perform the RK4 integration at 1ms intervals until
  // we have exceeded the current dt.
  LMState state;
  DampedDynamic *ds = static_cast<DampedDynamic*>(owner);
  state.pos = ds->position->getValue(); 
  ///state.vel = ds->velocity->getValue();
  ///state.mom = ds->momentum->getValue();
  state.force = ds->force->getValue(); 
  state.orn = Quaternion( ds->orientation->getValue() ); 
  ///state.spin = ds->spin->getValue();
  ///state.angVel = ds->angularVelocity->getValue();
  ///state.angMom = ds->angularMomentum->getValue();
  state.torque = ds->torque->getValue(); 
  state.mass = ds->mass->getValue() * 20;  // 20 is a fudge factor to get it closer to kgs
  state.inertiaTensor = ds->inertiaTensor->getValue();

  updateState(state, dt);

  if( ( ds->position->getValue() - state.pos ).lengthSqr() > 0 )
     ds->position->setValue( state.pos );
  ds->momentum->setValue( Vec3f(0.0f,0.0f,0.0f) );
  if( ( Quaternion( ds->orientation->getValue() ) - state.orn ).norm() > Constants::f_epsilon )
      ds->orientation->setValue( Rotation( state.orn ) );
  ds->angularMomentum->setValue( Vec3f(0.0f,0.0f,0.0f) );

}

// -------------------------------------------------
///void DampedDynamic::SFMotion::updateState(H3DNetworkingUtils::LMState &state, H3DTime dt) {
void DampedDynamic::SFMotion::updateState(H3D::LMState &state, H3DTime dt) {
   state.pos += state.force / state.mass * dt; 
   state.orn += Quaternion(state.inertiaTensor.inverse() * state.torque * dt); 
}

