#include "Behaviors/StateMachine.h"

$nodeclass FlipSodaCanVision : DynamicMotionSequenceNode{
  $nodeclass FlipSetup : DynamicMotionSequenceNode : doStart {
	cout << "FLIP UP" << endl;
	
	getMC()->advanceTime(2000);    
    	getMC()->loadFile("ArmUpWristReset.pos");
	
	getMC()->advanceTime(500);    
    	getMC()->loadFile("NeutralArmOut.pos");
	
	getMC()->advanceTime(500);    
    	getMC()->loadFile("NeutralArmGrabbing.pos");
	
	getMC()->advanceTime(500);    
    	getMC()->loadFile("NeutralArmUp.pos");

	getMC()->advanceTime(1500);    
    	getMC()->loadFile("StareAtCan.pos");
	cout << "FLIP UP DONE" << endl;
  }

  $nodeclass FlipSetdown : DynamicMotionSequenceNode : doStart {
	cout << "FLIP DOWN" << endl;
	
	getMC()->advanceTime(1500);    
    	getMC()->loadFile("NeutralArmUp.pos");

	getMC()->advanceTime(500);    
    	getMC()->loadFile("ArmUpCanFlipped.pos");
	
	getMC()->advanceTime(500);    
    	getMC()->loadFile("ArmDownCanDown.pos");
	
	getMC()->advanceTime(500);    
    	getMC()->loadFile("ArmDownCanReleased.pos");
	
	getMC()->advanceTime(500);    
    	getMC()->loadFile("ArmUpCanless.pos");
  }

  $nodeclass CheckCan : VisualRoutinesStateNode : doStart {
	cout << "CHECK CAN" << endl;
	NEW_SKETCH(camFrame, uchar, sketchFromSeg());
  	NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame,"blue"));
  	NEW_SHAPEVEC(blobs, BlobData, BlobData::extractBlobs(blue_stuff,10000));
	cout << "Observing can" << endl;	
	if(blobs.size() >= 1) {
		cout << "Observing can COMPLETE" << endl;	
		postStateSuccess();
	} else {
		cout << "Observing can FAILURE" << endl;	
		postStateFailure();
	}
  }

  $setupmachine{
	launch: StateNode =N=> {sayIt, flipup}
 	sayIt : SpeechNode("I'm flipping out!")
	flipup : FlipSetup()
	checkForCan : CheckCan()
	flipdown : FlipSetdown()
	help : SpeechNode("Please put my can back")
	
	flipup =C=> checkForCan 
	checkForCan =S=> flipdown =C=> flipup
	checkForCan =F=> help =T(5000)=> flipup 
  }

}

REGISTER_BEHAVIOR(FlipSodaCanVision);