#include "Behaviors/StateMachine.h"

$nodeclass Lab7Part3 : StateNode {

    $nodeclass HoldArmStraightOutNode : PostureNode : doStart {        
        getMC()->setWeights(1.0);
        getMC()->setOutputCmd(0,0); // base
	getMC()->setOutputCmd(1,0); // shoulder
	getMC()->setOutputCmd(2,0); // elbow
	getMC()->setOutputCmd(3,0); // wrist
    }
    $nodeclass SteerNode : WalkNode {
        virtual void doStart() {
	    erouter->addListener(this, EventBase::sensorEGID);
	}
	virtual void doEvent() {
	    PostureEngine *copy = new PostureEngine();
	    copy->takeSnapshot();

	    // turn right
	    if (copy->getOutputCmd(0).value < -.2) {
	        getMC()->setTargetVelocity(0,0,-.1);
	    }
	    // turn left
	    else if (copy->getOutputCmd(0).value > .2) {
	    	getMC()->setTargetVelocity(0,0,.1);
	    }
	    // back up
	    else if (copy->getOutputCmd(1).value < -.1) {
	        getMC()->setTargetVelocity(-100,0,0);
	    }
	    // front down
	    else if (copy->getOutputCmd(1).value > .1) {
	        getMC()->setTargetVelocity(100,0,0);
	    }

	    delete copy;
	}
    }

    $setupmachine {
        launch : StateNode =N=> {arm, steer}

	arm : HoldArmStraightOutNode
	steer : SteerNode
    }
}

REGISTER_BEHAVIOR(Lab7Part3);