#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
    };

  #shortnodeclass 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);

  #shortnodeclass Forward : WalkNode($, forwardFwdVel, 0, 0)

  #shortnodeclass VeerAway : WalkNode($, veerFwdVel, 0, veerAwayAngVel)

  #shortnodeclass VeerCloser : WalkNode($, veerFwdVel, 0, veerCloserAngVel)

  #shortnodeclass BackUpAndTurnLeft : WalkNode($, backupFwdVel, 0, backupAngVel)
			
  #nodemethod 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 =T(forwardTravelTime)=> check

	{veeraway, veercloser} =T(veerTravelTime)=> check

        {forward, veeraway, veercloser} =B(BumpRightButOffset)=> BackUpAndTurnLeft =T(backupTravelTime)=> forward
    #endstatemachine 
  #endnodemethod

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

#endnodeclass

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: ejt $
 * $Name:  $
 * $Revision: 1.1 $
 * $State: Exp $
 * $Date: 2010/02/18 02:20:52 $
 */