//-*-c++-*-

/****************************************************************

Sample code for running a Create around and reporting pink blobs.
This is the first bit of demo code to help people get started
using Tekkotsu and the Create for the Tapia competition.

  David S. Touretzky
  November, 2008
****************************************************************/

#ifndef _TapiaWander_h_
#define _TapiaWander_h_

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Events/DataEvent.h"

using namespace std;
using namespace DualCoding;

class LookForTarget : public VisualRoutinesStateNode {
public:
  LookForTarget(const std::string &name) : VisualRoutinesStateNode(name) {}

  virtual void DoStart() {
    MapBuilderRequest req(MapBuilderRequest::cameraMap);
    int const color_pink = ProjectInterface::getColorIndex("pink");
    req.objectColors[ellipseDataType].insert(color_pink);
    req.immediateRequest = true;
    mapBuilder.executeRequest(req);

    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
    if ( ellipses.empty() ) {
      postStateCompletion();
      return;
    }
    cout << "I saw " << ellipses[0] << endl;
    postStateSignal<Shape<EllipseData> >(ellipses[0]);
  }
};

class TapiaWander : public VisualRoutinesStateNode {
public:
  TapiaWander() : VisualRoutinesStateNode("TapiaWander") {}

  virtual void setup() {
#statemachine

  startnode: LookForTarget

  // If no pink ellipse found (completion event) , walk forward
  startnode =C[setSound("fart.wav")]=> walk

  // If we did find an ellipse, anounce it, then turn around and go the other way
  startnode =S<Shape<EllipseData> >=> SoundNode($,"ping.wav") =T(2000)=>
    aboutFace: WalkNode($, 0, 0, M_PI, 1) =C=> startnode

  // Walk section: go forward 200 mm, but if we hit something, back up and turn.
  // Bump buttons are defined in Shared/CreateInfo.h
  walk: WalkNode($, 200, 0, 0, 1) =C=> startnode

  walk =B(BumpLeftButOffset,activateETID)[setSound("crash.wav")]=>
    lbump: WalkNode($, -50, 0, -M_PI/3, 1) =C=> startnode

  walk =B(BumpRightButOffset,activateETID)[setSound("crash.wav")]=>
    rbump: WalkNode($, -50, 0, M_PI/3, 1) =C=> startnode

#endstatemachine
  }
};

#endif