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


// -----------------------------------------------------------------------------
// (See Attractor.h for details)
// -----------------------------------------------------------------------------

#include "H3DNetworkingUtils/Attractor.h"
#include "H3D/H3DHapticsDevice.h"

using namespace H3D;
using namespace HAPI;
using namespace H3DNetworkingUtils;

// -----------------------------------------------------------------------------
H3DNodeDatabase Attractor::database(   
  "Attractor", 
  &(newInstance<Attractor>),
  typeid(Attractor),
  &X3DChildNode::database);

namespace AttractorInternals {   
  FIELDDB_ELEMENT( Attractor, point,            INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, withOffset,       INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, radius,           INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, strength,         INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, enabled,          INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, deviceIndex,      INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, maxForce,         INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, maxDeltaForce,    INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, repel,            INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, offset,           OUTPUT_ONLY );
  FIELDDB_ELEMENT( Attractor, activeDevice,     OUTPUT_ONLY );
  FIELDDB_ELEMENT( Attractor, activeOnlyOnClick,INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, rampUpTime,       INPUT_OUTPUT );
  FIELDDB_ELEMENT( Attractor, rampDownTime,     INPUT_OUTPUT );
}


Attractor::Attractor(Inst<ThreadSafeSField<SFVec3f> >_point,
                     Inst<SFBool>  _withOffset,
                     Inst<SFFloat> _radius,
                     Inst<SFFloat> _strength,
                     Inst<BoolOr > _enabled,
                     Inst<SFInt32> _deviceIndex,
                     Inst<SFFloat> _maxForce,
                     Inst<SFFloat> _maxDeltaForce,
                     Inst<SFVec3f> _offset,
                     Inst<SFBool>  _repel,
                     Inst<SFInt32> _activeDevice,
                     Inst<SFBool>  _activeOnlyOnClick,
                     Inst<SFFloat>  _rampUpTime,
                     Inst<SFFloat>  _rampDownTime):
point(_point),
withOffset (_withOffset),
radius(_radius),
strength(_strength),
enabled (_enabled),
deviceIndex (_deviceIndex),
maxForce(_maxForce),
maxDeltaForce(_maxDeltaForce),
offset (_offset),
repel (_repel),
activeDevice(_activeDevice),
activeOnlyOnClick (_activeOnlyOnClick),
rampUpTime          (_rampUpTime),
rampDownTime          (_rampDownTime),
attract_pt      (0.0,0.0,0.0),
scale           (1.0),
selected_device (NONE),
inside (false),
force_strength (0.0f),
prev_force_strength (0.0f),
ramp_up_incr_per_sec (new RampIncrPerSec) ,
ramp_down_incr_per_sec (new RampIncrPerSec),
was_inside(false),
was_enabled(false),
ramp_start_time(0),
ramping(RAMP_NONE) {

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

  tool_data[0].traverse_time = TimeStamp::now();
  tool_data[0].last_force = Vec3f(0,0,0);
  tool_data[1].traverse_time = TimeStamp::now();
  tool_data[1].last_force = Vec3f(0,0,0);

  point->setValue(Vec3f(0,0,0));
  radius->setValue(0.05f);
  strength->setValue(2.0f);
  activeDevice->setValue(NONE, id);
  deviceIndex->setValue(0);
  maxForce->setValue(5.0);
  maxDeltaForce->setValue(100.0);
  withOffset->setValue(true);
  offset->setValue(Vec3f(0,0,0), id);
  enabled->setValue(false);
  repel->setValue(false);
  activeOnlyOnClick->setValue(false);
  rampUpTime->setValue(0.0f);
  rampDownTime->setValue(0.0f);

  rampUpTime->route(ramp_up_incr_per_sec);
  strength->route(ramp_up_incr_per_sec);
  rampDownTime->route(ramp_down_incr_per_sec);
  strength->route(ramp_down_incr_per_sec);

}

