/*****************************
File:      Dynamic.h
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>
***************************/

/// \file Dynamic.h
/// \brief Header file for Dynamic, adds haptic input to the DynamicTransform class

#ifndef Dynamic_H
#define Dynamic_H

#include "H3DNetworkingUtils/Config.h"
#include "H3D/DynamicTransform.h"
#include "H3D/MFVec3f.h"
#include "H3D/SFVec3f.h"
#include "H3D/SFMatrix4f.h"
#include "H3D/SFInt32.h"
#include "H3D/MFInt32.h"

namespace H3DNetworkingUtils {

/// \class Dynamic
/// The Dynamic class is a DynamicTransform that puts the necessary routes in place to collect haptic forces 
/// on its enclosed geometry and influence the motion of the transform.
/// It encapsulates the routes that can be seen in the H3D example Dynamic/Dynamic.x3d inside the 
/// class so that the user does not have to set them up. In this way it is closer to the Reachin Dynamic class.
/// However, it only searches for these geometries at initialization time, so it won't work for geometries added later. \n
/// <b>Examples:</b>
///   - <a href="../../examples/DynamicTest.x3d">DynamicTest.x3d</a>
///
/// Notes on scaling:
/// If the Dynamic is beneath a scaling transform, the inertiaTensor, rotationalDamping and linearDamping values may need to be changed.
/// If you increase scaling, you need to reduce the inertia tensor , and vice versa, to get the same behaviour
/// If you increase the scale you need to reduce the damping, and vice versa to get the same behaviour.

class H3D_NETWORKING_UTILS_DLL_SPEC Dynamic : public H3D::DynamicTransform {

public:
   // Fortward references;
   struct SumMFVec3f;
   class SumForces;
   class SumTorques;

   typedef H3D::TypedMFNodeObject<H3D::X3DGeometryNode> MFGeometryNode;
      
    /// Overrides parent class SFMotion to only set position, orientation if they are actually changed by a significant amount.
    class H3D_NETWORKING_UTILS_DLL_SPEC SFMotion:  public  DynamicTransform::SFMotion  {
    public:
    protected:
      /// Update the matrix from the fields in the Transform node.
      virtual void update();
    };

   /// Constructor
   Dynamic( H3D::Inst< AddChildren              > _addChildren        = 0,
            H3D::Inst< RemoveChildren           > _removeChildren     = 0,
            H3D::Inst< MFChild                  > _children           = 0,
            H3D::Inst< H3D::SFNode              > _metadata           = 0,
            H3D::Inst< SFBound                  > _bound              = 0,
            H3D::Inst< H3D::SFVec3f             > _bboxCenter         = 0,
            H3D::Inst< H3D::SFVec3f             > _bboxSize           = 0,
            H3D::Inst< SFTransformedBound       > _transformedBound   = 0,
            H3D::Inst< SFMatrix4f               > _matrix             = 0,
            H3D::Inst< SFMatrix4f               > _accumulatedForward = 0,
            H3D::Inst< SFMatrix4f               > _accumulatedInverse = 0,
            H3D::Inst< H3D::SFVec3f             > _position           = 0,
            H3D::Inst< H3D::SFRotation          > _orientation        = 0,
            H3D::Inst< SFVelocity               > _velocity           = 0,
            H3D::Inst< H3D::SFVec3f             > _momentum           = 0,

            H3D::Inst< H3D::SFVec3f             > _force              = 0,
            H3D::Inst< SFAngularVelocity        > _angularVelocity    = 0,
            H3D::Inst< H3D::SFVec3f             > _angularMomentum    = 0, 
            H3D::Inst< SFSpin                   > _spin               = 0,
            H3D::Inst< H3D::SFVec3f             > _torque             = 0,
            H3D::Inst< H3D::SFFloat             > _mass               = 0,
            H3D::Inst< H3D::SFMatrix3f          > _inertiaTensor      = 0,
            H3D::Inst< SFMotion                 > _motion             = 0,
            
            H3D::Inst<H3D::MFVec3f              > _contactForces      = 0,
            H3D::Inst<H3D::MFVec3f              > _contactTorques     = 0,
            H3D::Inst<SumMFVec3f                > _totalContactForce  = 0,
            H3D::Inst<SumMFVec3f                > _totalContactTorque = 0,
            H3D::Inst<SumForces                 > _externalForces     = 0,
            H3D::Inst<SumTorques                > _externalTorques    = 0,
            H3D::Inst<H3D::SFVec3f              > _freedom            = 0,
            H3D::Inst<H3D::SFVec3f              > _angularFreedom     = 0,
            H3D::Inst<H3D::SFFloat              > _linearDamping      = 0,
            H3D::Inst<H3D::SFFloat              > _rotationalDamping  = 0,
            H3D::Inst<H3D::SFFloat              > _linearSpringStiffness = 0,
            H3D::Inst<H3D::SFFloat              > _angularSpringStiffness = 0,
            H3D::Inst<H3D::SFVec3f              > _globalPosition = 0);
   
