//-*-c++-*-
#ifndef INCLUDED_Lab10_h_
#define INCLUDED_Lab10_h_

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Planners/RRT/RRTPlanner.h"
#include "Sound/SoundManager.h"

using namespace std;
using namespace DualCoding;

class Lab10 : public VisualRoutinesStateNode {
public:

	class LookDown : public HeadPointerNode {
	public:
		LookDown() : HeadPointerNode("LookDown") {}

		virtual void DoStart() {
			getMC()->setJoints(0, -0.167, -1.2);
		}
	};

	class GoHome : public ArmNode {
	public:
		GoHome() : ArmNode("GoHome") {}

		virtual void DoStart() {
			getMC()->setJoints(1.4, 0, 0);
		}
	};

	class SayReady : public StateNode {
	public:
		SayReady() : StateNode("SayReady") {}

		virtual void DoStart() {
			cout << "Ready." << endl;
			sndman->speak("ready");
		}
	};

	class FindObstacles : public MapBuilderNode {
	public:
		FindObstacles() : MapBuilderNode("FindObstacles",MapBuilderRequest::localMap) {}

		virtual void DoStart() {
			mapreq.addObjectColor(blobDataType, "pink");
			mapreq.addObjectColor(blobDataType, "blue");
			mapreq.addObjectColor(blobDataType, "green");
		}
	};

	class ExecutePath : public DynamicMotionSequenceNode {
	public:
		ExecutePath() : DynamicMotionSequenceNode("ExecutePath") {}

		virtual void DoStart() {
			vector<PlannerObstacle*> obstacles;
			float rotM[] = {1, 0, 0, 1};
			NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS))
			SHAPEVEC_ITERATE(blobs, BlobData, b)
				RectangularObstacle *ob = new RectangularObstacle;
  			float bbox[] = {
					b->topLeft.coordX(),
					b->topLeft.coordY(),
					b->bottomLeft.coordX(),
					b->bottomLeft.coordY(),
					b->bottomRight.coordX(),
					b->bottomRight.coordY(),
					b->topRight.coordX(),
					b->topRight.coordY() };
 				ob->rot = rotM;
				ob->points = bbox;
				obstacles.push_back(ob);
			END_ITERATE;
				
			int const numJoints = 3;
			RRTStateDef startSt(numJoints), endSt(numJoints);
			startSt[0] = state->outputs[ArmShoulderOffset];
			startSt[1] = 0;
			startSt[2] = 0; 
			endSt[0] = startSt[0] > 0 ? -1.2 : 1.4;
			endSt[1] = 0;
			endSt[2] = 0;
			
			RRTStateVector stateVec(1000, numJoints);
			const KinematicJoint*  goodroot = kine->getKinematicJoint(ArmShoulderOffset);
			RRTPlanner rrtp(stateVec, goodroot, numJoints, NULL, 1000, 0.4f, M_PI/30.0f, M_PI/120.0f);
    /* Generate the instance of the planner
       stateVec		- RRTStateVector* for where to store the states used in planning
       goodroot		- The start of the kinematics chain you want to use
       numJoints (optional)	- How many joints down the kinematic chain you want to go
       1000 (optional)		- The maximum number of iterations you want the planner to go through
       0.4f (optional)		- The allowed error between two states to call them "equal". Make lower for better results, but may not converge
       M_PI/30.0f (optional)	- The largest change each joint can change by between states
       M_PI/120.0f (optional)	- The largest change each joint can change by during an interpolation check (finer grain so it won't miss any obstacles)
    */

			RRTPath path;
			if ( !rrtp.Plan(path, startSt, endSt, &obstacles) ) {
				cout << "No path" << endl;
				postStateCompletion();
				return;
			}

			/*** My code ***/
			PostureEngine *fake = new PostureEngine();
			generateFakePostures(fake, path);
			/***************/

			getMC()->clear();
			generatePostures(path);
			getMC()->play();
		}

		/*** My modified version of generatePostures that draws graph ***/
		void generateFakePostures(PostureEngine *&fake, const RRTPath &path) {
			rgb red, green, blue;
			
			red.red = blue.blue = green.green = 255;
			green.red = green.blue = red.blue = red.green = blue.red = blue.green = 0;

			fake->clear();
			
			for(int i = 0; i < path.size(); i++) {
				for(int j = 0; i < path[i].size(); j++)
					fake->setOutputCmd(ArmOffset+j, path[i][j]);

				const KinematicJoint* gripper = fake->getKinematicJoint(GripperFrameOffset);
				fmat::Column<3> grip_pos = gripper->getWorldPosition();
				PointData *target = new PointData(localShS, DualCoding::Point(grip_pos[0],grip_pos[1],grip_pos[2]));
				
				if (i == path.size() -1)
					target->setColor(blue);
				else if(i > 0)
					target->setColor(red);
				else
					target->setColor(green);

				NEW_SHAPE(dot, PointData, target); 
			}
			return;
		}
		/***************************************************************************/

		void generatePostures(const RRTPath& path) {
			cout << "Making postures..." << endl;
			getMC()->setTime(3000);
			for(unsigned int i = 0; i < path.size(); i++) {
				cout << "Step " << i << "   ";
				for(unsigned int j = 0; j < path[i].size(); j++) {
					cout << "  " << j << ": " << path[i][j];
					getMC()->setOutputCmd(ArmOffset+j, path[i][j]);
				}
				cout << endl;
				getMC()->advanceTime(2000);
			}
		}
	};


  Lab10() : VisualRoutinesStateNode("Labl10") {}

	virtual void setup() {
#statemachine
  startnode: LookDown() =C=> 
			loop: SpeechNode("ready") =B(GreenButOffset)=>
  		FindObstacles() =MAP=>
			exec: ExecutePath() =C=> loop
			exec =S<bool>=> SpeechNode("No path found") =T(2500)=> loop
#endstatemachine
			}

};

#endif