#include "Shared/RobotInfo.h"
#ifdef TGT_IS_CREATE

#include "Behaviors/StateMachine.h"
using namespace DualCoding;

//! Wall following using the Create's buit-in IR wall sensor.  Min/max distance thresholds are chosen for a white wall (high reflectance).
$nodeclass CreateWallFollow(float minval=20.f, float maxval=150.f) :  VisualRoutinesStateNode {

  static float const forwardFwdVel = 50;  // in mm/sec
  static unsigned int const forwardTravelTime = 500;  // in msec

  static float const veerFwdVel = 20;  // in mm/sec
  static float const veerAwayAngVel = 8*M_PI/180;  // deg/sec converted to rad/sec
  static float const veerCloserAngVel = -8*M_PI/180;  // deg/sec converted to rad/sec
  static unsigned int const veerTravelTime = 800;  // in msec

  static float const backupFwdVel = -40; // in mm/sec
  static float const backupAngVel = 50*M_PI/180; // deg/sec converted to rad/sec
  static unsigned int const backupTravelTime = 1250; // in msec

  enum IRsignal {
      goodDistance,
      tooClose,
      tooFar
    };

  $nodeclass CheckIR(float minval, float maxval) : StateNode : doStart {
    float wallReading = state->sensors[WallSignalOffset];
    if ( wallReading > maxval )
      postStateSignal<IRsignal>(tooClose);
    else if ( wallReading < minval )
      postStateSignal<IRsignal>(tooFar);
    else
      postStateSignal<IRsignal>(goodDistance);
  }

  $nodeclass Forward : WalkNode($, forwardFwdVel, 0, 0, forwardTravelTime, WalkNode::VEL) {}

  $nodeclass VeerAway : WalkNode($, veerFwdVel, 0, veerAwayAngVel, veerTravelTime, WalkNode::VEL) {}

  $nodeclass VeerCloser : WalkNode($, veerFwdVel, 0, veerCloserAngVel, veerTravelTime, WalkNode::VEL) {}

  $nodeclass BackUpAndTurnLeft : WalkNode($, backupFwdVel, 0, backupAngVel, backupTravelTime, WalkNode::VEL) {}
			
  virtual void setup() {
    $statemachine {
      startnode: SpeechNode($,"Starting Wall Follow Demo") =C=> check

      check: CheckIR($, minval, maxval)
      check =S<IRsignal>(goodDistance)=> forward: Forward
      check =S<IRsignal>(tooClose)=> veeraway: VeerAway
      check =S<IRsignal>(tooFar)=> veercloser: VeerCloser

      {forward, veeraway, veercloser} =C(1)=> check

      {forward, veeraway, veercloser} =B(BumpRightButOffset)=>
         BackUpAndTurnLeft =C=> forward
    }
  }

  virtual std::string getDescription() const {
    return "Follows a right-side wall using the Create's IR wall sensor"; }

}

REGISTER_BEHAVIOR_MENU(CreateWallFollow,DEFAULT_TK_MENU"/Navigation Demos");

#endif

/*! @file
 * @brief Wall-following demo using the Create's IR wall sensor
 * @author Owen Watson (Creator) and Dave Touretzky
 *
 * $Author$
 * $Name$
 * $Revision$
 * $State$
 * $Date$
 */