   // The X3D interface
   static H3D::H3DNodeDatabase database;

   /// The array of forces being applied to the object from user haptic tool contacts \n
   /// access type: outputOnly \n
   /// basic type: MFVec3f \n
   /// default value: 
   auto_ptr<H3D::MFVec3f>     contactForces;

   /// The array of torques being applied to the object from user haptic tool contacts \n
   /// access type: outputOnly \n
   /// basic type: MFVec3f \n
   /// default value: 
   auto_ptr<H3D::MFVec3f>     contactTorques;

   /// The freedom field allows the Dynamic to be restricted in its movement in one, two or three axes \n
   /// Note that this has no effect on its rotational behaviour \n
   /// For example 1,1,1 allows full movement \n
   ///             0,0,0 allows no movement \n
   ///             1,0,0 allows movement along the x axis only (this could be called a 'slot-Dynamic') \n
   ///             1,1,0 allows movement on the xy plane only (this could be called a 'plane-Dynamic') \n
   ///             0.5,1,1 allows movement in any direction but is 50% restricted in the x direction \n
   /// Note that this class can behave as a slot-dynamic if you restrict two dimensions \n
   /// access type: inputOutput \n
   /// basic type: SFVec3f \n
   /// default value: 1 1 1
   auto_ptr<H3D::SFVec3f>     freedom;

   /// angularFreedom allows the Dynamic to be restricted in its rotation in one, two or three axes \n
   /// For example, an angularFreedom of 0 0 1 will only allow rotation about the z axis \n
   /// For finer rotational control \n
   /// set inertiaTensor = 0.0001 0 0   0 0.0001 0  0 0 0.0001 for a really loose spin \n
   /// or                  10000 0 0   0 1000 0   0 0 1000   for no spin \n
   /// access type: inputOutput \n
   /// basic type: SFVec3f \n
   /// default value: 1 1 1
   auto_ptr<H3D::SFVec3f>     angularFreedom;

   /// Damping for linear movement \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0
   auto_ptr<H3D::SFFloat>     linearDamping;

   /// Damping for rotations \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0
   auto_ptr<H3D::SFFloat>     rotationalDamping;

   /// The accumulated total of forces in contactForces \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<SumMFVec3f>       totalContactForce;

   /// The accumulated total of torques in contactTorques \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<SumMFVec3f>       totalContactTorque;

   /// Other forces can be routed to this - for example, in a networked system, \n
   /// the user forces from another machine on this object can be routed to this field \n
   /// Assumed to be global forces
   /// access type: inputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<SumForces>        externalForces;

   /// Other torques can be routed to this - for example, in a networked system, \n
   /// the user torques from another machine on this object can be routed to this field \n
   /// access type: inputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<SumTorques>       externalTorques;
 
   /// This node can optionally can have a spring force pulling it towards its initial position \n
   /// A typical stiffness = 100 \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0 (off by default)
   auto_ptr<H3D::SFFloat>     linearSpringStiffness;

   /// This node can optionally can have a spring force rotating it towards its original orientation \n
   /// A typical stiffness = 100 \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0 (off by default)
   auto_ptr<H3D::SFFloat>     angularSpringStiffness;

   /// The global position \n
   /// access type: inputOutput \n
   /// basic type: SFVec3f \n
   /// default value: (0,0,0)
   auto_ptr<H3D::SFVec3f>     globalPosition;

   /// addImpulse: apply a force lasting one graphics cycle.
   virtual void addImpulse(const H3D::Vec3f &f) const {
      impulse_forces->setValue(impulse_forces->getValue() + f);
   }

   /// addImpulseTorque: apply a torque lasting one graphics cycle.
   virtual void addImpulseTorque(const H3D::Vec3f &t) const {
      impulse_torques->setValue(impulse_torques->getValue() + t);
   }

  /// SumMFVec3f implements the impulseForce and impulseTorque
  /// fields of the MF route_in.
  struct H3D_NETWORKING_UTILS_DLL_SPEC SumMFVec3f : public H3D::TypedField<H3D::SFVec3f, H3D::MFVec3f> {
     virtual void update() {
        value = H3D::Vec3f(0.0f, 0.0f, 0.0f);
        vector<H3D::Vec3f> vec = static_cast<H3D::MFVec3f*>(event.ptr)->getValue();
        for (u_int i = 0; i < vec.size(); i++) {
           value += vec[i];
        }
     }
  };
    
   /// The SumForces class just sums the forces of the MFVec3f with any SFVec3f fields that are routed to it.
   class H3D_NETWORKING_UTILS_DLL_SPEC SumForces : public H3D::TypedField< H3D::SFVec3f, H3D::MFVec3f, H3D::AnyNumber<H3D::SFVec3f > > {
   public:
      SumForces() : dynP (0) {}
      virtual void update();
      Dynamic * dynP;
   };
   
