/*****************************
File:      Dynamic.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/Dynamic.h"
#include "H3D/H3DHapticsDevice.h"
#include "H3D/X3DGeometryNode.h"
#include "H3D/X3DShapeNode.h"
#include "H3D/Inline.h"

using namespace H3D;
using namespace H3DNetworkingUtils;

// -----------------------------------------------------------
H3DNodeDatabase Dynamic::database(   
   "Dynamic", 
   &(newInstance<Dynamic>),
   typeid(Dynamic),
   &DynamicTransform::database);

namespace DynamicInternals {   
  FIELDDB_ELEMENT( Dynamic, freedom, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, angularFreedom, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, linearDamping, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, rotationalDamping, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, contactForces, OUTPUT_ONLY );
  FIELDDB_ELEMENT( Dynamic, contactTorques, OUTPUT_ONLY );
  FIELDDB_ELEMENT( Dynamic, totalContactForce, OUTPUT_ONLY );
  FIELDDB_ELEMENT( Dynamic, totalContactTorque, OUTPUT_ONLY );
  FIELDDB_ELEMENT( Dynamic, externalForces, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, externalTorques, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, linearSpringStiffness, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, angularSpringStiffness, INPUT_OUTPUT );
  FIELDDB_ELEMENT( Dynamic, globalPosition, OUTPUT_ONLY );
}

// -------------------------------------------------
Dynamic::Dynamic( 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) :
DynamicTransform(_addChildren, _removeChildren, _children, _metadata, _bound, _bboxCenter, _bboxSize,
                 _transformedBound, _matrix, _accumulatedForward, _accumulatedInverse, _position,
                 _orientation, _velocity, _momentum, _force, _angularVelocity, _angularMomentum,
                 _spin, _torque, _mass, _inertiaTensor, _motion),
impulse_forces (new SFVec3f),
impulse_torques (new  SFVec3f),
contactForces  (_contactForces),
contactTorques  (_contactTorques),
totalContactForce (_totalContactForce),
totalContactTorque    (_totalContactTorque),
externalForces      (_externalForces),
externalTorques     (_externalTorques),
freedom        (_freedom),
angularFreedom        (_angularFreedom),
linearDamping  (_linearDamping),
rotationalDamping (_rotationalDamping),
linearSpringStiffness   (_linearSpringStiffness),
angularSpringStiffness  (_angularSpringStiffness),
globalPosition (_globalPosition),
geometries     (new MFGeometryNode),
///invertForces   (new InvertForces),
lin_damper     (new LinDamper),
rot_damper     (new RotDamper),
lin_spring     (new LinSpring),
ang_spring     (new AngSpring),
anchor_point   (new SFVec3f),
anchor_orn     (new SFRotation),
lin_spring_rest_length (new SFFloat),
anchor_slack_dist   (new SFFloat),
lin_spring_enabled (new SFBool),
frame_period(new SFFloat),
first_pass (true) {
   
   type_name = "Dynamic";
   database.initFields( this );

   externalForces->dynP = this;
   externalTorques->dynP = this;
   freedom->setValue(Vec3f(1.0f,1.0f,1.0f));
   angularFreedom->setValue(Vec3f(1.0f,1.0f,1.0f));
   linearDamping->setValue(0.0f);
   rotationalDamping->setValue(0.0f);
	frame_period->setValue(0.033f);

   contactForces->route(externalForces);
   contactForces->route(totalContactForce, id);
   ///matrix->route(invertForces);
   ///invertForces->route( externalForces );
   externalForces->route(force, id);
   velocity->route(lin_damper);
   linearDamping->route(lin_damper);
   mass->route(lin_damper);
   frame_period->route(lin_damper);
   lin_damper->route( externalForces );

   contactTorques->route(totalContactTorque, id);
   orientation->route( externalTorques );
   totalContactTorque->route( externalTorques );
   externalTorques->route(torque, id);
   angularVelocity->routeNoEvent(rot_damper);
   matrix->routeNoEvent(rot_damper);
   rotationalDamping->routeNoEvent(rot_damper);
   rot_damper->route( externalTorques );

   linearSpringStiffness->setValue(0.0f);
   angularSpringStiffness->setValue(0.0f);

   lin_spring_enabled->setValue(true); // Always enabled - control with stiffness
   anchor_slack_dist->setValue(0.002f);
   lin_spring_rest_length->setValue(0.0f);
   lin_spring_enabled->routeNoEvent(lin_spring);
   position->routeNoEvent(lin_spring);
   anchor_point->routeNoEvent(lin_spring);
   linearSpringStiffness->routeNoEvent(lin_spring);
   lin_spring_rest_length->routeNoEvent(lin_spring);
   anchor_slack_dist->routeNoEvent(lin_spring);

   orientation->route(ang_spring);
   anchor_orn->route(ang_spring);
   angularSpringStiffness->route(ang_spring);
   lin_spring->route( externalForces ); // In parent class
   ang_spring->route( externalTorques ); // In parent class

   impulse_forces->route(externalForces);
   impulse_torques->route(externalTorques);


}

// -------------------------------------------------
void Dynamic::initialize() {
   DynamicTransform::initialize();
   anchor_point->setValue(position->getValue());
   anchor_orn->setValue(orientation->getValue());
}

// -------------------------------------------------
void Dynamic::reInitialize() {
   anchor_point->setValue(position->getValue());
   anchor_orn->setValue(orientation->getValue());
}

// -------------------------------------------------
void Dynamic::InvertForces::update() {
   if (routes_in.size() < 2) {
      return;
   }
   try {
      MFVec3f * forces = static_cast<MFVec3f*>(routes_in[0]);
      SFMatrix4f * matrix = static_cast<SFMatrix4f*>(routes_in[1]);
      if( forces == event.ptr ) {
         vector<Vec3f> fcs = forces->getValue();
         vector<Vec3f> forces_to_return;
         Matrix3f to_global = matrix->getValue().getRotationPart();
         for (unsigned int i = 0; i < fcs.size(); i++) {
            forces_to_return.push_back( ( to_global * fcs[i] ) * -1 );
         }
         value = forces_to_return;
      } else {
         value.resize(0);
         value.push_back(Vec3f(0,0,0));
      }
   } catch (...) {
         value.resize(0);
         value.push_back(Vec3f(0,0,0));
   }
}

// -------------------------------------------------
void Dynamic::SumForces::update() {
   Vec3f force( 0, 0, 0 );
   vector<Vec3f> fcs = static_cast<MFVec3f*>(routes_in[0])->getValue();
   for (unsigned int j = 0; j < fcs.size(); j++) {
      force += fcs[j];
   }
   for (unsigned int i = 1; i < routes_in.size(); i++) {
      Vec3f f = static_cast<SFVec3f*>(routes_in[i])->getValue();
      force += f;
   }
   Vec3f freedm = dynP->freedom->getValue();
   value = Vec3f(force.x*freedm.x, force.y*freedm.y, force.z*freedm.z);
}

// ---------------------------------
void Dynamic::SumTorques::update() {
   Vec3f f( 0, 0, 0 );
   H3D::Rotation rot = static_cast<SFRotation*>(routes_in[0])->getValue();
   Vec3f contact_torque = static_cast<SFVec3f*>(routes_in[1])->getValue();
   f += contact_torque;

   for (unsigned int i = 2; i < routes_in.size(); i++) {
      Vec3f v = static_cast<SFVec3f*>(routes_in[i])->getValue();
      f += v;
   }
   Vec3f freedm = dynP->angularFreedom->getValue();
   ///value = rot * (-Vec3f(f.x*freedm.x, f.y*freedm.y, f.z*freedm.z));
   ///value = rot * (Vec3f(f.x*freedm.x, f.y*freedm.y, f.z*freedm.z));
   value = Vec3f(f.x*freedm.x, f.y*freedm.y, f.z*freedm.z);
}

// ---------------------------------
void Dynamic::traverseSG(TraverseInfo & ti) {
   DynamicTransform::traverseSG(ti);

   H3DTime now = Scene::time->getValue();
	double dT = now - prev_time;
	prev_time = now;
	frame_period->setValue((H3DFloat)dT);

	///Matrix4f acc_fwd_trans = matrix->getValue();
	///Matrix4f acc_fwd_trans = ti.getAccForwardMatrix();
	// We need a transform matrix for our internal structure to determine forces.
   // These forces are applied to the internal structure, so they don't need to know about what is 
   // the transform outside. So we start with an indentity matrix, not the current forward matrix.
   Matrix4f acc_fwd_trans;
   const vector< H3DHapticsDevice * > devices = ti.getHapticsDevices();
   vector<Vec3f> forces(devices.size());
   vector<Vec3f> torques(devices.size());
	collectContactForces(acc_fwd_trans, forces, torques, this);
   contactForces->setValue(forces, id);
   contactTorques->setValue(torques, id);
   
   impulse_forces->setValue(Vec3f(0.0f,0.0f,0.0f));
   impulse_torques->setValue(Vec3f(0.0f,0.0f,0.0f));
   first_pass = false;

   // This could cause a lot of updates - and inefficiency
   globalPosition->setValue(acc_fwd_trans * position->getValue(), id);

}

// -------------------------------------------------
void Dynamic::collectContactForces(Matrix4f & acc_fwd_trans,
                                   vector<Vec3f> & forces, 
                                   vector<Vec3f> & torques, 
                                   Node* nodeP) {
   // Search the sub tree below this class to find any geometries.
   // We are interested in any groups we find (in which case we iteratively search them),
   // or any Shapes we find as they may contain geometries.
   X3DShapeNode * shapeP = dynamic_cast<X3DShapeNode*>(nodeP);
   if (shapeP) {
      X3DGeometryNode * geomP = shapeP->geometry->getValue();
      if (geomP) {
         // Found a geometry!
         if (first_pass) {
            geometries->push_back(geomP);
         }
         vector<Vec3f> fce = geomP->force->getValue() ;
         vector<Vec3f> pts = geomP->contactPoint->getValue();
         Matrix3f scale_rot_mat = acc_fwd_trans.getScaleRotationPart();
         ///Vec3f scale = acc_fwd_trans.getScalePart();
         for (unsigned int i = 0; i < fce.size(); i++) {
            if (geomP->isTouched->getValueByIndex(i)) {
               ///Vec3f dbg_f = fce[i];
               ///Vec3f dbg_p = pts[i];
               ///if (geomP->getName() == "blue") {
               ///   cout << dbg_p << endl;
               ///}
               ///Vec3f opposing_force = -fce[i]; 
               ///forces[i] += acc_fwd_trans.getRotationPart() * opposing_force;
               // The force reported by geom is 'on the tool' we want the force 'on the geom'.
               Vec3f global_force = scale_rot_mat * -fce[i]; 
               
               // There is a bug in H3D::DynamicTransform where the scaling is not taken into account, 
               // so as a work around we do it here.
               ///Vec3f scaled_global_force = global_force; 
               ///scaled_global_force.x /= scale.x;
               ///scaled_global_force.y /= scale.y;
               ///scaled_global_force.z /= scale.z;
               forces[i] += global_force;
               Vec3f transformed_pt = acc_fwd_trans * pts[i];
               torques[i] += global_force % -transformed_pt; // -ve because we have RH coord sys, but RH torque rule
                                                             // e.g. force(0,0,-1) * pt(1,0,0) needs to give +ve torque
            }
         }
      }
   } else {
      Inline * inlineP = dynamic_cast<Inline*>(nodeP);;
      if (inlineP) {
         NodeVector contents = inlineP->loadedScene->getValue();
         for (u_int i = 0; i < contents.size(); i++) {
            collectContactForces(acc_fwd_trans, forces, torques, contents[i]);
         }
      } else {
         Dynamic * dynP = dynamic_cast<Dynamic*>(nodeP);
         if (dynP) {
            if (dynP == this) {
               // Just go to the children
               Matrix4f next_transform = acc_fwd_trans * dynP->matrix->getValue();
               // Remove translation part
               next_transform.setElement(0,3, 0.0f);
               next_transform.setElement(1,3, 0.0f);
               next_transform.setElement(2,3, 0.0f);
               for (MFChild::const_iterator i = dynP->children->begin(); 
                     i != dynP->children->end(); i++) {
                  collectContactForces(next_transform, forces, torques, *i);
               }
            } else {
               // Found another Dynamic - the forces should have already been collected for this
               // in its own traverseSG()
               Matrix4f next_transform = acc_fwd_trans * dynP->matrix->getValue();
               for (unsigned int i = 0; i < forces.size(); i++) {
                  Matrix3f rot_part = next_transform.getRotationPart();
                  Vec3f dyn_force = rot_part * dynP->contactForces->getValueByIndex(i);
                  forces[i] += dyn_force;
                  ///torques[i] += dyn_force % (next_transform * dynP->position->getValue());
                  ///Vec3f rot_stiffness = Vec3f(1,1,1) - dynP->freedom->getValue();
                  ///torques[i] += (next_transform * dynP->position->getValue()) % dyn_force
                  ///               + dynP->totalContactTorque->getValue() % rot_stiffness;
                  ///torques[i] += (acc_fwd_trans * dynP->position->getValue()) % dyn_force
                  Vec3f pos = acc_fwd_trans * Vec3f(0,0,0);
                 /// Vec3f prev_torque += pos % dyn_force
                 /// torques[i] += pos % dyn_force;
                 torques[i] += rot_part * dynP->totalContactTorque->getValue();
               }
            }
         } else {
            MatrixTransform * transP = dynamic_cast<MatrixTransform*>(nodeP);
            if (transP ) {
               Matrix4f next_transform = acc_fwd_trans * transP->matrix->getValue();
               for (MFChild::const_iterator i = transP->children->begin(); 
                     i != transP->children->end(); i++) {
                  collectContactForces(next_transform, forces, torques, *i);
               }
            } else {
               X3DGroupingNode * groupP = dynamic_cast<X3DGroupingNode*>(nodeP);
               if (groupP) {
                  for (MFChild::const_iterator i = groupP->children->begin(); 
                        i != groupP->children->end(); i++) {
                     collectContactForces(acc_fwd_trans, forces, torques, *i);
                  }
               }
            }
         }
      } 
   }
}

// 1st input is enabled, 
   // 2nd input is position, 
   // 3rd is springAnchorPoint, 
   // 4th is springStiffness, 
   // 5th is springRestLength,
   // 6th is springSlackLength
// ---------------------------------
void Dynamic::LinSpring::update() {
   bool enabled = static_cast<SFBool*>(routes_in[0])->getValue();
   if (enabled) {
      H3DFloat stiffness = static_cast<SFFloat*>(routes_in[3])->getValue();
      if (stiffness > 0.0f) {
         Vec3f pos = static_cast<SFVec3f*>(routes_in[1])->getValue();
         Vec3f anchor = static_cast<SFVec3f*>(routes_in[2])->getValue();
         H3DFloat spring_rest_length = static_cast<SFFloat*>(routes_in[4])->getValue();
         H3DFloat slack = static_cast<SFFloat*>(routes_in[5])->getValue();

         Vec3f spring = anchor - pos;
         H3DFloat len = spring.length();
         value = Vec3f(0.0f,0.0f,0.0f);
         // It turns out that extension and compression of the spring require the same algoritm.
         if ((len - spring_rest_length) > slack) {
            Vec3f extension = spring * ((len - spring_rest_length - slack) / len);
            value = extension * stiffness;
			} else if ((spring_rest_length - len) > slack) {
            Vec3f extension = spring * ((len - spring_rest_length + slack) / len);
            value = extension * stiffness;
			} else {
				value = Vec3f(0.0f, 0.0f, 0.0f);
			}
      } else {
         value = Vec3f(0.0f, 0.0f, 0.0f);
      }
	} else {
      value = Vec3f(0.0f, 0.0f, 0.0f);
	}
}

// ---------------------------------
void Dynamic::LinDamper::update() {
   H3D::Vec3f vel = static_cast<H3D::SFVec3f*>(routes_in[0])->getValue();
   H3D::H3DFloat damping = static_cast<H3D::SFFloat*>(routes_in[1])->getValue();
   H3D::H3DFloat mass = static_cast<H3D::SFFloat*>(routes_in[2])->getValue();
   H3D::H3DFloat dT = static_cast<H3D::SFFloat*>(routes_in[3])->getValue();

	// We don't want damping force to add energy to the system, so it can't send the body in the
	// opposite direction, just slow it in the current direction. So the speed it contributes cannot
	// be greater than the current speed.
	H3DFloat speed = vel.length();
	H3DFloat damping_force_mag = damping * speed;
	H3DFloat acc = damping_force_mag / mass;
	H3DFloat dv = acc * dT;
	if (dv > speed) {
		dv = speed;
		acc = dv / dT;
		damping_force_mag = mass * acc;
		damping = damping_force_mag / speed;
	}
	value = -vel * damping;
}

// ---------------------------------
void Dynamic::AngSpring::update() {
   H3D::H3DFloat stiffness = static_cast<H3D::SFFloat*>(routes_in[2])->getValue();
   if (stiffness > 0.0f) {
      H3D::Rotation orn = -static_cast<H3D::SFRotation*>(routes_in[0])->getValue();
      H3D::Rotation anchor = -static_cast<H3D::SFRotation*>(routes_in[1])->getValue();
      H3D::Vec3f v1 = orn * H3D::Vec3f(1,0,0);
      H3D::Vec3f v2 = anchor * H3D::Vec3f(1,0,0);
      H3D::Rotation diff = H3D::Rotation(v2, v1);  
      value = diff.axis * diff.angle * -stiffness;
   } else {
      value = H3D::Vec3f(0.0f,0.0f,0.0f);
   }
}

// ---------------------------------
void Dynamic::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;
  /// DEBUG if ( dt > 0.2 || dt <= 0 ) return; // ignore large dt's
  if ( dt > 0.4 || dt <= 0 ) return; // ignore large dt's

  // iteratively perform the RK4 integration at 1ms intervals until
  // we have exceeded the current dt.
  LMState state;
  DynamicTransform *ds = static_cast<DynamicTransform*>(owner);
  state.pos = ds->position->getValue(); 
  state.vel = ds->velocity->getValue();
  state.mom = ds->momentum->getValue();
  state.force = ds->force->getValue(); 
  state.orn = H3D::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();
  state.inertiaTensor = ds->inertiaTensor->getValue();

  updateState( state, dt );
  // -----------------
  // These lines differ from the parent class
  /***
  Vec3f movement = ds->position->getValue() - state.pos;
  if (movement.lengthSqr() > 0.0000000001f) {
     ds->position->setValue( state.pos );
  }
  ds->momentum->setValue( state.mom );
  ds->orientation->setValue( H3D::Rotation( state.orn ) );
  ds->angularMomentum->setValue( state.angMom );
  ***/
  // -----------------
   if( ( ds->position->getValue() - state.pos ).lengthSqr() > Constants::f_epsilon )
     ds->position->setValue( state.pos );  
	if( ( ds->momentum->getValue() - state.mom ).lengthSqr() > Constants::f_epsilon )
     ds->momentum->setValue( state.mom );
	if( ( Quaternion( ds->orientation->getValue() ) - state.orn ).norm() > Constants::f_epsilon )
     ds->orientation->setValue( Rotation( state.orn ) );
	if( ( ds->angularMomentum->getValue() - state.angMom ).lengthSqr() > Constants::f_epsilon )
     ds->angularMomentum->setValue( state.angMom );

}
