//-*-c++-*-
#ifndef INCLUDED_Lab6b_h_
#define INCLUDED_Lab6b_h_

#include "DualCoding/DualCoding.h"
#include "Events/EventBase.h" 

using namespace DualCoding;

class Lab6b : public VisualRoutinesBehavior {
 public:
  Lab6b(): VisualRoutinesBehavior("Lab6b"), mapreq(NULL), pilot_id(Pilot::invalid_Pilot_ID) {}

  virtual void DoStart() {
    VisualRoutinesBehavior::DoStart();
    
    const color_index orange_index = ProjectInterface::getColorIndex("orange");

    localShS.clear();
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));

    mapreq = new MapBuilderRequest(MapBuilderRequest::localMap);
    mapreq->searchArea = gazePoly;
    mapreq->doScan = true;
    mapreq->pursueShapes = true;
    mapreq->maxDist = 2000;
    mapreq->rawY = true;
    mapreq->clearShapes = false;
    mapreq->objectColors[blobDataType].insert(orange_index);
    mapreq->minBlobAreas[orange_index] = 100;
    mapreq->blobOrientations[orange_index] = BlobData::pillar;  // this blob stands on the floor; it does not lie in the ground plane

    PilotRequest preq(PilotRequest::visualSearch);
    preq.mapBuilderRequest = mapreq;
    preq.exitTest = &checkForBlob;
    preq.searchRotationAngle = 1;

    pilot_id = pilot.executeRequest(preq);
    erouter->addListener(this,EventBase::pilotEGID,pilot_id,EventBase::deactivateETID);
  }

  virtual void DoStop() {
    delete mapreq;
    pilot.abort();
    VisualRoutinesBehavior::DoStop();
 }


  virtual void processEvent(const EventBase &event) {
    cout << "Lab6b saw; " << event.getDescription(true,3) << endl;
    if ( event.getSourceID() == pilot_id ) {
      delete mapreq;
      mapreq = NULL;
      PilotRequest preq(PilotRequest::gotoShape);
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
      NEW_SHAPE(biggestblob, BlobData, max_element(blobs,BlobData::AreaLessThan()));
      preq.targetShape = biggestblob;
      pilot.executeRequest(preq);
    }
    else
      cout << "Done!";
  }

  static bool checkForBlob() {
    return find_if<BlobData>(localShS).isValid();
  }

 private:
  MapBuilderRequest *mapreq;
  unsigned int pilot_id;
    
  Lab6b(const Lab6b&);
  Lab6b& operator=(const Lab6b&);

};

#endif