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

$nodeclass HW2Part2: VisualRoutinesStateNode {

	static void buildMap() {
    	cout << "Building map..." << endl;
    	// vee-shaped wall
    	vector<Point> pts;
    	pts.push_back(Point(  0,  866, 0, allocentric));
    	pts.push_back(Point(500,    0, 0, allocentric));
    	pts.push_back(Point(  0, -866, 0, allocentric));
    	NEW_SHAPE(wall, PolygonData, new PolygonData(worldShS, pts, false));

    	NEW_SHAPE(tag0, AprilTagData,
        	new AprilTagData(worldShS, Point( 92,  704, 177.8),
            AprilTags::TagDetection(0)));

   	 	NEW_SHAPE(tag1, AprilTagData,
            new AprilTagData(worldShS, Point(245,  440, 177.8),
            AprilTags::TagDetection(1)));

   		NEW_SHAPE(tag2, AprilTagData,
        	new AprilTagData(worldShS, Point(397,  176, 177.8),
            AprilTags::TagDetection(2)));

    	NEW_SHAPE(tag3, AprilTagData,
        	new AprilTagData(worldShS, Point(397, -176, 177.8),
        	AprilTags::TagDetection(3)));

		NEW_SHAPE(tag4, AprilTagData,
    		new AprilTagData(worldShS, Point(245, -440, 177.8),
        	AprilTags::TagDetection(4)));

	    NEW_SHAPE(tag5, AprilTagData,
    	   	new AprilTagData(worldShS, Point( 92, -704, 177.8),
           	AprilTags::TagDetection(5)));
	}



	$nodeclass Report : VisualRoutinesStateNode : doStart {
    	cout << "Robot stops at " << theAgent->getCentroid() << endl;
       	cout << "Robot Orientation is " << theAgent->getOrientation() << endl;
		postStateCompletion();
	}

	$nodeclass Localize : PilotNode(PilotTypes::localize) : doStart {
		
		HW2Part2::buildMap();
	}

	$setupmachine {

		rundemo: WalkForward(-500) =C=> Turn(1.5708) =C=> WalkForward(1500) =C=> Turn(-1.5708) =C=> WalkForward(1500)
			=C=> Turn(-1.5708) =C=> WalkForward(3000) =C=> Turn(-1.5708) =C=> WalkForward(1500) =C=> Turn(-1.5708)
			=C=> WalkForward(1500) =C=> Turn(-1.5708) =C=> WalkForward(500) =C=> Report =C=> Localize =C=> Report =C=> 
			PostMachineCompletion	
	}
}

REGISTER_BEHAVIOR(HW2Part2);