#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"

$nodeclass HoldingHands {
    $nodeclass HoldArmOut : PostureNode {
        virtual void doStart() {
            getMC()->setOutputCmd(ArmShoulderOffset, 0.f);
            getMC()->setOutputCmd(ArmOffset, 0.f);
        }

        $nodeclass FollowLeader : WalkNode {
            virtual void doStart() {
                erouter->addListener(this, EventBase::sensorEGID);
            }

            virtual void doEvent() {
                if(fabs(state->outputs[ArmShoulderOffset]) < 0.02f &&
                    fabs(state->outputs[ArmOffset]) < 0.02f) {
                    getMC()->setTargetVelocity(0, 0, 0);
                    return;
                }
                getMC()->setTargetVelocity(state->outputs[ArmShoulderOffset]*3000,
                                            0,
                                            state->outputs[ArmOffset]*7);
                cout << "arm: " << state->outputs[ArmOffset] << endl;
                cout << "arm shoulder: " << state->outputs[ArmShoulderOffset] << endl << endl;
            }
        }

        $setupmachine {
            startnode: FollowLeader
        }
    }

    virtual void setup() {
        MotionManager::MC_ID pmc_id = addMotion(MotionPtr<PostureMC>());
        $statemachine {
            startnode: HoldArmOut[setMC(pmc_id)]
        }
    }
}

REGISTER_BEHAVIOR(HoldingHands);