   /// The SumTorques class sums all SFVec3f fields that are routed to it and applies the angularFreedom field.
   class H3D_NETWORKING_UTILS_DLL_SPEC SumTorques : public H3D::TypedField<H3D::SFVec3f, H3D::Types<H3D::SFRotation, H3D::SFVec3f>, H3D::AnyNumber<H3D::SFVec3f > > {
   public:
      SumTorques() : dynP (0) {}
      virtual void update();
      Dynamic * dynP;
   };

   /// Accumulates the forces on the haptic devices and inverts them to act on the Dynamic.
   class H3D_NETWORKING_UTILS_DLL_SPEC InvertForces : public H3D::AutoUpdate<H3D::TypedField< H3D::MFVec3f, H3D::Types<H3D::MFVec3f, H3D::SFMatrix4f > > > {
   public:
      virtual void update();
   };


   /// The LinDamper class generates a force to dampen the linear motion
   /// of a DynamicTransform.
   /// routes_in[0] is the velocity of the DynamicTransform node.
   /// routes_in[1] is the damping factor of the Dynamic node.
   /// routes_in[2] is the mass of the DynamicTransform node.
   /// routes_in[3] is the frame_period .
   class H3D_NETWORKING_UTILS_DLL_SPEC LinDamper : public H3D::TypedField<H3D::SFVec3f, H3D::Types<H3D::SFVec3f, H3D::SFFloat, H3D::SFFloat, H3D::SFFloat> > {
      virtual void update();
   };

   /// RotDamper dampens the angular motion of the dynamic. We route from
   /// the dynamic's angularVelocity field to the dynamic's torque.
   /// routes_in[0] is the current angular velocity.
   /// routes_in[10] is the damping factor.
   class H3D_NETWORKING_UTILS_DLL_SPEC RotDamper : public H3D::TypedField< H3D::SFVec3f, H3D::Types<H3D::SFVec3f, H3D::SFMatrix4f, H3D::SFFloat> > {
      virtual void update() {
         H3D::Vec3f angVel = static_cast<H3D::SFVec3f*>(routes_in[0])->getValue();
         H3D::Matrix4f matr = static_cast<H3D::SFMatrix4f*>(routes_in[1])->getValue();
         H3D::H3DFloat damping = static_cast<H3D::SFFloat*>(routes_in[2])->getValue();
         //value = matr.getRotationPart() * angVel * -damping;
         value = angVel * -damping;
      }
   };

   virtual void traverseSG(H3D::TraverseInfo & ti);
   
   virtual void initialize();

   // Reinitialize the anchor point to the current position
   virtual void reInitialize();

   /// The LinSpring class generates a spring force that tries to pull the node to the origin /n
   /// routes_in[0] is the current position of the DynamicTransform node /n
   /// routes_in[1] is the spring force /n
   /// 1st input is enabled, /n 
   /// 2nd input is position,  /n
   /// 3rd is springAnchorPoint,  /n
   /// 4th is springStiffness,  /n
   /// 5th is springRestLength,  /n
   /// 6th is springSlackLength
   class H3D_NETWORKING_UTILS_DLL_SPEC LinSpring : public H3D::TypedField<H3D::SFVec3f, 
                        H3D::Types<H3D::SFBool, H3D::SFVec3f, H3D::SFVec3f, H3D::SFFloat, H3D::SFFloat, H3D::SFFloat> > {
      virtual void update();
   };

protected:
   // Find any geometries within the sub-tree of this class and 
   // collect the forces and contact points (reletive to this class's coord system) 
   // from them.
   virtual void collectContactForces(H3D::Matrix4f & transform_matrix,
                                     std::vector<H3D::Vec3f> & forces, 
                                     std::vector<H3D::Vec3f> & points, 
                                     H3D::Node* nodeP);

   // The AngSpring class generates a torque that tries to keep the
   // DynamicTransform in the original position.
   // routes_in[0] is the orientation of the DynamicTransform node.
   // routes_in[1] is springAnchor orientation.
   // routes_in[2] is the spring force.
   class AngSpring : public H3D::TypedField<H3D::SFVec3f, H3D::Types<H3D::SFRotation, H3D::SFRotation, H3D::SFFloat> > {
      virtual void update();
   };

protected:
   auto_ptr<H3D::SFVec3f> impulse_forces;
   auto_ptr<H3D::SFVec3f> impulse_torques;

   auto_ptr<MFGeometryNode>         geometries;  
   bool                             first_pass;
   ///auto_ptr<InvertForces>           invertForces;
   auto_ptr<LinDamper>              lin_damper;
   auto_ptr<RotDamper>              rot_damper;
   auto_ptr<LinSpring>              lin_spring;
   auto_ptr<AngSpring>              ang_spring;
   auto_ptr<H3D::SFVec3f>           anchor_point;
   auto_ptr<H3D::SFRotation>        anchor_orn;
   auto_ptr<H3D::SFFloat>           anchor_slack_dist;
   auto_ptr<H3D::SFFloat>           lin_spring_rest_length; // this gets set to zero
   auto_ptr<H3D::SFBool>            lin_spring_enabled; // this gets set to zero
   auto_ptr<H3D::SFFloat>           frame_period; 
	H3D::H3DTime                     prev_time;

};

}
#endif