//-*-c++-*-
#ifndef INCLUDED_DisplayPath_h_
#define INCLUDED_DisplayPath_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 that creates and displays the path the arm takes to move around an obstacle */
class DisplayPath : 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.2, 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(ellipseDataType, "pink");
      mapreq.addObjectColor(ellipseDataType, "blue");
      mapreq.addObjectColor(ellipseDataType, "green");
    }
  };
  
  class ExecutePath : public DynamicMotionSequenceNode {
  public:
    ExecutePath() : DynamicMotionSequenceNode("ExecutePath") {}
    
    virtual void DoStart() {
      int const numJoints = 3;
      RRTStateVector stateVec(3000, numJoints);
      const KinematicJoint*  goodroot = kine->getKinematicJoint(ArmShoulderOffset);
      
      KinematicJoint *n = const_cast<KinematicJoint*>(goodroot);
      
      vector<PlannerObstacle*> obstacles;
      NEW_SHAPEVEC(blobs, EllipseData, select_type<EllipseData>(localShS))
	SHAPEVEC_ITERATE(blobs, EllipseData, b)
	CircularObstacle * ob = new CircularObstacle();
      ob->centerPoint = fmat::pack(b->getCentroid().coordX(), b->getCentroid().coordY());
      ob->radius = max(b->getSemimajor(), b->getSemiminor()) / 2.0f;
      
      cout<<"Obstacle at "<<ob->centerPoint[0]<<","<<ob->centerPoint[1]<<" with radius "<<ob->radius<<endl;
      obstacles.push_back(ob);
      END_ITERATE;
      
      
      
      RRTPlanner rrtp(stateVec, goodroot, numJoints, NULL, 1000, 0.0001f, M_PI/60.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)
      */
      
      
      RRTStateDef startSt(numJoints), endSt(numJoints);
      
      for(unsigned int i = 0; i < (int)numJoints; i++) {
	startSt[i] = state->outputs[ArmShoulderOffset + i];
	n->setQ(startSt[i]);
	n = n->nextJoint();
      }
      
      endSt[0] = startSt[0] > 0 ? -1.2 : 1.4;
      endSt[1] = 0;
      endSt[2] = 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])));
      
      RRTPath path;
      if ( !rrtp.Plan(path, startSt, endSt, &obstacles) ) {
	cout << "No path" << endl;
	postStateCompletion();
	return;
      }
      
      getMC()->clear();
      generatePostures(path);
      getMC()->play();
    }
    
    void generatePostures(const RRTPath& path) {
      cout << "Making postures..." << endl;
      getMC()->setTime(3000);
      
      /* Posture engine used to emulate the outside world */
      PostureEngine newEngine(WorldState().getCurrent());
      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]);
	  /* Set outputs on the posture engine */
	  newEngine.setOutputCmd(ArmOffset+j, path[i][j]);
	}
	/* Transform from joint values to local coordinates using linkToBase 
	 * from the posture engine */
	fmat::Transform Tgripper = newEngine.linkToBase(GripperFrameOffset);
	fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));

	/* Create a shape in the local world coordinates representing the position of the arm */
	NEW_SHAPE(point, PointData, new PointData(localShS, Point(target[0], target[1], target[2], egocentric)));
	/* Set the color of the shape */
	if(i == 0)
	  point.getData().setColor("green");
	else if(i == path.size()-1)
	  point.getData().setColor("pink");
	else
	  point.getData().setColor("blue");
	cout << endl;
	getMC()->advanceTime(500);
      }
    }
  };
  
  
  DisplayPath() : VisualRoutinesStateNode("DisplayPath") {}
  
  virtual void setup() {
#statemachine
  startnode: LookDown() =C=> GoHome() =C=> 
      loop: SayReady() =B(GreenButOffset)=>
      FindObstacles() =MAP=>
      ExecutePath() =C=> loop
#endstatemachine
      }
};

#endif