/*****************************
File:      GrabableDynamic.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/GrabableDynamic.h"
#include <H3D/Node.h>
#include <H3D/Shape.h>
#include <H3D/H3DStiffnessSurfaceNode.h>
#include <H3D/X3DGeometryNode.h>

using namespace H3D;
using namespace H3DNetworkingUtils;


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

namespace GrabableDynamicInternals {   
  FIELDDB_ELEMENT( GrabableDynamic, grabbed,             INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( GrabableDynamic, grabPoint,           INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( GrabableDynamic, grabStrength,        INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( GrabableDynamic, grabSlackRadius,     INPUT_OUTPUT ); 
  FIELDDB_ELEMENT( GrabableDynamic, grabForce,           OUTPUT_ONLY); 
  FIELDDB_ELEMENT( GrabableDynamic, grabReactionForce,   OUTPUT_ONLY); 
  FIELDDB_ELEMENT( GrabableDynamic, grabSpringAnchorPt,  OUTPUT_ONLY); 
  FIELDDB_ELEMENT( GrabableDynamic, grabTorque,          OUTPUT_ONLY); 
  FIELDDB_ELEMENT( GrabableDynamic, grabForceRampCycles, INPUT_OUTPUT); 
  FIELDDB_ELEMENT( GrabableDynamic, grabDamping,         INPUT_OUTPUT); 
  FIELDDB_ELEMENT( GrabableDynamic, slaveMode,				INPUT_OUTPUT );
  FIELDDB_ELEMENT( GrabableDynamic, appliedForce,			OUTPUT_ONLY );
  FIELDDB_ELEMENT( GrabableDynamic, appliedTorque,			OUTPUT_ONLY );
  FIELDDB_ELEMENT( GrabableDynamic, smoothing,	   		INPUT_OUTPUT );
  FIELDDB_ELEMENT( GrabableDynamic, noSurfaceOnGrab,	   		INPUT_OUTPUT );
}


// ------------------------------------------------
GrabableDynamic::GrabableDynamic(
                  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       > _grabForceRampCycles,
						Inst<H3D::SFFloat       > _grabDamping,
                  Inst<SlaveSwitch        > _slaveMode,
                  Inst<SumVec3f           > _appliedForce,
                  Inst<SumVec3f           > _appliedTorque,
                  Inst<SFFloat            > _smoothing,
                  Inst<SFBool             > _noSurfaceOnGrab) :
CollidableDynamic (_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 (_grabbed),
grabPoint (_grabPoint),
grabStrength (_grabStrength),
grabSlackRadius (_grabSlackRadius),
grabForce (_grabForce),
grabReactionForce (_grabReactionForce),
grabSpringAnchorPt (_grabSpringAnchorPt),
grabTorque (_grabTorque),
grabForceRampCycles (_grabForceRampCycles),
grabDamping (_grabDamping),
slaveMode (_slaveMode),
appliedForce (_appliedForce),
appliedTorque (_appliedTorque),
smoothing (_smoothing),
first_slave_setting (true),
localGrabReactionForce (new ToLocal),
localGrabReactionTorque (new ToLocal),
grab_spring (new SpringEffect),
grab_spring_switch (new Switch),
contactReactionForce (new Inverter),
noSurfaceOnGrab (_noSurfaceOnGrab),
grabSurfaceControl (new GrabSurfaceControl)
{

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

   slaveMode->setOwner(this);
   grabForce->setOwner(this);
   grabTorque->setOwner(this);
   localGrabReactionForce->setOwner(this);
   localGrabReactionTorque->setOwner(this);

	
	grabForce->setValue(Vec3f(0.0f, 0.0f, 0.0f), id);
   grabReactionForce->setValue(Vec3f(0.0f, 0.0f, 0.0f), id);
   grabbed->setValue(false);
   grabPoint->setValue(Vec3f(0.0f, 0.0f, 0.0f));
   grabStrength->setValue(60.0f);
   grabSlackRadius->setValue(0.004f);
   grabForceRampCycles->setValue(10);
   grabDamping->setValue(0.01f);

   grabbed->routeNoEvent(grabSpringAnchorPt, id);
   grabPoint->routeNoEvent(grabSpringAnchorPt, id);
   position->routeNoEvent(grabSpringAnchorPt, id);
   orientation->routeNoEvent(grabSpringAnchorPt, id);

   grabbed->routeNoEvent(grabForce, id);
   grabSpringAnchorPt->routeNoEvent(grabForce, id);
   grabPoint->routeNoEvent(grabForce, id);
   grabStrength->routeNoEvent(grabForce, id);
   grabForceRampCycles->routeNoEvent(grabForce, id);
	grabForce->route(grabReactionForce, id);
	grabReactionForce->route(localGrabReactionForce, id);
	localGrabReactionForce->route(externalForces, id);

   grabbed->routeNoEvent(grabTorque, id);
   position->routeNoEvent(grabTorque, id);
   grabSpringAnchorPt->routeNoEvent(grabTorque, id);
   grabReactionForce->route(grabTorque, id);
   grabSlackRadius->routeNoEvent(grabTorque, id);
	grabTorque->route(localGrabReactionTorque, id);
	localGrabReactionTorque->route(externalTorques, id);
	/// DEBUG grabDamping->route(grab_spring->damping, id);

	// This adds user forces to the reported applied force and torque
   totalContactForce->route(appliedForce, id);
   //contactReactionForce->route(appliedForce, id);
   localGrabReactionForce->route(appliedForce, id);
   totalContactTorque->route(appliedTorque, id);
   grabTorque->route(appliedTorque, id);

	slaveMode->setValue(false);
   grab_spring->springConstant->setValue(0.0f);
   grab_spring->startDistance->setValue(1.0f);
   grab_spring->escapeDistance->setValue(1.1f);
   grab_spring->positionInterpolation->setValue(0.6f);
   grab_spring_switch->children->push_back(grab_spring.get());
   smoothing->route(grab_spring->positionInterpolation);
   smoothing->setValue(0.1f);
   children->push_back(grab_spring_switch.get()); 
   grab_spring_switch->whichChoice->setValue(-1);

   grabbed->route(grabSurfaceControl);
   noSurfaceOnGrab->route(grabSurfaceControl);
   noSurfaceOnGrab->setValue(true);

   // DEBUG
   //children->push_back(grab_spring.get()); 
   //SpringEffect * dbg = new SpringEffect;
   //dbg->startDistance->setValue(0.5);
   //dbg->escapeDistance->setValue(0.6);
   //children->push_back(dbg);
}

// ------------------------------------------------
void GrabableDynamic::initialize() {
	CollidableDynamic::initialize();
}

// ------------------------------------------------
// Return closest point in local coords. Also sets closest_point_dist in local coords
Vec3f GrabableDynamic::getClosestPoint(const Vec3f & pt, H3DFloat & closest_point_dist) {
   closest_point_dist = 999999.0f;
   Vec3f closest(0.0f,0.0f,0.0f);
   const NodeVector &c = geometries->getValue();
   Vec3f local_pt = accumulatedInverse->getValue() * pt;
   for( unsigned int i = 0; i < c.size(); i++ ) {
      if( c[i] ) {
         X3DGeometryNode * geom = dynamic_cast< X3DGeometryNode* >( c[i] );
         if (geom) {
            NodeIntersectResult result;
            geom->closestPoint(local_pt, result);
            HAPI::Vec3 p = result.result.front().point;
            Vec3f near_pt((float)p.x, (float)p.y, (float)p.z);
            H3DFloat dist = (near_pt - local_pt).length();
            if (dist < closest_point_dist) {
               closest_point_dist = dist;
               closest = near_pt;
            }
         }
      }
   }
   return closest;
}

// Provides the position of the current (perhaps moved) position of the initial point on the
// object that was grabbed, in global coords
// input 0 is a 'grabbed' boolean, 
// input 1 is the current grabPoint, 
// input 2 is the position of the object,
// input 3 is the orientation of the object
// --------------------------------------------
void GrabableDynamic::MovedPos::update() {
   bool grab = static_cast<SFBool*>(routes_in[0])->getValue();
   if (grab) {
      Vec3f grab_pt = static_cast<SFVec3f*>(routes_in[1])->getValue(); // global
      Vec3f body_pos = static_cast<SFVec3f*>(routes_in[2])->getValue(); // of this DynamicTransform
      Rotation body_rot = static_cast<SFRotation*>(routes_in[3])->getValue(); // of this DynamicTransform
      GrabableDynamic * dynP = static_cast<GrabableDynamic*>(getOwner());
      if (!was_grabbed) {
         // First grab
         was_grabbed = true;
         Vec3f initial_grab_pt = grab_pt;
         initial_anchor_local = dynP->getClosestPoint(grab_pt, dynP->initial_grab_dist); // local
         initial_position = body_pos;
         initial_rotation = body_rot;
      } 

      /////Rotation extra_rot = body_rot * (-initial_rotation);
      ///Vec3f rotated_grab_spring_anchor_pt_local = extra_rot * initial_anchor_local;
      ///Vec3f rotated_grab_spring_anchor_pt_global = bodyP->accumulatedForward->getValue() * rotated_grab_spring_anchor_pt_local;
      ///value = rotated_grab_spring_anchor_pt_global; // i.e. moved grab pt
      value = dynP->accumulatedForward->getValue() * initial_anchor_local; // global
   } else {
      was_grabbed = false;
   }
}

// -------------------------------------------------
void GrabableDynamic::GrabTorque::update() {
   GrabableDynamic * dynP = static_cast<GrabableDynamic*>(getOwner());
   bool grabbed = static_cast<SFBool*>(routes_in[0])->getValue();
   if (!grabbed) {
		value = Vec3f(0.0f, 0.0f, 0.0f);
      return;
   }

   // work in local because it may be within outer transforms
   ///Vec3f body_centre_pos = static_cast<SFVec3f*>(routes_in[1])->getValue(); // global
   Vec3f anchor = static_cast<SFVec3f*>(routes_in[2])->getValue(); // global
   Vec3f local_anchor = dynP->accumulatedInverse->getValue() * anchor; // local
   Vec3f local_body_centre_pos = Vec3f(0,0,0); // local
   Vec3f global_body_centre_pos = dynP->accumulatedForward->getValue() * local_body_centre_pos; // global
   Vec3f global_fce = static_cast<SFVec3f*>(routes_in[3])->getValue();
   H3DFloat slack = static_cast<SFFloat*>(routes_in[4])->getValue();

   Vec3f global_lever = anchor - global_body_centre_pos;

   Vec3f global_torque = global_lever % global_fce;
   H3DFloat len = global_torque.length();
   if (len > slack) {
      value = global_torque;
   } else {
      value = Vec3f(0.0f,0.0f,0.0f);
   }
   ///value =  dynP->accumulatedForward->getValue() * value;  // Returned in global coords

}

// Input 0 is grabbed, /n 
// Input 1 is grabSpringAnchorPoint,  /n
// Input 2 is current grabPoint,  /n
// Input 3 is grabStrength,  /n
// Input 4 is grabForceRampCycles,  /n
// -------------------------------------------------
void GrabableDynamic::GrabForce::update() {
   GrabableDynamic * dynP = static_cast<GrabableDynamic*>(getOwner());
   ///if (dynP->slaveMode->getValue()) {
   ///   value = Vec3f(0,0,0);
///return;
  /// }
   
   bool grabbed = static_cast<SFBool*>(routes_in[0])->getValue();
   Vec3f anchor = static_cast<SFVec3f*>(routes_in[1])->getValue();
   Vec3f grab_pt = static_cast<SFVec3f*>(routes_in[2])->getValue();
   H3DFloat max_spring_const = static_cast<SFFloat*>(routes_in[3])->getValue();
   int cycles = static_cast<SFInt32*>(routes_in[4])->getValue();
   H3DTime now = Scene::time->getValue();
   H3DTime deltaT = now - prev_time;
   prev_time = now;

   bool just_grabbed = false;
   if (grabbed) {
      if (was_grabbed) {
         if (spring_constant <= max_spring_const) {
            spring_constant += max_spring_const / cycles;
         }
      } else {
         just_grabbed = was_grabbed = true;
         spring_constant = 0.0f;
         dynP->grab_spring_switch->whichChoice->setValue(0);
      }
   } else {
      spring_constant = 0.0f;
      prev_connection_spring = Vec3f(0,0,0);
      was_grabbed = false;
      dynP->grab_spring_switch->whichChoice->setValue(-1);
   }
   
   dynP->grab_spring->springConstant->setValue(spring_constant, dynP->id);
   
   if (grabbed && dynP->grab_spring->force->size() > 0) {
      // The next step is to move the spring anchor to be the same distance out from the object anchor point
      // that was the case when it was first grabbed. This stops a sudden pull in towards the object when first grabbed.
      Vec3f local_anchor = dynP->accumulatedInverse->getValue() * anchor;
      Vec3f local_grab_pt = dynP->accumulatedInverse->getValue() * grab_pt;
      Vec3f connecting_line = local_grab_pt - local_anchor;
      connecting_line.normalizeSafe();
      connecting_line *= dynP->initial_grab_dist;
      Vec3f spring_anchor(local_anchor + connecting_line);

      dynP->grab_spring->position->setValue(spring_anchor, dynP->id);

      value = dynP->grab_spring->force->getValueByIndex(0);
   } else {
      value = Vec3f(0,0,0);
   }
}

// -------------------------------------------------
void GrabableDynamic::setSlaveMode(bool v) {
   // These two lines make this a slave dynamic.
	if (v) {
		orig_freedom = freedom->getValue();
		orig_angular_freedom = angularFreedom->getValue();
		freedom->setValue(Vec3f(0.0f, 0.0f, 0.0f));
		angularFreedom->setValue(Vec3f(0.0f, 0.0f, 0.0f));
      if (localGrabReactionForce->routesTo(externalForces.get())) {
         localGrabReactionForce->unroute(externalForces);
      }
      if (localGrabReactionTorque->routesTo(externalTorques.get())) {
         localGrabReactionTorque->unroute(externalTorques);
      }
	} else {
		if (first_slave_setting) {
		   orig_freedom = freedom->getValue();
		   orig_angular_freedom = angularFreedom->getValue();
			// Do nothing else
		} else {
			freedom->setValue(orig_freedom);
			angularFreedom->setValue(orig_angular_freedom);
		}
      if (!localGrabReactionForce->routesTo(externalForces.get())) {
         localGrabReactionForce->route(externalForces);
      }
      if (!localGrabReactionTorque->routesTo(externalTorques.get())) {
         localGrabReactionTorque->route(externalTorques);
      }
	}
	first_slave_setting = false;
}

// -------------------------------------------------
void GrabableDynamic::setSurfaces(bool on) {
  if (on) {
     for (unsigned int i = 0; i < grabbed_surfaces.size(); i++) {
        static_cast<H3DStiffnessSurfaceNode*>(grabbed_surfaces[i])->stiffness->setValue(stored_stiffnesses[i]);
     }
     grabbed_surfaces.clear();
     stored_stiffnesses.clear();
  } else {
     for (unsigned int i = 0; i < children->size(); i++) {
        X3DChildNode * chP = children->getValueByIndex(i);
        Shape * shP = dynamic_cast<Shape*>(chP);
        if (shP) {
           X3DAppearanceNode * appP = shP->appearance->getValue();
           if (appP) {
              H3DStiffnessSurfaceNode * surfP = dynamic_cast<H3DStiffnessSurfaceNode*>(appP->surface->getValue());
              if (surfP) {
                 H3DFloat stiffness = surfP->stiffness->getValue();
                 surfP->stiffness->setValue(0);
                 grabbed_surfaces.push_back(surfP);
                 stored_stiffnesses.push_back(stiffness);
              }
           }
        }
     }
  }
}