// ------------------------------------------------------------
void Attractor::traverseSG(TraverseInfo & ti) {

   X3DChildNode::traverseSG(ti);
   
   H3DTime now = TimeStamp::now();

   prev_force_strength = force_strength;
   force_strength = 0;

   if (enabled->getValue()) { 
		if (!was_enabled) { 
         was_enabled = true;
         ramp_start_time = now;
         ramp_start_strength = prev_force_strength;
         ramping = RAMP_UP;
         was_inside = false;
      }
   } else {
      if (was_enabled) {
         was_enabled = false;
         ramp_start_time = now;
         ramp_start_strength = prev_force_strength;
         ramping = RAMP_DOWN;
      }
   }

   Vec3f tool_pos;
	Vec3f current_attr_pt = getPoint();

	if (enabled->getValue()) {
      force_strength = getForce(ti, inside, now, tool_pos, current_attr_pt); 
      // Returns any force, sets the selected device, and sets inside if it is within range.
		if (selected_device != NONE) {
         activeDevice->setValue(selected_device, id); // We set it to NONE only after ramp_down
      }

      if (inside & !was_inside) {
         ramp_start_time = now;
         ramp_start_strength = prev_force_strength;
         was_inside = true;
         ramping = RAMP_UP;
         Vec3f o(0.0f,0.0f,0.0f);
         if (withOffset->getValue()) {
            o = tool_pos - current_attr_pt; 
         }   
         setOffset(o);
      } else if (!inside && was_inside) {
         ramp_start_time = now;
         ramp_start_strength = prev_force_strength;
         was_inside = false;
         ramping = RAMP_DOWN;
      } else if ((ramping == RAMP_UP) && !inside) {
         ramping = RAMP_NONE;
      }
   }
   H3DTime deltaT = now - ramp_start_time;

   if (enabled->getValue()) {
      if (inside) {
         if (ramping == RAMP_UP) {
            if (rampUpTime->getValue() == 0.0f) {
               force_strength = strength->getValue();
            } else {
               force_strength = ramp_start_strength + ramp_up_incr_per_sec->getValue() * (H3DFloat)deltaT;
            }
            if (force_strength >= strength->getValue()) {
               ramping = RAMP_NONE;
               force_strength = strength->getValue();
            } 
         }
      } else { // !inside
         selected_device = NONE;
         if (ramping == RAMP_DOWN) {
            if (rampDownTime->getValue() == 0.0f) {
               force_strength = 0.0f;
            } else {
               force_strength = ramp_start_strength - ramp_down_incr_per_sec->getValue() * (H3DFloat)deltaT;
            }
            if (force_strength <= 0.0f) {
               force_strength = 0.0f;
               ramping = RAMP_NONE;
               setOffset(Vec3f(0,0,0));
               activeDevice->setValue(NONE, id);
            } 
         }
      }
   } else { // !enabled 
      ///if (inside) {
         if (ramping == RAMP_DOWN) {
            if (rampDownTime->getValue() == 0.0f) {
               force_strength = 0.0f;
               setOffset(Vec3f(0,0,0));
            } else {
               force_strength = ramp_start_strength - ramp_down_incr_per_sec->getValue() * (H3DFloat)deltaT;
            }
            if (force_strength <= 0.0f) {
               force_strength = 0.0f;
               ramping = RAMP_NONE;
               setOffset(Vec3f(0,0,0));
               inside = false;
               activeDevice->setValue(NONE, id);
            } 
         }
      ///}
   }

   if (inside || (ramping == RAMP_DOWN)) {
     AttractForce * at_force_P = createAttractForce(now,  
                                             point->getValue(), repel->getValue(),
                                             maxForce->getValue(), maxDeltaForce->getValue(),
                                             radius->getValue(), scale, offset->getValue());
     at_force_P->strength = force_strength;
     ti.addForceEffect( activeDevice->getValue(), at_force_P ); 
   }
}

// ------------------------------------------------------------
H3DFloat Attractor::getForce(TraverseInfo & ti, bool & active, H3DTime now, Vec3f & tool_pos,
											 H3D::Vec3f const & current_attr_pt) {
   selected_device = deviceIndex->getValue();
   if (selected_device == NEAREST_TOOL) {
      selected_device = getNearestDevice(ti, tool_pos, current_attr_pt);
   } 
   if (selected_device < 0) {
      return 0; // Shouldn't happen
   }

	tool_data[selected_device].traverse_time = now;
   scale = ti.getAccInverseMatrix().getScalePart().x; 

   tool_pos = ti.getHapticsDevices()[selected_device]->weightedProxyPosition->getValue();   
   return calcForceStrength(tool_pos, active, current_attr_pt);
}

// ------------------------------------------------------------
int Attractor::getNearestDevice(TraverseInfo & ti, Vec3f & tool_pos,
											 H3D::Vec3f const & current_attr_pt) {
   int i = 0;
   H3DFloat closest_dist = 999999.0f;
   int tool_index = -1;
   
   const vector< H3DHapticsDevice *> &haptics_devices = ti.getHapticsDevices();
   for( vector< H3DHapticsDevice *>::const_iterator hd = haptics_devices.begin();
        hd != haptics_devices.end();
        hd++, i++ ) {
      tool_pos = (*hd)->devicePosition->getValue();
      H3DFloat dist = calcDistSquared(tool_pos, current_attr_pt);
      if (dist < closest_dist) {
         closest_dist = dist;
         tool_index = i;
      }
   }

  return tool_index;
}

