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

$nodeclass ToFifthCylinder : PilotDemo
{
    $provide Point old_position;
    $provide Point new_position;
    $provide AngTwoPi old_angle;
    $provide AngTwoPi new_angle;

    $setupmachine {
        startdemo: OriginalPosition =C=> TurnCW(100) =C=> GoForward(1200) =C=> TurnCCW(100) =C=> GoForward(500) =C=> TurnCCW(110) =C=> RunDemo =C=> PostMachineCompletion
    }

    $nodeclass OriginalPosition : PilotNode(PilotTypes::walk) : doStart
    {
        $reference ToFifthCylinder::old_position, ToFifthCylinder::old_angle;
        old_position = theAgent->getCentroid();
        old_angle = theAgent->getOrientation();
        cout << "Robot starts out at " << old_position << " and oriented at " << float(old_angle)*180/M_PI << " degrees" << endl;
    }

    $nodeclass TurnCCW(float angle) : PilotNode(PilotTypes::walk) : doStart
    {
        pilotreq.collisionAction = collisionIgnore;
        pilotreq.da = deg2rad(angle);
    }

    $nodeclass TurnCW(float angle) : PilotNode(PilotTypes::walk) : doStart
    {
        pilotreq.collisionAction = collisionIgnore;
        pilotreq.da = -deg2rad(angle);
    }

    $nodeclass GoForward(float x) : PilotNode(PilotTypes::walk) : doStart
    {
        pilotreq.collisionAction = collisionIgnore;
        pilotreq.dx = x;
    }

    $nodeclass RunDemo : PilotDemo
    {
        $setupmachine {
            rundemo: FinalPosition =C=> PostMachineCompletion
        }

        $nodeclass FinalPosition : PilotNode(PilotTypes::walk) : doStart
        {
            $reference ToFifthCylinder::new_position, ToFifthCylinder::old_position;
            $reference ToFifthCylinder::new_angle;

            new_position = theAgent->getCentroid();
            new_angle = theAgent->getOrientation();
            float new_ang = float(new_angle) * 180 / M_PI;
            cout << "Robot ends at " << new_position << " and oriented at " << float(new_angle)*180/M_PI << " degrees" << endl;
            float move_pos = (new_position - old_position).xyNorm();
            float to_ang = rad2deg(atan(new_position.coordY() / new_position.coordX()));
            cout << "Robot must get to " << to_ang << " degrees" << endl;
            if(to_ang >= 0)
                to_ang = to_ang - 180;
            else
                to_ang = to_ang + 180;
            float move_ang = to_ang - new_ang;
            if(move_ang > 90)
                move_ang = 180 - move_ang;
            else if(move_ang < -90)
                move_ang = move_ang + 180;
            cout << "Robot must turn " << move_ang << " degrees and move " << move_pos << endl;
            pilotreq.collisionAction = collisionIgnore;
            pilotreq.da = deg2rad(move_ang);
            pilotreq.dx = move_pos;
        }
    }
}

REGISTER_BEHAVIOR(ToFifthCylinder);