//-*-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"), armRelaxer(SharedObject<PIDMC>(ArmOffset, ArmOffset+JointsPerArm, 0.f)) {}

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

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

  virtual void processEvent(const EventBase&) {
	printGripperFrame();
  }
  
  MotionPtr<PIDMC> armRelaxer;
};

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