// ------------------------------------------------------------
H3DFloat Attractor::calcForceStrength(Vec3f const & tool_pos, bool & active,
											 H3D::Vec3f const & current_attr_pt) {
  H3DFloat mag = 0.0;
  Vec3f x = tool_pos - current_attr_pt;
  H3DFloat r = x.length();

  if (r <= radius->getValue()) {
	 mag = strength->getValue();
    active = true;
  } else {
    mag = 0.0;    // Zero force
    active = false;
  }
  return mag;
}

// ------------------------------------------------------------
Attractor::AttractForce * Attractor::createAttractForce(H3DTime t, 
										 					  Vec3f const & point,
															  bool repel,
															  H3DFloat maxForce, 
															  H3DFloat maxDeltaForce,
															  H3DFloat radius,
															  H3DFloat scale,
															  Vec3f const & offset) {
	return new AttractForce(t, point, repel,
                           maxForce, maxDeltaForce,
                           radius, scale, offset, this);
}

// ------------------------------------------------------------
Attractor::AttractForce::AttractForce( H3DTime t, 
                                       Vec3f const & _point,
                                       bool _repel,
                                       H3DFloat _maxForce, 
                                       H3DFloat _maxDeltaForce,
                                       H3DFloat _radius,
                                       H3DFloat _scale,
													Vec3f const & _offset,
													Attractor * attractorP) :
maxForce(_maxForce),
maxDeltaForce(_maxDeltaForce),
traverse_time(t),
attraction_point (_point),
radius (_radius),
strength (0.0),
scale     (_scale),
repel (_repel),
prev_force (0.0f, 0.0f, 0.0f),
offset (_offset),
attrP (attractorP) {
}


// ------------------------------------------------------------
Attractor::AttractForce::AttractForce(AttractForce & that):
maxForce(that.maxForce),
maxDeltaForce(that.maxDeltaForce),
traverse_time(that.traverse_time),
attraction_point (that.attraction_point),
radius (that.radius),
strength (that.strength),
scale      (that.scale),
prev_force (that.prev_force),
offset (that.offset),
attrP (that.attrP) {
}

// ------------------------------------------------------------
Vec3f Attractor::AttractForce::smooth(Vec3f const & raw_force, TimeStamp const & deltaT) {
   Vec3f F = raw_force;


   Vec3f dF = F - prev_force;
   H3DFloat dFdt = (dF.length()) / (float)deltaT;

   if (dFdt > maxDeltaForce) {
      dF.normalize();
      dF *= (maxDeltaForce * deltaT);
      //dF.setLength(maxDeltaForce * deltaT);
      F = prev_force + dF;
   }

   if (F.length() > maxForce) {
      F.normalize();  
      F *= maxForce;
      //F.setLength(maxForce); 
   }

   return F;
}

// ------------------------------------------------------------
HAPIForceEffect::EffectOutput Attractor::AttractForce::calculateForces(
                                                const EffectInput &input ) {
   Vec3f F(0,0,0);

   Vec3d attr_pt = (Vec3d)((1.0 / scale) * getLatestAttractionPoint() + offset);

   Vec3d x = attr_pt - input.hd->getPosition();
   double r = x.length();

   if (strength > 0.0) {
     ///if ((r > H3DUtil::Constants::d_epsilon) && (r <= (double)radius)) {
       H3DFloat t = (float)(r / (double)radius);
       H3DFloat mag = (H3DFloat)sin(t * H3DUtil::Constants::pi) * strength;
       if (mag < 0.0)
         mag = 0.0;
       F = Vec3f((H3DFloat)x.x, (H3DFloat)x.y, (H3DFloat)x.z); // Force direction is x
       F.normalizeSafe();
       F *= mag;
     ///}
   }
   if (repel) {
     F = -F;
   }
   F = smooth(F, input.deltaT);
   prev_force = F;

   return EffectOutput(F); 
}

//H3D::Vec3f Attractor::getAttractionPoint() {
//   Vec3f pt = point->getValue();
//   Vec3f off = offset->getValue();
//   Vec3f point = pt + off;
//   return point;
//} 
