/**
* do world map request - orange can, green can
* compute average of two landmarks' centroids
* gaze at average point, take picture1
*** uploade picture1, upload html1, display html1
* walk to (2*ave_x - cur_x, 2*ave_y - cur_y) 
** (angleIsRelative==false)
** OR: angleIsRelative==true and angle=-180 ?
* turn around  (HERE: Should I find objects again, gaze at center?)
* take pic2
* upload picture2, upload html2, display html2
**/



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

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

using namespace DualCoding;
using namespace std;

#define WALKTIME 10.0

class BothSides: public VisualRoutinesStateNode {
public:

  class LookForCans: public MapBuilderNode {
  public:
    LookForCans() : MapBuilderNode("LookForCans",MapBuilderRequest::worldMap) {}
		
		int mapreq_id;

    virtual void DoStart() {
		MapBuilderRequest req(MapBuilderRequest::worldMap);
		const color_index green_index = ProjectInterface::getColorIndex("green");
		const color_index blue_index = ProjectInterface::getColorIndex("blue");
    mapreq.objectColors[blobDataType].insert(green_index);
	  mapreq.minBlobAreas[green_index] = 100;
	  mapreq.blobOrientations[green_index] = BlobData::pillar;

    mapreq.objectColors[blobDataType].insert(blue_index);
	  mapreq.minBlobAreas[blue_index] = 100;
	  mapreq.blobOrientations[blue_index] = BlobData::pillar;

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

		void processEvent(const EventBase &e) {
			if (e.getGeneratorID() == EventBase::mapbuilderEGID &&
					e.getSourceID() == mapreq_id ) {
				postStateCompletion();
			}
		}
  };

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

    virtual void DoStart() {
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS));
	  NEW_SHAPEVEC(gblobs, BlobData, subset(blobs, IsColor("green")));
	  NEW_SHAPEVEC(bblobs, BlobData, subset(blobs, IsColor("blue")));
		cout << "found " << gblobs.size() << " green and " << bblobs.size() << " blue!" << endl;
		if ((gblobs.size() > 0) && (bblobs.size() > 0)) {
			Point g = (gblobs[0])->getCentroid(); Point b = bblobs[0]->getCentroid();
			const coordinate_t gx = g.coordX(); const coordinate_t gy = g.coordY();
			const coordinate_t bx = b.coordX(); const coordinate_t by = b.coordY();
			const coordinate_t tx = bx + gx; const coordinate_t ty = by + gy;
			const Point tgt = Point(tx, ty);
   	 	postStateSignal<Point >(tgt);
		}
		else {
			postStateCompletion();
		}
    }
  };

  class WalkToTarget: public WalkNode {
  public:
    WalkToTarget() : WalkNode("WalkToTarget",0,0,0) {}

    virtual void DoStartEvent(const EventBase &event) {
    	const DataEvent< Point > *datev = dynamic_cast<const DataEvent< Point >*>(&event);
      if ( datev ) {
				const Point &tgt = datev->getData();
				const float tx = tgt.coordX(), ty = tgt.coordY();
				const double distance = sqrt(tx*tx + ty*ty);
				const double bearing = atan2(ty,tx);
				cout << "Target=" << tgt << "   Distance=" << distance
	    		 << " mm,    Bearing=" << bearing*180/M_PI << " deg\n";
				double dx = tx / WALKTIME;
				double dy = ty / WALKTIME;
				double da = (180.0 + bearing) / WALKTIME;
				getMC()->setTargetVelocity(dx, dy, da);
      }
    }
  };

  class StopWalking: public WalkNode {
  public:
    StopWalking() : WalkNode("StopWalking",0,0,0) {}

    virtual void DoStartEvent(const EventBase &event) {
				getMC()->setTargetVelocity(0, 0, 0);
    }
  };

  BothSides() :  VisualRoutinesStateNode("BothSides") {}

  virtual void setup() {
#statemachine

looker: LookForCans()
finder: FindTarget()
walker: WalkToTarget()
stopper: StopWalking()
complainer: SoundNode($, "howl.wav")

  startnode: StateNode =N=> looker
	looker =C=> finder
	finder =S< Point >=> walker
	finder =C=> complainer 
	walker =T(WALKTIME)=> stopper

#endstatemachine
      }
};

DATAEVENT_IMPLEMENTATION(DualCoding::Point, DualCoding::Point);

#endif