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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
#include "Motion/Kinematics.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/PIDMC.h"
#include "Motion/MotionPtr.h"
#include "Shared/fmat.h"
#include "Shared/RobotInfo.h"
#include "Sound/SoundManager.h"

using namespace DualCoding;

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 orange_index = ProjectInterface::getColorIndex("orange");
    const int green_index = ProjectInterface::getColorIndex("green");
    const int yellow_index = ProjectInterface::getColorIndex("yellow");
    const int blue_index = ProjectInterface::getColorIndex("blue");

    NEW_SHAPE(gazePt, PointData, new PointData(localShS,Point(800,390,-900,camcentric)));

    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.searchArea = gazePt;
    req.maxDist = 1500;
    req.pursueShapes = true;
    req.numSamples = 5;
    req.motionSettleTime = 2000;
    
    req.objectColors[lineDataType].insert(pink_index);
    req.objectColors[lineDataType].insert(blue_index);
    req.occluderColors[lineDataType].insert(yellow_index);
    req.occluderColors[lineDataType].insert(green_index);
    req.occluderColors[lineDataType].insert(orange_index);

    req.objectColors[ellipseDataType].insert(orange_index);
    req.objectColors[ellipseDataType].insert(blue_index);
    req.objectColors[ellipseDataType].insert(pink_index);
    req.objectColors[ellipseDataType].insert(green_index);

    unsigned int mapreq_id = mapBuilder.executeRequest(req);
    erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
  }
  virtual void processEvent(const EventBase &) {
    postStateCompletion();
  }
};


  
//! Relaxes the arm and keeps the camera pointed at the gripper as the user moves the arm around
/*class MoveArm : public BehaviorBase {
public:
  MoveArm() : BehaviorBase("MoveArm"),
	      armRelaxer(SharedObject<PIDMC>(ArmOffset, ArmOffset+NumArmJoints, 0.f)), {}
  
  virtual void DoStart() {
    addMotion(armRelaxer);
    erouter->addListener(this, EventBase::sensorEGID);
  }
  
  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));
    headMover->lookAtPoint(target[0],target[1],target[2]);
  }
  
protected:
  MotionPtr<PIDMC> armRelaxer;
  
private:
  MoveArm(const MoveArm&);
  MoveArm& operator=(const MoveArm&);
  };*/

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

  virtual void DoStart() {
    //construct a local map containing both ellipses and lines
    std::vector<ShapeRoot> allLines = localShS.allShapes(lineDataType);
    std::vector<ShapeRoot> allBlobs =localShS.allShapes(blobDataType);
    //go through all ellipses found, calculate the distance from the baseframe
    
    

  
    
    //std::vector<Shape<BlobData>> allBlobs = select_type<BlobData>(localShS.allShapes(blobDataType));
  }
};

class SortingObjects : public VisualRoutinesStateNode {
public:
  SortingObjects(): VisualRoutinesStateNode("SortingObjects") {}
  virtual void setup() {
#statemachine

startSort: StateNode

wait: StateNode

//set the joints of the arm
movearm: ArmNode[
getMC()->setMaxSpeed(0,0.2);
getMC()->setMaxSpeed(1,0.2);
getMC()->setMaxSpeed(2,0.2);
getMC()->setJoints(1.1695f, -0.0230f, -1.4459f);
getMC()->checkinMotion(4);]

      //movehead: HeadPointerNode[getMC()->setMaxSpeed(0,0.2);
      //getMC()->lookInDirection(800.00f, 390.00f, -900.00f);]

buildmap: BuildMap()

startSort =N=> movearm

movearm=C=>wait

wait=T(2000)=>buildmap

      //movehead =T(2000)=>buildmap

 //=C=> MapCalc()
      //mapcalc: MapCalc() =C=>movegrip
      //movegrip: 
#endstatemachine
startnode = startSort;
  }
};

#endif