#include "DualCoding/DualCoding.h"
#include "Behaviors/StateMachine.h"
using namespace DualCoding;

class WaitRemoveNode: public VisualRoutinesStateNode{
  private:
 		Sketch<bool> viewArea;
  public:
    WaitRemoveNode(Sketch<bool> view): VisualRoutinesStateNode("WaitRemoveNode") {this->viewArea = view;}
    virtual void DoStart() {
        while(true)
        {
            NEW_SKETCH(camFrame, uchar, sketchFromSeg());
            NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame, "blue")&viewArea);
            NEW_SKETCH(blue_area, uint, visops::areacc(blue_stuff));
            //if(blue_area->max() < 10)
               break;
        }
        postStateCompletion();
    }

 };

class CheckTapeNode: public VisualRoutinesStateNode{
  private:
    Sketch<bool> viewArea;
  public:
    CheckTapeNode(Sketch<bool> view): VisualRoutinesStateNode("CheckTapeNode") {this->viewArea = view;}
    virtual void DoStart() {
        NEW_SKETCH(camFrame, uchar, sketchFromSeg());
        NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame, "green")&viewArea);
        NEW_SKETCH(green_area, uint, visops::areacc(green_stuff));
        if(green_area->max() > 10)
            postStateSignal<bool>(true);
        else
            postStateSignal<bool>(false);

    }

 };


class ShellBehavior : public VisualRoutinesStateNode {
 public:
  ShellBehavior() : VisualRoutinesStateNode("ShellBehavior") {}
  
	virtual void setup() {

		 printf("ShellBehavior setup\n");
     Sketch<bool> left(camSkS, "left");
     Sketch<bool> mid(camSkS, "mid");
     left = false;
     mid = false;
     for(int i = 0; i < left.height; i++)
     {
         int j;
         for(j = 0; j < left.width/3; j++)
         {
             left(j,i)=true;
         }
         left->V();
         
         for(; j < 2*left.width/3; j++)
         {
             mid(j,i)=true;
         }
         mid->V();

     }
     NEW_SKETCH(rt, bool, !(left|mid));

     #statemachine
     startnode:StateNode
     armLeft:ArmNode("s0")[getMC()->moveJoint(0,-15);]=C=>waitLeft
     armMid:ArmNode("s1")[getMC()->moveJoint(0,0);]=C=>waitMid
     armRt:ArmNode("s2")[getMC()->moveJoint(0,15);]=C=>waitRt
     waitLeft: WaitRemoveNode(left)=C=> checkLeft
     waitRt: WaitRemoveNode(rt)=C=> checkRt
     waitMid: WaitRemoveNode(mid)=C=> checkMid
     checkLeft: CheckTapeNode(left)=S<bool>(true)=> respHappy
     checkLeft=S<bool>(false)=>respSad
     checkMid: CheckTapeNode(mid)=S<bool>(true)=> respHappy
     checkMid=S<bool>(false)=>respSad
     checkRt: CheckTapeNode(rt)=S<bool>(true)=> respHappy
     checkRt=S<bool>(false)=>respSad
     respHappy:SoundNode($,"barkhigh.wav")
     respSad:SoundNode($,"howl.wav")
     #endstatemachine
     RandomTrans *randomTrans = new RandomTrans(armLeft);
     randomTrans->addDestination(armMid);
     randomTrans->addDestination(armRt);
     startnode->addTransition(randomTrans);
	}
};