/* Solution to Part III 
    Chiara randomly picks one third of the screen, waits for the
    orange shell in that third to be removed, and plays a happy
    noise if it finds a green blob and an unhappy noise if it
    does not.
    @author David Klionsky (dklionsk)
    @author Sekhar Bhagavatula (cbhagava)
*/

#include "DualCoding/DualCoding.h"
#include "Behaviors/StateMachine.h"

#define PI 3.14159
using namespace DualCoding;

enum Emotion_t {
    happiness,
    despair,
};

DATAEVENT_IMPLEMENTATION(Emotion_t, unsigned int)

class LeftShellNode : public VisualRoutinesStateNode {
public:
    LeftShellNode() : VisualRoutinesStateNode("LeftShellNode") {}

    void DoStart() {
        camSkS.clear();
        NEW_SKETCH(camFrame, uchar, sketchFromSeg());
        NEW_SKETCH(mask, bool, visops::zeros(camFrame));
        
        int width = mask.width;
        int numPixels = mask.width * mask.height;
        for(int i = 0; i < numPixels; i++) {
            if(i % width <= width / 3)
                mask[i] = true;
        }

        NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame,"green"));
        NEW_SKETCH(orange_stuff, bool, visops::colormask(camFrame,"orange"));
        NEW_SKETCH(green_result, bool, green_stuff & mask);
        NEW_SKETCH(orange_result, bool, orange_stuff & mask);
        NEW_SKETCH(orange_blobs, uint, visops::areacc(orange_result));

        if(orange_blobs->max() < 40) {
            NEW_SKETCH(green_blobs, uint, visops::areacc(green_result));
            if(green_blobs->max()>20){
                postStateSignal<Emotion_t>(happiness);
            }
            else {
                postStateSignal<Emotion_t>(despair);
            }
        }
    }
};

class MiddleShellNode : public VisualRoutinesStateNode {
public:
    MiddleShellNode() : VisualRoutinesStateNode("MiddleShellNode") {}

    void DoStart() {
        camSkS.clear();
        NEW_SKETCH(camFrame, uchar, sketchFromSeg());
        NEW_SKETCH(mask, bool, visops::zeros(camFrame));
        
        int width = mask.width;
        int numPixels = mask.width * mask.height;
        for(int i = 0; i < numPixels; i++) {
            if(i % width <= 2*width / 3 && i % width >= width / 3)
                mask[i] = true;
        }

        NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame,"green"));
        NEW_SKETCH(orange_stuff, bool, visops::colormask(camFrame,"orange"));
        NEW_SKETCH(green_result, bool, green_stuff & mask);
        NEW_SKETCH(orange_result, bool, orange_stuff & mask);
        NEW_SKETCH(orange_blobs, uint, visops::areacc(orange_result));

        if(orange_blobs->max() < 40) {
            NEW_SKETCH(green_blobs, uint, visops::areacc(green_result));
            if(green_blobs->max()>20){
                postStateSignal<Emotion_t>(happiness);
            }
            else {
                postStateSignal<Emotion_t>(despair);
            }
        }
    }
};

class RightShellNode : public VisualRoutinesStateNode {
public:
    RightShellNode() : VisualRoutinesStateNode("RightShellNode") {}

    void DoStart() {
        camSkS.clear();
        NEW_SKETCH(camFrame, uchar, sketchFromSeg());
        NEW_SKETCH(mask, bool, visops::zeros(camFrame));
        
        int width = mask.width;
        int numPixels = mask.width * mask.height;
        for(int i = 0; i < numPixels; i++) {
            if(i % width >= 2*width / 3)
                mask[i] = true;
        }

        NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame,"green"));
        NEW_SKETCH(orange_stuff, bool, visops::colormask(camFrame,"orange"));
        NEW_SKETCH(green_result, bool, green_stuff & mask);
        NEW_SKETCH(orange_result, bool, orange_stuff & mask);
        NEW_SKETCH(orange_blobs, uint, visops::areacc(orange_result));

        if(orange_blobs->max() < 40) {
            NEW_SKETCH(green_blobs, uint, visops::areacc(green_result));
            if(green_blobs->max()>20){
                postStateSignal<Emotion_t>(happiness);
            }
            else {
                postStateSignal<Emotion_t>(despair);
            }
        }
    }
};

class ShellGameBehavior : public VisualRoutinesStateNode {
public:
    ShellGameBehavior() : VisualRoutinesStateNode("ShellGameBehavior") {}

    virtual void setup() {
        #statemachine

        startnode: StateNode >==RandomTrans==> {leftArmNode, middleArmNode, rightArmNode}

        leftArmNode: ArmNode [getMC()->setJoints((PI / 4.0), 0, 0);] =T(5000)=> leftShell
        middleArmNode: ArmNode [getMC()->setJoints(0, 0, 0);] =T(5000)=> middleShell
        rightArmNode: ArmNode [getMC()->setJoints(-(PI / 4.0), 0, 0);] =T(5000)=> rightShell

        leftShell: LeftShellNode() =S<Emotion_t>(happiness)=> happySound
        leftShell =S<Emotion_t>(despair)=> sadSound
        leftShell =T(1000)=> leftShell
        middleShell: MiddleShellNode() =S<Emotion_t>(happiness)=> happySound
        middleShell =S<Emotion_t>(despair)=> sadSound
        middleShell =T(1000)=> middleShell
        rightShell: RightShellNode() =S<Emotion_t>(happiness)=> happySound
        rightShell =S<Emotion_t>(despair)=> sadSound
        rightShell =T(1000)=> rightShell

        happySound: SoundNode($, "barkhigh.wav")
        sadSound: SoundNode($, "howl.wav")

        #endstatemachine
    }
};