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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Sound/SoundManager.h"
#include "Events/EventRouter.h"
#include "Shared/fmat.h"
#include "Motion/XWalkMC.h"
#include "Motion/MotionPtr.h"

#include "ChuShared.h"
#include "ChuArmPush.h"
#include "ChuArmPushPlain.h"
#include "ChuEstimateObjectSize.h"
#include "ChuDecider.h"
#include "ChuFindObject.h"
#include "ChuMoveToObject.h"
#include "ChuTalker.h"

//using namespace DualCoding;




class ChuBehavior : public VisualRoutinesStateNode {
public:



  
  ChuBehavior(): VisualRoutinesStateNode("ChuBehavior"),color("blue"),lap(50),maxCount(6) {}
  
	virtual void setup() {
    MotionManager::MC_ID walkID = addMotion(MotionPtr<XWalkMC>());


#statemachine
  //Look for the correct blob, prepare the arm and compute the new destination
    nodeFindObject: ChuFindObject(color)
    nodeActionPrep : ChuArmPushPlain(0)
    nodeMoveToObject: ChuMoveToObject()

  //Rotating (0), Moving (1) and angle readjusting (2, non used here)
    nodeTalk0: ChuTalker(0)
    xwalknode0: XWalkNode[setMC(walkID)]
    nodeTalk1: ChuTalker(1)
    xwalknode1: XWalkNode[setMC(walkID)]
    nodeTalk2: ChuTalker(2)
    xwalknode2: XWalkNode[setMC(walkID)]
    
  //Look at position, and take the first estimate
    nodeHeadPointer: HeadPointerNode[ getMC()->setMaxSpeed(0, 0.7); getMC()->setMaxSpeed(1,0.7);getMC()->lookAtPoint(350, 0, 41);]
    nodeEstimateOnce: ChuEstimateObjectSize(0,color)
    nodeDecider : ChuDecider(maxCount)
    
  //ARM TYPE 1 (do the push)
    nodeActionDo : ChuArmPushPlain(1)

  //ARM TYPE 2 (ending)
    nodeActionEnd :ChuArmPushPlain(2)

  //ESTIMATE IMAGE LOOP WITH DECIDER
    nodeEstimate : ChuEstimateObjectSize(1,color)
    nodeWait : StateNode

  //Speech for findings
    nodeSpeechR : SpeechNode("It is rollable")

    nodeSpeechNR : SpeechNode("That's not very rollable")

//____________________________TRANSITIONS________________________________
    
    nodeFindObject   =E(pilotEGID,(size_t)nodeFindObject,deactivateETID)=> nodeActionPrep
    nodeActionPrep   =C=> nodeMoveToObject

    nodeMoveToObject =C=> nodeTalk0 
    nodeTalk0        =S<WalkRequest>=> xwalknode0
    xwalknode0       =C=>nodeTalk1
    nodeTalk1        =S<WalkRequest>=>xwalknode1 
    xwalknode1       =C=>nodeHeadPointer

    nodeHeadPointer  =C=> nodeEstimateOnce
    nodeEstimateOnce =S<ChuDeciderRequest>=> nodeDecider
    nodeDecider      =S<int>(0)=>nodeActionDo    

    nodeActionDo     =C=>{nodeEstimate,nodeActionEnd}

    nodeEstimate     =S<ChuDeciderRequest>=> nodeDecider 
    nodeDecider      =S<int>(1)=>nodeWait   
    nodeWait         =T(lap)=> nodeEstimate
    
    nodeDecider      =S<int>(2)=>nodeSpeechR

    nodeDecider      =S<int>(3)=>nodeSpeechNR    



# endstatemachine
startnode = nodeFindObject;
  }

//_______________________________________________________________________

  virtual void DoStart() {
    std::cout << getName() << " is starting up." << std::endl;
  }
 
  virtual void DoStop() {
    std::cout << getName() << " is shutting down." << std::endl;
  }

protected:
  string color;
  int lap;
  int maxCount;
  //ChuBehavior(const ChuBehavior&);
  //ChuBehavior& operator=(const ChuBehavior&);
};



#endif