#include "Behaviors/StateMachine.h"

$nodeclass HW3_Free_Style : VisualRoutinesStateNode {
  enum SelectedBlob {
    noBlob,
    blueBlob,
    pinkBlob,
    orangeBlob,
    yellowBlob
  };

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

    NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame,"green"));
    NEW_SKETCH(green_filled, bool, visops::fillin(green_stuff,2,3,8));
    NEW_SKETCH(green_interior, bool, visops::fillInterior(green_filled));

    NEW_SKETCH(blue_stuff,   bool, visops::colormask(camFrame,"blue"));
    NEW_SKETCH(pink_stuff,   bool, visops::colormask(camFrame,"pink"));
    NEW_SKETCH(orange_stuff, bool, visops::colormask(camFrame,"orange"));
    NEW_SKETCH(yellow_stuff, bool, visops::colormask(camFrame,"yellow"));

    NEW_SKETCH(blue_filled,   bool, visops::fillin(blue_stuff,2,3,8));
    NEW_SKETCH(pink_filled,   bool, visops::fillin(pink_stuff,2,3,8));
    NEW_SKETCH(orange_filled, bool, visops::fillin(orange_stuff,2,3,8));
    NEW_SKETCH(yellow_filled, bool, visops::fillin(yellow_stuff,2,3,8));
    
    NEW_SKETCH(blue_areas,   uint, visops::areacc(blue_filled));
    NEW_SKETCH(pink_areas,   uint, visops::areacc(pink_filled));
    NEW_SKETCH(orange_areas, uint, visops::areacc(orange_filled));
    NEW_SKETCH(yellow_areas, uint, visops::areacc(yellow_filled));
  
    NEW_SKETCH(blue_contained,   uint, green_interior * blue_areas);
    NEW_SKETCH(pink_contained,   uint, green_interior * pink_areas);
    NEW_SKETCH(orange_contained, uint, green_interior * orange_areas);
    NEW_SKETCH(yellow_contained, uint, green_interior * yellow_areas);

    int blue_size =   blue_contained->max();
    int pink_size =   pink_contained->max();
    int orange_size = orange_contained->max();
    int yellow_size = yellow_contained->max();

    int max_size = 0;
    if (blue_size>max_size)   max_size = blue_size;
    if (pink_size>max_size)   max_size = pink_size;
    if (orange_size>max_size) max_size = orange_size;
    if (yellow_size>max_size) max_size = yellow_size;

    if (max_size < 40) max_size = -1;

    if (blue_size == max_size) {
      postStateSignal<SelectedBlob>(blueBlob);
    
    } else if (pink_size == max_size) {
      postStateSignal<SelectedBlob>(pinkBlob);
    
    } else if (orange_size == max_size) {
      postStateSignal<SelectedBlob>(orangeBlob);
    
    } else if (yellow_size == max_size) {
      postStateSignal<SelectedBlob>(yellowBlob);
    
    } else {
      postStateSignal<SelectedBlob>(noBlob);
    }
  }

  $setupmachine {
    ringselect: RingSelect

    none:   SoundNode($, "ping.wav")
    blue:   SpeechNode($, "blue")
    pink:   SpeechNode($, "pink")
    orange: SpeechNode($, "orange")
    yellow: SpeechNode($, "yellow")

    ringselect =S<SelectedBlob>(noBlob)=>     none
    ringselect =S<SelectedBlob>(blueBlob)=>   blue
    ringselect =S<SelectedBlob>(pinkBlob)=>   pink
    ringselect =S<SelectedBlob>(orangeBlob)=> orange
    ringselect =S<SelectedBlob>(yellowBlob)=> yellow

    {none, orange, pink, yellow, blue} =C(1)=> StateNode =T(3000)=> ringselect
  }
}

REGISTER_BEHAVIOR(HW3_Free_Style);