#ifndef PILOTDEMO_H_
#define PILOTDEMO_H_

#if defined(TGT_HAS_LEGS) || defined(TGT_HAS_WHEELS)

#include <sstream>

#include "Behaviors/StateMachine.h"

using namespace std;
using namespace DualCoding;
//using namespace mathutils;

//! Drive the robot around manually and check its odometry
$nodeclass PilotDemo :  VisualRoutinesStateNode {

  $nodeclass PrintInstructions : StateNode : doStart {
    cout << endl;
    cout << "PilotDemo keyboard commands:" << endl;
    cout << "   f/b         move forward/back 100 mm" << endl;
    cout << "   F/B         move forward/back 500 mm" << endl;
    cout << "   fwd <mm>    move forward specified distance" << endl;
    cout << "   l/r         turn left/right 10 degrees" << endl;
    cout << "   L/R         turn left/right 90 degrees" << endl;
    cout << "   turn <deg>  turn by specified angle (positive turns left)" << endl;
#ifdef TGT_HAS_LEGS
    cout << "   strafe <mm> move sideways specified distance (positive moves left)" << endl;
#endif
    cout << "   loc         localize" << endl;
    cout << "   disp <n>    display n particles" << endl;
    cout << "   clear       clear world" << endl;
    cout << "   build       rebuild world map from vision" << endl;
    cout << "   randomize   randomize the particles (to solve the kidnapped robot problem)" << endl;
    cout << "   help        print these instructions" << endl;
    cout << "   q           quit (abort current operation)" << endl;
  }

  $nodeclass ClearWorldMap : VisualRoutinesStateNode : doStart {
    worldShS.deleteShapes<AprilTagData>();
    worldShS.deleteShapes<PolygonData>();
    cout << "World map cleared." << endl;
  }

  $nodeclass BuildWorldMap : MapBuilderNode($, MapBuilderRequest::worldMap) : doStart {
    mapreq.setAprilTagFamily();
    mapreq.clearVerbosity = -1U;
    mapreq.setVerbosity = MapBuilder::MBVimportShapes;
    cout << "Looking for shapes to add to world map." << endl;
  }

  $nodeclass Randomize : VisualRoutinesStateNode : doStart {
    particleFilter->resetFilter();
    cout << "Particles have been randomized." << endl;
  }

  $nodeclass ReportPosition : VisualRoutinesStateNode : doStart {
    AngTwoPi heading = theAgent->getOrientation();
    cout << "Agent now at " << theAgent->getCentroid() << " hdg " << heading
	 << " (= " << float(heading)*180/M_PI << " deg.)" << endl;
  }

  $nodeclass ParseCommand : StateNode : doStart {
     const TextMsgEvent *txtev = dynamic_cast<const TextMsgEvent*>(event);
     if ( txtev != NULL ) {
       std::istringstream is(txtev->getText());
       string cmd;
       float arg1 = 0;
       PilotRequest preq(PilotTypes::walk);
       is >> cmd >> arg1;
       cout << "Heard '" << cmd << "' and " << arg1 << endl;
       if ( cmd == "f" )
	 preq.dx = 100;
       else if ( cmd == "F" )
	 preq.dx = 500;
       else if ( cmd == "b" )
	 preq.dx = -100;
       else if ( cmd == "B" )
	 preq.dx = -500;
       else if ( cmd == "l" )
	 preq.da = M_PI/18;
       else if ( cmd == "L" )
	 preq.da = M_PI/2;
       else if ( cmd == "r" )
	 preq.da = -M_PI/18;
       else if ( cmd == "R" )
	 preq.da = -M_PI/2;
       else if ( cmd == "fwd" ) {
	 if ( is.fail() ) {
	   cout << "Missing or invalid argument to 'fwd' command" << endl;
	   postStateFailure();
	 }
	 else
	   preq.dx = arg1;
       }
       else if ( cmd == "turn" ) {
	 if ( is.fail() ) {
	   cout << "Missing or invalid argument to 'turn' command" << endl;
	   postStateFailure();
	 }
	 else
	   preq.da = arg1/180*M_PI;
       }
#ifdef TGT_HAS_LEGS
       else if ( cmd == "strafe" ) {
	 if ( is.fail() ) {
	   cout << "Missing or invalid argument to 'strafe' command" << endl;
	   postStateFailure();
	 }
	 else
	   preq.dy = arg1;
       }
#endif
       else if ( cmd == "loc" )
	 preq.requestType = PilotTypes::localize;
       else if ( cmd == "disp" ) {
	 if ( arg1 < 0 || arg1 != int(arg1) )
	   cout << "Error: argument to 'disp' must be a non-negative integer" << endl;
	 else {
	   VRmixin::particleFilter->displayParticles(arg1);
	   cout << "Displaying " << int(arg1) << " particles." << endl;
	 }
	 postStateFailure();
	 return;
       } else {
	 cout << "Unrecognized command: '" << cmd << "'" << endl;
	 postStateFailure();
	 return;
       }
       // completed command dispatch; now send the PilotRequest
       postStateSignal<PilotRequest>(preq);
     }
  }

  $nodeclass CommandInterpreter {
    $setupmachine {
      startnode: 
	PrintInstructions  =N=>
	    SpeechNode($, "Pilot Demo Ready")  =N=>  report

      report: StateNode =T(1000)=> ReportPosition =N=> waitForCommand

      waitForCommand: StateNode
      waitForCommand =TM("clear")=>     clearmap:    ClearWorldMap =N=> report
      waitForCommand =TM("build")=>     buildmap:    BuildWorldMap =MAP=> report
      waitForCommand =TM("randomize")=> random:      Randomize  =N=> report
      waitForCommand =TM("help")=> PrintInstructions =N=> waitForCommand
      waitForCommand =TM=> parse

      parse: ParseCommand
      parse =S<PilotRequest>=> doit: PilotNode =PILOT=> report
      parse =F=> waitForCommand

     {buildmap, doit} =TM("q")=> SpeechNode($,"quit") =N=> waitForCommand

    }
  }

  // This can be overridden if we subclass PilotDemo; the user will
  // then have to instantiate CommandInterpreter within their own
  // setup function.
  $setupmachine{
    CommandInterpreter
  }

  virtual void start() {
    VisualRoutinesStateNode::start();
    buildMap();
  }

  //! Users can override this method to set up a map of their choice.
  virtual void buildMap() {
    cout << "No map loaded.";
  }

  static std::string getClassDescription() {
    return "Demo Pilot functions by keyboard command.";
  }

}

#endif

#endif