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

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

using namespace DualCoding;

//! Finds the biggest ellipse in the image and keeps the gripper centered over it
class ArmTrackObject : public VisualRoutinesStateNode {
public:

  //! Use the MapBuilder to find ellipses
  class LookForObjects : public MapBuilderNode {
  public:
    LookForObjects() : MapBuilderNode("LookForObjects",MapBuilderRequest::localMap) {}
    virtual void DoStart() {
      mapreq.addObjectColor(ellipseDataType,"pink");
      mapreq.addObjectColor(ellipseDataType,"orange");
      mapreq.addObjectColor(ellipseDataType,"blue");
    }
  };

  //! Sort ellipses by size and send the largest one as target for the next state
  class ChooseObject : public VisualRoutinesStateNode {
  public:
    ChooseObject() : VisualRoutinesStateNode("ChooseObject") {}
    virtual void DoStart() {
      NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(localShS));
      if ( ellipses.size() == 0 )
	postStateCompletion();
      else {
	NEW_SHAPEVEC(bigEllipses, EllipseData, stable_sort(ellipses, not2(EllipseData::AreaLessThan())));
	postStateSignal<Shape<EllipseData> >(bigEllipses[0]);
      }
    }
  };

  //! Move the gripper to the position of the target
  class ReachToObject : public ArmNode {
  public:
    ReachToObject() : ArmNode("ReachToObject") {}
    virtual void DoStartEvent(const EventBase &event) {
      const DataEvent<Shape<EllipseData> > *datev = dynamic_cast<const DataEvent<Shape<EllipseData> >*>(&event);
      if ( datev != NULL ) {
	const Point target = datev->getData()->getCentroid();
	cout << "Reach to " << target << endl;
	for ( unsigned int i=0; i<NumArmJoints; i++ )
	  getMC()->setMaxSpeed(i, 0.3);
	getMC()->moveToPoint(target.coordX(), target.coordY(), 0);
      }
    }
  };

  //**************** ArmTrackObject State Machine ****************
  ArmTrackObject() : VisualRoutinesStateNode("ArmTrackObject") {}

  virtual void setup() {
#statemachine
    // Look for ellipses, choose the largest, put the gripper there, wait 2 seconds, and repeat
  startnode: LookForObjects() =MAP=>
    choose: ChooseObject() =S<Shape<EllipseData> >=>
      ReachToObject() =C=> StateNode =T(2000)=> startnode

      // If no ellipse found, go back and look again
      choose =C=> startnode
#endstatemachine
      }

};

#endif