#include "Shared/RobotInfo.h"
#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Planners/Manipulation/ShapeSpacePlanner3DR.h"
#include "Sound/SoundManager.h"
#include "local/DeviceDrivers/MirageComm.h"

using namespace std;
using namespace DualCoding;

$nodeclass ArmPathPlan : VisualRoutinesStateNode {
	
	$provide std::vector<ShapeSpacePlanner3DR<NumArmJoints-2>::NodeValue_t> pathResult;

	$nodeclass FindObstacles : MapBuilderNode : doStart {
		mapreq.addObjectColor(cylinderDataType, "red");
	}
	
	$nodeclass ExecutePath : DynamicMotionSequenceNode : doStart {
		$reference ArmPathPlan::pathResult;

		NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(localShS))
		
		GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
		ShapeSpacePlanner3DR<NumArmJoints-2> planner(VRmixin::worldShS, worldBounds, 10, GripperFrameOffset, NULL);
		
		ShapeSpacePlanner3DR<NumArmJoints-2>::NodeValue_t startSt, endSt;
		for (unsigned int i = 0; i < NumArmJoints-2; i++)
			startSt[i] = state->outputs[ArmOffset + i];
		
		endSt[0] = startSt[0] > 0 ? -1.2f : 1.4f;
		for (unsigned int i = 1; i < NumArmJoints-2; i++)
			endSt[i] = 0;
		
		const KinematicJoint* gripper = kine->getKinematicJoint(GripperFrameOffset);
		fmat::Column<3> where = gripper->getWorldPosition();
		NEW_SHAPE(tgt, PointData, new PointData(localShS, DualCoding::Point(where[0], where[1], where[2])));
		
		ShapeSpacePlanner3DR<NumArmJoints-2>::NodeValue_t value;
		fmat::Transform t;
		DualCoding::Point c = VRmixin::theAgent->getCentroid();
		t.translation()[0] = c.coordX();
		t.translation()[1] = c.coordY();
		t.rotation() = fmat::Matrix<3,3>(fmat::rotation2D(VRmixin::theAgent->getOrientation()));
		GenericRRTBase::PlanPathResultCode result = planner.planPath(startSt, endSt, value, t, 40000, &pathResult, NULL, NULL).code;
		if (result != GenericRRTBase::SUCCESS) {
			cout << "result: " << result << endl;
			postStateFailure();
			return;
		}
	}
		
	$nodeclass GeneratePostures : DynamicMotionSequenceNode : doStart {
		$reference ArmPathPlan::pathResult;

		getMC()->clear();
		cout << "Making postures..." << endl;
		getMC()->setTime(1000);

		for(unsigned int i = 0; i < pathResult.size(); i++) {
			cout << "Step " << i << "   ";
			for(unsigned int j = 0; j < NumArmJoints-2; j++) {
				cout << "  " << j << ": " << pathResult[i][j];
				getMC()->setOutputCmd(ArmOffset+j, (float)pathResult[i][j]);
			}
			cout << endl;
			getMC()->advanceTime(200);
		}
		getMC()->play();
	}
	
	$setupmachine {
		obs: FindObstacles() =C=>
		exec: ExecutePath()
		exec =C=> GeneratePostures =N=> SpeechNode("done")
		exec =F=> SpeechNode("No path found.")
	}
};

REGISTER_BEHAVIOR(ArmPathPlan);