#include "Behaviors/StateMachine.h"

#define ARM_RIGHT  -0.59
#define ARM_MIDDLE -0.17
#define ARM_LEFT   0.27

const float armAngles[3] = {ARM_LEFT, ARM_MIDDLE, ARM_RIGHT};
const string shellColors[3] = {"orange", "blue", "yellow"};
const string markerColor = "green";

$nodeclass HW3_Shell_Game : StateNode {
  enum ShellDir {
    left = 0,
    middle,
    right
  };

  $nodeclass PointToShell(ShellDir shell) : ArmNode : doStart {
    getMC()->setJoints(armAngles[shell], 0, 0, 0, 0, 0);
  }

  $nodeclass WaitHideShell(ShellDir shell) : VisualRoutinesStateNode : doStart {
    camSkS.clear();
    NEW_SKETCH(camFrame, uchar, sketchFromSeg());

    NEW_SKETCH(mycolor, bool, visops::colormask(camFrame, shellColors[shell]));
    NEW_SKETCH(myarea, uint, visops::areacc(mycolor));

    cout << "Max area for " << shellColors[shell] << " is " << myarea->max() << endl;

    if (myarea->max() < 500) {
      postStateSignal<int>(1);
    } else {
      postStateSignal<int>(0);
    }
  }

  $nodeclass CheckForMarker : VisualRoutinesStateNode : doStart {
    camSkS.clear();
    NEW_SKETCH(camFrame, uchar, sketchFromSeg());

    NEW_SKETCH(marker, bool, visops::colormask(camFrame, markerColor));
    NEW_SKETCH(marker_area, uint, visops::areacc(marker));

    cout << "Marker color " << markerColor << " is " << marker_area->max() << endl;

    if (marker_area->max() > 100) {
      postStateSignal<int>(1);
    } else {
      postStateSignal<int>(0);
    }
  }

  $setupmachine {
    startnode: StateNode =RND=> {pointLeft, pointMiddle, pointRight}

    pointLeft:   PointToShell($, left)   =C=> waitShowLeft
    pointMiddle: PointToShell($, middle) =C=> waitShowMiddle
    pointRight:  PointToShell($, right)  =C=> waitShowRight

    waitShowLeft:   WaitHideShell($, left)   =N=> SoundNode("ping.wav") =T(1000)=> waitShowLeft
    waitShowMiddle: WaitHideShell($, middle) =N=> SoundNode("ping.wav") =T(1000)=> waitShowMiddle
    waitShowRight:  WaitHideShell($, right)  =N=> SoundNode("ping.wav") =T(1000)=> waitShowRight

    {waitShowLeft, waitShowMiddle, waitShowRight} =S<int>(1)=> checkForMarker

    checkForMarker: CheckForMarker
    checkForMarker =S<int>(1)=> happySound =C=> startnode
    checkForMarker =S<int>(0)=> sadSound =C=> startnode

    happySound: SoundNode($, "yap.wav")
    sadSound: SoundNode($, "whimper.wav")
  }
}

REGISTER_BEHAVIOR(HW3_Shell_Game);