/*****************************
File:      Attractor.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 Attractor.h
/// \brief Header file for Attractor, attracts the haptic tool.

#ifndef Attractor_H
#define Attractor_H




// -----------------------------------------------------------------------------

#include "H3DNetworkingUtils/Config.h"
#include <H3D/X3DChildNode.h>
#include <H3DUtil/AutoRef.h>
#include <HAPI/HAPIForceEffect.h> 
#include <H3D/SFString.h> 
#include <H3D/SFInt32.h> 
#include <H3D/SFVec3f.h> 
#include <H3D/SFFloat.h> 
#include <H3D/SFBool.h> 
#include <H3D/TraverseInfo.h> 
#include <H3D/ThreadSafeFields.h> 
#include <H3DUtil/AutoRef.h> 

namespace H3D {
H3D_VALUE_EXCEPTION( const char *, invalidAttractorMode );
}

namespace H3DNetworkingUtils{
   
/// \class Attractor
/// The Attractor class provides a force toward a point.
/// The point is retrieved from the point field at graphics rates.
/// (typically you would use a RealtimeAttractor which extends this class to retrieve the point at haptics rates).
/// Features:
/// Can attract to a point or to an offset from a point, the offset being the position
/// the tool is at the time of becoming enabled.
/// Can work with any number of tools.
/// Smoothes the attraction forces.
/// Has a radius of attraction - no attraction outside the radius, sin(r) attraction inside.
/// Can limit the force applied.
/// Can limit the change in force applied.
/// Can be enabled locally, or remotely. 
/// This class works with any number of haptic devices.   \n
/// <b>Examples:</b>
///   - <a href="../../examples/AttractorTest.x3d">AtractorTest.x3d</a>
/// 
///

   class H3D_NETWORKING_UTILS_DLL_SPEC Attractor : public H3D::X3DChildNode {
public:
#define NEAREST_TOOL -1
#define NONE -2

   struct BoolOr : public H3D::SFBool {
      virtual void update() {
         for (u_int i = 0; i < routes_in.size(); i++) {
            if (static_cast<H3D::SFBool*>(routes_in[i])->getValue()) {
               value = true;
               return;
            }
         }
         value = false;
      }
   };
   Attractor(  H3D::Inst<H3D::ThreadSafeSField<H3D::SFVec3f> >  point = 0,
               H3D::Inst<H3D::SFBool>  withOffset = 0,
               H3D::Inst<H3D::SFFloat> radius = 0,
               H3D::Inst<H3D::SFFloat> strength = 0,
               H3D::Inst<BoolOr      >  enabled = 0,
               H3D::Inst<H3D::SFInt32> deviceIndex = 0,
               H3D::Inst<H3D::SFFloat> maxForce = 0,
               H3D::Inst<H3D::SFFloat> maxDeltaForce = 0,
               H3D::Inst<H3D::SFVec3f> offset = 0,
               H3D::Inst<H3D::SFBool>  repel = 0,
               H3D::Inst<H3D::SFInt32> activeDevice = 0,
               H3D::Inst<H3D::SFBool>  activeOnlyOnClick = 0,
               H3D::Inst<H3D::SFFloat> rampUpTime = 0,
               H3D::Inst<H3D::SFFloat> rampDownTime = 0);
   
   virtual void traverseSG(H3D::TraverseInfo & ti);

   static H3D::H3DNodeDatabase database;
   
   // public fields

   /// The global point of attraction \n
   /// access type: inputOutput \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
	auto_ptr<H3D::ThreadSafeSField<H3D::SFVec3f> > point;
   
   /// If true, the attraction is to an offset from the point value \n
   /// The offset is the value of the haptic device location when the attractor becomes active  \n
   /// access type: inputOutput \n
   /// basic type: SFBool \n
   /// default value: TRUE
   auto_ptr<H3D::SFBool>  withOffset;
   
   /// The radius of attraction from the point \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0.05
   auto_ptr<H3D::SFFloat> radius;
   
   /// The strength of attraction (newtons) \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value:  2.0
   auto_ptr<H3D::SFFloat> strength;
   
   /// Enable the force\n
   /// Any number of routes can come into this field\n
   /// It will be true
   /// if any of them are true - false if all are false \n
   /// access type: inputOutput=\n
   /// basic type: SFBool \n
   /// default value: FALSE
   auto_ptr<H3D::SFBool>  enabled;

   /// If "deviceIndex is NEAREST_DEVICE (-1), the nearest tool at the attraction  \n
   /// start time is attracted \n
   /// The attraction start time is either: \n
   ///   when the tool comes within radius of the point while enabled is true \n
   ///   or \n
   ///   when enabled become true while the tool is within the radius of the point \n
   /// access type: inputOutput \n
   /// basic type: SFInt32 \n
   /// default value: 0
   auto_ptr<H3D::SFInt32> deviceIndex;

   /// Limits maximum force (newtons) \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 5
   auto_ptr<H3D::SFFloat> maxForce;

   /// Limits maximum change of force (newtons/sec) \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 100
   auto_ptr<H3D::SFFloat> maxDeltaForce;

   /// If withOffset is true, this field holds the offset from the tool \n
   /// access type: outputOnly \n
   /// basic type: SFVec3f \n
   /// default value: 0 0 0
   auto_ptr<H3D::SFVec3f> offset;

   /// Change the force to repel \n
   /// access type: inputOutput \n
   /// basic type: SFBool \n
   /// default value: FALSE
   auto_ptr<H3D::SFBool>  repel;

   /// The device that is currently being attracted, -1 if none \n
   /// access type: inputOutput \n
   /// basic type: SFInt32 \n
   /// default value:  -1
   auto_ptr<H3D::SFInt32> activeDevice;

   /// If this is false, the attraction can become active (if already enabled) as soon as
   /// the device crosses the threshold to come within range (radius) of the attraction point \n
   /// If true, it becomes active only if enabled becomes true while within range \n
   /// access type: inputOutput \n
   /// basic type: SFBool \n
   /// default value: FALSE
   auto_ptr<H3D::SFBool>  activeOnlyOnClick;

   /// If rampUpTime is non-zero, the force ramps up over the designated time \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0
   auto_ptr<H3D::SFFloat> rampUpTime;

   /// If rampDownTime is non-zero, the force ramps down over the designated time \n
   /// access type: inputOutput \n
   /// basic type: SFFloat \n
   /// default value: 0
   auto_ptr<H3D::SFFloat> rampDownTime;


protected:

   virtual void setOffset(H3D::Vec3f const & offs) { offset->setValue(offs, id); }

   virtual H3D::Vec3f getPoint() {return point->getValue();}
   // getPoint() is provided to get the point in the
   // haptics loop. The default implementation retrives a copy of the point field
   // last read during the graphics loop. Overriding classes may allow the point
   // to change at faster rates (see RealtimeAttractor).

   // AttractForce is called by the H3D force mechanism in the haptics loop.
   // It calls AttractorForceFunc then smoothes it before returning it to H3D.
   class AttractForce : public HAPI::HAPIForceEffect {
   public:
   // Constructor
      AttractForce( H3D::H3DTime t, 
                     H3D::Vec3f const & _point,
                     bool _repel,
                     H3D::H3DFloat maxForce, 
                     H3D::H3DFloat maxDeltaForce,
                     H3D::H3DFloat _radius,
                     H3D::H3DFloat _scale,
							H3D::Vec3f const & offset,
							Attractor * attrP);

      AttractForce(AttractForce & that);

		virtual H3D::Vec3f getLatestAttractionPoint() {return attraction_point;}
		
		HAPI::HAPIForceEffect::EffectOutput virtual calculateForces( const HAPI::HAPIForceEffect::EffectInput &input );

      H3D::H3DFloat strength;

   protected:
      H3D::Vec3f smooth(H3D::Vec3f const & raw_force, H3D::TimeStamp const & deltaT);

		H3D::AutoRef<Attractor> attrP;
   private:
      H3D::H3DFloat maxForce;
      H3D::H3DFloat maxDeltaForce;
      H3D::H3DTime traverse_time;
      H3D::Vec3f attraction_point;
      H3D::H3DFloat radius;
      int currently_attracted_device;
      H3D::H3DFloat scale;
      H3D::Vec3f offset;
      bool repel;
      H3D::Vec3f prev_force; 
   };

	virtual AttractForce * createAttractForce(H3D::H3DTime t, 
														 	 H3D::Vec3f const & _point,
															 bool _repel,
															 H3D::H3DFloat maxForce, 
															 H3D::H3DFloat maxDeltaForce,
															 H3D::H3DFloat _radius,
															 H3D::H3DFloat _scale,
															 H3D::Vec3f const & offset);

   virtual H3D::H3DFloat getForce(H3D::TraverseInfo & ti, 
										    bool & inside, 
											 H3D::H3DTime now, 
											 H3D::Vec3f & tool_pos,
											 H3D::Vec3f const & current_attr_pt);

   int getNearestDevice(H3D::TraverseInfo & ti, 
								H3D::Vec3f & pos,
								H3D::Vec3f const & current_attr_pt);
   // get the closest device to pos.

   virtual H3D::H3DFloat calcDistSquared(H3D::Vec3f const & pt1, H3D::Vec3f const & pt2) {
		H3D::Vec3f x = pt1 - pt2; return x * x;
	}

   virtual H3D::H3DFloat calcForceStrength(H3D::Vec3f const & pos,
                                           bool & now_inside,
														 H3D::Vec3f const & current_attr_pt);

  enum Ramping {RAMP_UP, RAMP_DOWN, RAMP_NONE};

  struct RampIncrPerSec : public H3D::TypedField<H3D::SFFloat, H3D::Types<H3D::SFFloat, H3D::SFFloat> > {
     virtual void update() {
        float ramp_time = static_cast<H3D::SFFloat*>(routes_in[0])->getValue();
        float str = static_cast<H3D::SFFloat*>(routes_in[1])->getValue();
        if (ramp_time > 0.0f) {
           value = str / ramp_time;
        } else {
           value = str;
        }
      }
   };

   H3DUtil::MutexLock point_lock;
   H3D::Vec3f attract_pt;

private:
   struct TraverseData {
     H3D::H3DTime traverse_time;
     H3D::Vec3f last_force;
   };
   H3D::H3DFloat scale;
   TraverseData tool_data[4]; // index 0 is right hand, index 1 is left  hand tool.

   // traverseSG data
   int selected_device;
   bool just_enabled;
   bool inside;
   H3D::H3DTime ramp_time;
   bool ramping_up;
   bool ramping_down;
   H3D::H3DFloat force_strength;
   H3D::H3DFloat prev_force_strength;
   auto_ptr<RampIncrPerSec> ramp_up_incr_per_sec;
   auto_ptr<RampIncrPerSec> ramp_down_incr_per_sec;
   bool was_inside;
   bool was_enabled;
   H3D::H3DTime ramp_start_time;
   Ramping ramping;
   H3D::H3DFloat ramp_start_strength;

};

}

#endif

