//-*-c++-*-
#ifndef INCLUDED_Lab10Display_h_
#define INCLUDED_Lab10Display_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 Lab10Display : public VisualRoutinesStateNode {
public:

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

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

    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() {
            vector<PlannerObstacle*> obstacles;
            NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(localShS))
                SHAPEVEC_ITERATE(ellipses, EllipseData, e)
                CircularObstacle * ob = new CircularObstacle();
            ob->centerPoint = fmat::pack(e->getCentroid().coordX(), e->getCentroid().coordY());
            ob->radius = max(e->getSemimajor(), e->getSemiminor()) / 2.0f;
            cout<<"Obstacle at "<<ob->centerPoint[0]<<","<<ob->centerPoint[1]<<" with radius "<<ob->radius<<endl;
            obstacles.push_back(ob);
            END_ITERATE;
			
            unsigned int const numJoints = 3;
            RRTStateVector stateVec(6000, numJoints);
            const KinematicJoint*  goodroot = kine->getKinematicJoint(ArmShoulderOffset);
            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 < numJoints; i++)
                startSt[i] = state->outputs[ArmShoulderOffset + i];
            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) ) {
                postStateSignal<bool>(0);
                return;
            }

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

        void generatePostures(const RRTPath& path) {
            PostureEngine pos_eng;
            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]);
                    pos_eng.setOutputCmd(ArmOffset+j, path[i][j]);
                }
                Point pos(pos_eng.getJointPosition(GripperFrameOffset));
                NEW_SHAPE(temp, PointData, *new PointData(localShS, pos));
                if (i == 0) {
                    temp->setColor("green");
                } else if (i == path.size() - 1) {
                    temp->setColor("pink");
                } else {
                    temp->setColor("blue");
                }
                cout << endl;
                getMC()->advanceTime(1000);
            }
        }
    };


  Lab10Display() : 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