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

#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"
#include "Motion/ArmMC.h"
#include "Shared/fmat.h"

using namespace std;
using namespace DualCoding;

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

  virtual void DoStart() {
    /*
    MotionPtr<PostureMC> armMover = SharedObject<PostureMC>();
    fmat::Column<3> Ptarget = fmat::pack(250.f, 0.f, 41.f);
    fmat::Column<3> Peff = fmat::pack(0.f, 0.f, 0.f);
    bool solved = 
      posture->solveLinkPosition(Ptarget, GripperFrameOffset, Peff);
    addMotion(posture);
    */
    MotionPtr<ArmMC> armMover = SharedObject<ArmMC>();
    bool solved = armMover->moveToPoint(250, 0, 41);
    cout << "arm solved=" << solved << endl;
    addMotion(armMover);
    erouter->addTimer(this,0,2000,false);
  }

  virtual void processEvent(const EventBase&) {
    fmat::Transform gripper = kine->jointToBase(GripperFrameOffset);
    fmat::Column<3> gripperPos = fmat::SubVector<3>(&gripper(0,3));
    cout << " Gripper=" << gripperPos.fmt("%5.1f") << endl;
  }
};

class xArmSortByColor : public VisualRoutinesStateNode {
public:
  xArmSortByColor() : VisualRoutinesStateNode("ArmSortByColor") {}

  virtual void setup() {
    /*
      #statemachine
      startnode: MoveToInitialPosition =C=>
      buttonwait: StateNode =B(GreenButOffset)=>
      BuildMap =MAP=> 
      select: SelectTarget =C[setSound("howl.wav")]=> startnode

      select =S<ShapeEllipseData> >=> 

      #endstatemachine	
    */
  }

};

#endif