#ifndef _Lab6a_h_
#define _Lab6a_h_

#include "Sound/SoundManager.h"
#include "DualCoding/DualCoding.h"

using namespace std;
using namespace DualCoding;

class Lab6a : public VisualRoutinesBehavior {
public:
  Lab6a() : VisualRoutinesBehavior("Lab6a") {}

  void DoStart() {
    VisualRoutinesBehavior::DoStart();

    const int orange_index = ProjectInterface::getColorIndex("orange");
    const int green_index = ProjectInterface::getColorIndex("green");

    // Set up a pre-specified world map
    NEW_SHAPE(orange1,EllipseData,new EllipseData(worldShS,Point(35,-50,0,allocentric),27.5,27.5));
    orange1->setColor(orange_index);
    NEW_SHAPE(orange2,EllipseData,new EllipseData(worldShS,Point(135,-50,0,allocentric),27.5,27.5));
    orange2->setColor(orange_index);
    NEW_SHAPE(green1,EllipseData,new EllipseData(worldShS,Point(135,-150,0,allocentric),27.5,27.5));
    green1->setColor(green_index);

    // Build a local map from what we can see
    localShS.clear();  // needed because mapreq.clearShapes will be false
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));

    MapBuilderRequest mapreq(MapBuilderRequest::localMap);
    mapreq.searchArea = gazePoly;
    mapreq.doScan = true;
    mapreq.pursueShapes = true;
    mapreq.maxDist = 2000;
    mapreq.clearShapes = false;   // preserve the scan PolygonShape
    mapreq.objectColors[ellipseDataType].insert(orange_index);
    mapreq.objectColors[ellipseDataType].insert(green_index);
    mapreq.rawY = true;

    unsigned int mapreq_id = mapBuilder.executeRequest(mapreq);
    erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::deactivateETID);
  }

  void processEvent(const EventBase &) {
    sndman->playFile("barkmed.wav");

    particleFilter->setMinAcceptableWeight(-3);
    for (int i=0; i<5; i++)
      particleFilter->update();

    particleFilter->setAgent();
    particleFilter->displayParticles();

    cout << "Done!" << endl;
  }

};

#endif