#include "Behaviors/Demos/Navigation/PilotDemo.h"

$nodeclass Hw2_part6 : PilotDemo {

    $nodeclass TurnHome : PilotNode(PilotTypes::walk) : doStart {
        float theta = theAgent->getOrientation();

        pilotreq.da = -theta + AngTwoPi(theAgent->getCentroid().atanYX()) + AngTwoPi(3.14);
        pilotreq.collisionAction = collisionIgnore;
    }

    $nodeclass WalkHome : PilotNode(PilotTypes::walk) : doStart {
        int x = theAgent->getCentroid().coordX();
        int y = theAgent->getCentroid().coordY();
        int dist = sqrt(x*x + y*y);

        pilotreq.dx = dist;
        pilotreq.collisionAction = collisionIgnore;
    }

    $setupmachine{
        rundemo: TurnHome =C=> WalkHome =C=> PostMachineCompletion
  }

}

REGISTER_BEHAVIOR(Hw2_part6);