/*****************************
File:      GrabableDynamic.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 GrabableDynamic.h
/// \brief Header file for GrabableDynamic, a dynamic that can be 'grabbed' by the haptic tool.

#ifndef GrabableDynamic_H
#define GrabableDynamic_H

#include "H3DNetworkingUtils/Config.h"
#include "H3DNetworkingUtils/CollidableDynamic.h"
#include <H3D/SFTime.h>
#include <H3D/SpringEffect.h>
#include <H3D/Switch.h>

namespace H3DNetworkingUtils {

/// \class GrabableDynamic
/// The GrabableDynamic class is a CollidableDynamic that has functions that enable it to be grabbed and moved by a haptic tool.
/// The separation of the the current tool position and the dynamic's (moved) grab point is returned in a grabReactionForce 
/// that can be applied as a haptic force on the tool    \n
/// This node can also act as a slave to a master GrabableDynamic on another machine by setting slaveMode to TRUE
/// Typical usage:
/// \code
///    <ForceField DEF="grab_reaction_force"/>
///    <ROUTE fromNode='HDEV' fromField='mainButton' toNode='dyn' toField='grabbed'\>
///    <ROUTE fromNode='HDEV' fromField='proxyPosition' toNode='dyn' toField='grabPoint'\> 
///    <ROUTE fromNode='dyn' fromField='grabReactionForce' toNode='grab_reaction_force' toField='force'\>  
/// \endcode
/// <b>Examples:</b>
///   - <a href="../../examples/GrabableDynamicTest.x3d">GrabableDynamicTest.x3d</a>

class H3D_NETWORKING_UTILS_DLL_SPEC GrabableDynamic : public CollidableDynamic {
public:
  
   struct MovedPos;
   struct GrabTorque;
   class GrabForce;
	struct DiffVec3f;
	struct SumVec3f;
	struct SlaveSwitch;
	struct Inverter;

	/// Constructor
   GrabableDynamic(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,

            H3D::Inst<MFCollidableGeometry      > _collisionSpheres  = 0,
            H3D::Inst<H3D::SFFloat              > _collisionStiffness = 0,
            H3D::Inst< H3D::SFFloat             > _collisionResolution   = 0,
            H3D::Inst<H3D::SFBool               > _grabbed = 0,
            H3D::Inst<H3D::SFVec3f              > _grabPoint = 0,
            H3D::Inst<H3D::SFFloat              > _grabStrength = 0,
            H3D::Inst<H3D::SFFloat              > _grabSlackRadius = 0,
            H3D::Inst<GrabForce						> _grabForce = 0,
            H3D::Inst<Inverter                  > _grabReactionForce = 0,
            H3D::Inst<MovedPos                  > _grabSpringAnchorPt = 0,
            H3D::Inst<GrabTorque                > _grabTorque = 0,
				H3D::Inst<H3D::SFInt32              > _grabForceRampCycles = 0,
				H3D::Inst<H3D::SFFloat              > _grabDamping = 0,
				H3D::Inst<SlaveSwitch					> _slaveMode = 0,
            H3D::Inst<SumVec3f                  > _appliedForce = 0,
            H3D::Inst<SumVec3f                  > _appliedTorque = 0,
            H3D::Inst<H3D::SFFloat              > _smoothing = 0,
            H3D::Inst<H3D::SFBool               > _noSurfaceOnGrab = 0);

   static H3D::H3DNodeDatabase database;

   /// Set this to true to set a spring in place between the initial grabPoint \n
   /// (grabSpringAnchorPt) and subsequent grabPoint values \n
   /// access type: inputOutput \n
   /// basic type: SFBool \n
   /// default value: FALSE
   auto_ptr<H3D::SFBool>               grabbed;

   /// When grabbed turns true, a spring is set in place between the grabPoint \n
   /// value when it turns true and the current grabPoint as it moves around \n
   /// access type: inputOutput \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<H3D::SFVec3f>              grabPoint;

   /// The strength factor of the grabbing spring \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 60.0
   auto_ptr<H3D::SFFloat>              grabStrength;

   /// The damping factor of the grabbing spring \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0.01
   auto_ptr<H3D::SFFloat>              grabDamping;

   /// To stop oscillations there is a dead zone around the grabPoint \n
   /// If the current grabPoint is less than this amount from the original, \n
   /// the grabbing force is set to zero \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0.004
   auto_ptr<H3D::SFFloat>              grabSlackRadius;

   /// The force applied to the haptic tool by grabbing this object \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<GrabForce>						grabForce;

   /// The force currently being applied to this object by the grabbing tool \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<Inverter>					   grabReactionForce;

   /// The point on the dynamic that a grab spring is anchored \n
   /// This is set to the grabPoint value when grabbed becomes true \n
   /// As the object moves, this grabSpringAnchorPt moves and rotates with it \n
   /// In GLOBAL coords
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<MovedPos>                  grabSpringAnchorPt;

   /// The torque currently being applied to the dynamic \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<GrabTorque>                grabTorque;

   /// The cycles for the grab force to reach grabStrength\n
   /// access type: initializeOnly \n
   /// basic type: SFInt32 \n
	/// default value: 30 
	auto_ptr<H3D::SFInt32>                grabForceRampCycles;

   /// When slaveMode is true, the node has no dynamic behaviour itself, it only reports the    \n
	/// user's forces on it via the fields: appliedForce and appiedTorque     \n
	/// the intention of slaveMode is to allow it to be linked to another GrabableDynamic (master) across a network   \n
	/// and let the master do the dynamic calculations    \m
	/// In this mode, you should remoteRoute the appled force and appliedTorque to the master and route it position and orientation back
	/// The slaveMode mechanism is supplied as a replacement for the SlaveDynamic node in earlier versions for two reasons:    \n
	///   1. A GrabableDynamic can be switch to be a slave at run time
	///   2. Other nodes can inherit from GrabableDynamic and add features that also can be used when acting as a slave.
	/// access type: inputOutput \n
   /// basic type: SFBool \n
	/// default value: FALSE
	auto_ptr<SlaveSwitch>                slaveMode;

   /// The force applied to the object by user actions    \n
   /// This can be routed (via a remote route) to the externalForces field of a Dynamic on another machine   \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<SumVec3f>        appliedForce;

   /// The torque applied to the object by user actions    \n
   /// This can be routed (via a remote route) to the externalTorques field of a Dynamic    \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value:  0 0 0
   auto_ptr<SumVec3f>        appliedTorque;

   /// The smoothing on the haptic feel    \n
   /// A value between 0 and 1    \n
   /// Note that LOWER numbers are SMOOTHER (smaller steps)    \n
   /// BEWARE! Do NOT use a smoothing value of 0 : the grab point will not move with the dynamic
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value:  0.1
   auto_ptr<H3D::SFFloat>        smoothing;
   
   // Sometimes when you grab something, you don't want the thing to have a haptic surface, because it can cause an oscillation - 
   // the grab pulls it closer but the surface bumps it away. If this bool is true, the surface stiffness is temporarily
   // set ot zero while it is grabbed.
   auto_ptr<H3D::SFBool>              noSurfaceOnGrab;

	/// Initialize
	virtual void initialize();
	
	/// MovedPos provides the position of the current (perhaps moved) position of the initial point on the
   /// object that was grabbed in global coords \n
   /// input 1 is a 'grabbed' boolean, input 2 is the current grabPoint, /n
   /// input 3 is the position of the object, input 4 is the orientation of the object
   struct H3D_NETWORKING_UTILS_DLL_SPEC MovedPos : public H3D::AutoUpdate< H3D::TypedField<H3D::SFVec3f, 
                                            H3D::Types<H3D::SFBool, H3D::SFVec3f, H3D::SFVec3f, H3D::SFRotation> > > {
      MovedPos() : was_grabbed(false) {}
      virtual void update();
      bool was_grabbed;
      H3D::Vec3f initial_anchor;
      H3D::Vec3f initial_anchor_local;
      H3D::Vec3f initial_position;
      H3D::Rotation initial_rotation;
   };

   /// GrabTorque calculates the torque from the grab position and grab force \n
	/// input[0] is enabled
	/// input[1] is current grab position, /n
	/// input[2] is orig grab point, 
	/// input[3] is force vector causing torque, 
	/// input[4] is slackRadius /n
   struct H3D_NETWORKING_UTILS_DLL_SPEC GrabTorque : public H3D::TypedField< H3D::SFVec3f, 
      H3D::Types<H3D::SFBool, H3D::SFVec3f, H3D::SFVec3f, H3D::SFVec3f, H3D::SFFloat> > {
      virtual void update(); 
  };

   class H3D_NETWORKING_UTILS_DLL_SPEC GrabForce : public H3D::TypedField<H3D::SFVec3f, 
        H3D::Types<H3D::SFBool, H3D::SFVec3f, H3D::SFVec3f, 
						 H3D::SFFloat, H3D::SFInt32> > {
   public:
      GrabForce::GrabForce() : was_grabbed (false), spring_constant (0.0f) {
      }
      virtual void update();
	private:
		bool was_grabbed;
      H3D::H3DFloat spring_constant;
      H3D::H3DTime prev_time;
      H3D::Vec3f prev_connection_spring;
   };
      
      
   // Input 0 is the grabReactionForce force
   struct Inverter : public H3D::SFVec3f {
      virtual void update() {
         H3D::Vec3f v = static_cast<H3D::SFVec3f*>(event.ptr)->getValue();
         value = -v;
      }
   };

   /// Adds 2 SFVec3fs
   struct H3D_NETWORKING_UTILS_DLL_SPEC SumVec3f : public H3D::TypedField<H3D::SFVec3f, H3D::Types<H3D::SFVec3f, H3D::SFVec3f> >  {
      virtual void update() {
         value = static_cast<H3D::SFVec3f*>(routes_in[1])->getValue() + static_cast<H3D::SFVec3f*>(routes_in[0])->getValue();
      }
   };

   /// Subtracts 2 SFVec3fs
   struct H3D_NETWORKING_UTILS_DLL_SPEC DiffVec3f : public H3D::TypedField<H3D::SFVec3f, H3D::Types<H3D::SFVec3f, H3D::SFVec3f> >  {
      virtual void update() {
         value = static_cast<H3D::SFVec3f*>(routes_in[1])->getValue() - static_cast<H3D::SFVec3f*>(routes_in[0])->getValue();
      }
   };

	struct SlaveSwitch : public H3D::AutoUpdate<H3D::OnNewValueSField<H3D::SFBool> > {
		virtual void onNewValue(const bool & val) {
			static_cast<GrabableDynamic*>(getOwner())->setSlaveMode(val);
		}
	};

protected:

   // Convert global val to local coords of this class - WITHOUT including its internal transform
	struct ToLocal : public H3D::SFVec3f {
		virtual void update() {
         value = static_cast<GrabableDynamic*>(getOwner())->toLocal(static_cast<H3D::SFVec3f*>(event.ptr)->getValue());
		}
	};

   // Return global val in local coords of this class - WITHOUT including its internal transform
   virtual H3D::Vec3f toLocal(const H3D::Vec3f & val) {
      H3D::Vec3f internal_val = accumulatedInverse->getValue().getRotationPart() * val; //This includes internal transform
      ///cout << val << "    " << internal_val << "      " << matrix->getValue().getRotationPart() * internal_val << endl;
      return matrix->getValue().getRotationPart() * internal_val; // This removes it
   }
   
   virtual H3D::Vec3f getClosestPoint(const H3D::Vec3f & pt, H3D::H3DFloat & closest_point_dist);
   // Get the closest point in local coords to the given pt in global coords.
   // Also fills in the closest distance in local coords.

	virtual void setSlaveMode(bool val);

   // Input 0: grabbed
   // Input 1: noSurfaceOnGrab
   struct GrabSurfaceControl : public H3D::AutoUpdate<H3D::TypedField<H3D::Field, H3D::Types<H3D::SFBool, H3D::SFBool> > > {
      virtual void update() {
         bool grbd = static_cast<H3D::SFBool*>(routes_in[0])->getValue();
         bool no_surf = static_cast<H3D::SFBool*>(routes_in[1])->getValue();
         if (no_surf) {
           static_cast<GrabableDynamic*>(getOwner())->setSurfaces(!grbd);
         }
      }
   };
   // Switch haptic surfaces of internal shapes on or off
   virtual void GrabableDynamic::setSurfaces(bool on);

private:
	H3D::Vec3f orig_freedom;
	H3D::Vec3f orig_angular_freedom;
	bool first_slave_setting;
   auto_ptr<ToLocal> localGrabReactionForce;
   auto_ptr<ToLocal> localGrabReactionTorque;
   H3D::AutoRef<H3D::SpringEffect> grab_spring;
   H3D::AutoRef<H3D::Switch> grab_spring_switch;
   H3D::H3DFloat initial_grab_dist;
   auto_ptr<Inverter> contactReactionForce;
   vector<H3D::H3DFloat> stored_stiffnesses;
   H3D::NodeVector grabbed_surfaces;
   auto_ptr<GrabSurfaceControl> grabSurfaceControl;
};


}

#endif 

