#include "Behaviors/Demos/Navigation/PilotDemo.h"
$nodeclass Circumnavigate : PilotDemo {
    $setupmachine{
         startdemo: TurnLeft144 =C=> GoForward(1000) =C=> TurnRight90 =C=> GoForward(500) =C=> TurnRight90 =C=> GoForward(1500) =C=> TurnRightWeird =C=> GoForward(1500) =C=> TurnRight90 =C=> GoForward(500) =C=> TurnRight90 =C=> GoForward(1000) =C=> TurnRightWeird90
    }

    $nodeclass TurnRight90 : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.da = -1.57; // turn by 144 degrees
    }

    $nodeclass TurnRightWeird : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.da = -0.943; // turn by 144 degrees
    }

    $nodeclass TurnLeft144 : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.da = 2.513; // turn by 144 degrees
    }

    $nodeclass TurnRightWeird90 : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.da = -0.627; // turn by (90-(180-144)) degrees
    }

    $nodeclass GoForward(float x) : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.dx = x; // forward half a meter
    }
    

}
REGISTER_BEHAVIOR(Circumnavigate);