#include "Behaviors/StateMachine.h"

#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Shared/RobotInfo.h"

#define THRESHOLD 0.02
#define RIGHT_FINGER_TARGET 0.04
#define SHOULDER_TARGET 1.57
#define GRAVITY_OFFSET -0.02
#define CAP(x,c) (MAX(-(c),MIN(c,x)))
#define MAX_TURN 3.141
#define MAX_SPEED 200

$nodeclass Lab7Q3 : StateNode {    
    $nodeclass Guide : WalkNode : doStart{
        cout << "Finger: " << state->outputs[RightFingerOffset] << " Shoulder: " << state->outputs[ArmShoulderOffset] << endl;
        float diff = state->outputs[RightFingerOffset] - RIGHT_FINGER_TARGET;
        float desired_a = 0.0;
        float desired_speed = 0.0;
        if (abs(diff) > THRESHOLD) {
            cout << "Turning..." << endl;
            desired_a = CAP(-(diff/THRESHOLD), MAX_TURN);
        }
        
        diff = state->outputs[ArmShoulderOffset] - (SHOULDER_TARGET + GRAVITY_OFFSET);
        if (abs(diff) > THRESHOLD) {
            cout << "Moving..." << endl;
            desired_speed = CAP(-(diff/THRESHOLD)*100, MAX_SPEED);
        }
        
        getMC()->setTargetVelocity(desired_speed, desired_speed, desired_a);
            
    }
    
    $nodeclass HandHold : PostureNode : doStart{
        getMC()->setOutputCmd(ArmBaseOffset, OutputCmd(-0.0896));
        getMC()->setOutputCmd(ArmShoulderOffset, OutputCmd(SHOULDER_TARGET));
        getMC()->setOutputCmd(ArmElbowOffset, OutputCmd(0.15));
        getMC()->setOutputCmd(ArmWristOffset, OutputCmd(-1.141));
        getMC()->setOutputCmd(WristRotateOffset, OutputCmd(-1.6302));
        getMC()->setOutputCmd(LeftFingerOffset, OutputCmd(0.1663));
        getMC()->setOutputCmd(RightFingerOffset, OutputCmd(RIGHT_FINGER_TARGET));
    }
    
    virtual void setup() {

        // We make the posture motion command here and supply it to the
		// PostureNode via setMC() so that it only has to be registered
		// with the MotionManager once. Registering and deregistering a
		// PostureMC inside a tight sensor loop is inefficient and leads
		// to crashes.
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

        $statemachine {
            startnode : StateNode
            handhold: HandHold[setMC(posturemc)]
            guide : Guide
            
            startnode =N=> handhold =E(sensorEGID)=> guide =E(sensorEGID)=> startnode
        }
    }
}


REGISTER_BEHAVIOR(Lab7Q3);