/*****************************
File:       ToolPosDectector.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/ToolPosDetector.h"
#include <HAPI/HAPIHapticsDevice.h>
#include <iostream>

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

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

namespace ToolPosDetectorInternals {   
  FIELDDB_ELEMENT( ToolPosDetector, enabled,      INPUT_OUTPUT );
  FIELDDB_ELEMENT( ToolPosDetector, deviceIndex,      INPUT_OUTPUT );
  FIELDDB_ELEMENT( ToolPosDetector, localPos,         OUTPUT_ONLY );
  FIELDDB_ELEMENT( ToolPosDetector, globalPos,        OUTPUT_ONLY );
}

// ---------------------------------------------------------------
ToolPosDetector::ToolPosDetector (Inst< H3D::SFBool >  _enabled,
                                  Inst< H3D::SFInt32 >  _deviceIndex,
                                  Inst< ThreadSafeSFVec3f >  _globalPos,
                                  Inst< ThreadSafeSFVec3f >  _localPos) :
globalPos (_globalPos),
localPos (_localPos),
enabled (_enabled),
deviceIndex (_deviceIndex) {
///latest_detP (0) {

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

  enabled->setValue(false);
  globalPos->setValue(Vec3f(0,0,0), id);
  localPos->setValue(Vec3f(0,0,0), id);
  deviceIndex->setValue(0);
}
            
// ---------------------------------------------------------------
ToolPosDetector::Detector::Detector( ToolPosDetector * tpP,
                                     const Matrix4f & _global_to_local,
                                     int _node_id) :
tpdP (tpP),
global_to_local (_global_to_local),
node_id (_node_id),
creation_time (Scene::time->getValue()) {
}

// ----------------------------------------------------------------
HAPIForceEffect::EffectOutput ToolPosDetector::Detector::calculateForces(
   const EffectInput &input ) {
   // Multiple effects are in action at once (usually 2, one fading in and one fading out)
   // We only want the one to do the reporting - choose the newest one.
   if (creation_time > tpdP->latest_creation_time) {
      tpdP->latest_creation_time = creation_time;
   } else if (creation_time < tpdP->latest_creation_time) {
      return EffectOutput(Vec3f(0,0,0));  // Do nothing    
   }

   Vec3 pos = input.hd->getPosition(); // This is global
   Vec3f ppos((H3DFloat)pos.x, (H3DFloat)pos.y, (H3DFloat)pos.z);
  
   // DEBUG
   ///static bool on;
   ///static int count;
   ///static float dummy = 0.0f;
   ///static H3DTime prev = 0;
   ///dummy += 0.0001f;
   ///if (dummy > 0.5f)
   ///   dummy = 0.0f;
   ///ppos = Vec3f(dummy, 0, 0);
   ///H3DTime now = TimeStamp::now();
   ///H3DTime diff = now - prev;
   ///count++;
   ///if (diff >= 1.0) {
      ///cout << "ToolPosDetector frame rate = " << count << endl;
   ///   prev = now;
   ///   count = 0;
   ///}

   tpdP->globalPos->setValue(ppos, node_id);
   tpdP->localPos->setValue(global_to_local * ppos, node_id);

   return EffectOutput(Vec3f(0,0,0));      
}

// ---------------------------------------------------------------
void ToolPosDetector::traverseSG(TraverseInfo & ti) {
  X3DChildNode::traverseSG(ti);
  if (!enabled->getValue())
    return;
  
  unsigned int ind = deviceIndex->getValue();
  if (ti.hapticsEnabled() && (ind < ti.getHapticsDevices().size())) {
     ///latest_det_id = det_count++; 
     Detector * detectorP = new Detector(this, ti.getAccInverseMatrix(), id);

     ///// there may be more than one Detector current at the one time.
     ///// We only bother with the latest one
     ///latest_detP = detectorP;
     ti.addForceEffect( deviceIndex->getValue(), detectorP);
  } 

  ///if (det_count == 10000) {
  ///   det_count = 0;
  ///}
}
