#include "Behaviors/StateMachine.h"

$nodeclass Amap {

  $nodeclass Look : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    //mapreq.addObjectColor(cylinderDataType,"red");
		//mapreq.pursueShapes = true;
		//mapreq.searchArea = Lookout::groundSearchPoints();
		//mapreq.setVerbosity = -1U;
		mapreq.setAprilTagFamily();
		mapreq.clearWorld = true;
  }

	$nodeclass Report : doStart {
		Shape<AprilTagData> tag1 = AprilTagData::findTag(worldShS, 1);
		fmat::Quaternion tag_q = fmat::Quaternion();
		if ( tag1.isValid() ) {
			tag_q = tag1->getQuaternion();
			fmat::Column<3> tag_ypr = tag_q.ypr();
			cout << "Tag " << tag1->getId() << " id=" << tag1->getTagID()
					 << " orientation=" << tag1->getOrientation()
					 << " quat=" << tag1->getQuaternion()
					 << " yaw=" << tag_ypr[0]*180/M_PI << " pitch=" << tag_ypr[1]*180/M_PI << " roll=" << tag_ypr[2]*180/M_PI
					 << endl;
		}
		fmat::Quaternion cam_q = kine->getKinematicJoint(CameraFrameOffset)->getWorldQuaternion();
		fmat::Column<3> cam_ypr = cam_q.ypr();
		cout << "   Camera quat=" << cam_q
				 << " yaw=" << cam_ypr[0]*180/M_PI << " pitch=" << cam_ypr[1]*180/M_PI << " roll=" << cam_ypr[2]*180/M_PI
				 << endl;
		fmat::Quaternion tw_q = tag_q * cam_q;
		fmat::Column<3> tw_ypr = tw_q.ypr();
		cout << "   Tag world quat=" << tw_q
				 << " yaw=" << tw_ypr[0]*180/M_PI << " pitch=" << tw_ypr[1]*180/M_PI << " roll=" << tw_ypr[2]*180/M_PI
				 << endl;


  }

  $setupmachine{
  	look: Look =C=> Report =TM=> look
  }

}

REGISTER_BEHAVIOR(Amap);