/* Solution to Part III 
    Chiara randomly picks one third of the screen, waits for the
    orange shell in that third to be removed, and plays a happy
    noise if it finds a green blob and an unhappy noise if it
    does not.
    @author David Klionsky (dklionsk)
    @author Sekhar Bhagavatula (cbhagava)
*/

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

using namespace DualCoding;

enum Ball_t {
    blue,
    pink,
};

DATAEVENT_IMPLEMENTATION(Ball_t, unsigned int)


class BuildMap : public VisualRoutinesStateNode {
public:
    BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
    virtual void DoStart() {
    
        // set up map builder request
        const int pink_index = ProjectInterface::getColorIndex("pink");
        const int blue_index = ProjectInterface::getColorIndex("blue");

        MapBuilderRequest req(MapBuilderRequest::cameraMap);
        
        req.objectColors[lineDataType].insert(pink_index);
        req.objectColors[lineDataType].insert(blue_index);
				req.objectColors[ellipseDataType].insert(pink_index);
				req.objectColors[ellipseDataType].insert(blue_index);

        // execute request
        unsigned int mapreq_id = mapBuilder.executeRequest(req);
        erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
    }

    virtual void processEvent(const EventBase &) {
        postStateCompletion();
    }

};

class PositionArm1 : public ArmNode {
public:
  PositionArm1() : ArmNode("PositionArm1") {}

  virtual void DoStart() {
		erouter->addListener(this, EventBase::motmanEGID,getMC()->getID(),EventBase::statusETID);
		getMC()->moveToPoint(83.8622,360.377,24.5354);	
  }

	

  virtual void processEvent(const EventBase&) {
    //fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    //cout << "Transform:\n" << Tgripper.fmt("%7.3f") << endl;
    //fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));
		postStateCompletion();
		//cout<<target[0]<<" "<<target[1]<<" "<<target[2]<<endl;
  }

};


class PositionArm2 : public ArmNode {
public:
  PositionArm2() : ArmNode("PositionArm2") {}

  virtual void DoStart() {
		erouter->addListener(this, EventBase::motmanEGID,getMC()->getID(),EventBase::statusETID);
		getMC()->moveToPoint(226.127,0.742444,24.633);	
  }

	

  virtual void processEvent(const EventBase&) {
    //fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    //cout << "Transform:\n" << Tgripper.fmt("%7.3f") << endl;
    //fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));
		postStateCompletion();
		//cout<<target[0]<<" "<<target[1]<<" "<<target[2]<<endl;
  }

};

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

    virtual void setup() {
        #statemachine

		startnode: StateNode=N=>PositionArm1()=C=>PositionArm2()=C=>BuildMap()//=E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=>BuildMap()=C=>MoveArm()=C=>startnode

        #endstatemachine
    }
};