#ifndef __MANIP_OBJ_H
#define __MANIP_OBJ_H

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
#include "Motion/Kinematics.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/PIDMC.h"
#include "Motion/MotionPtr.h"
#include "Motion/XWalkMC.h"
#include "Shared/fmat.h"
#include "Shared/RobotInfo.h"
#include "Sound/SoundManager.h"

#include "DualCoding/DualCoding.h"
#include "Behaviors/Nodes/ArmNode.h"

#include "Planners/RRT/RRTPlanner.h"
#include "Sound/SoundManager.h"


#include "Behaviors/StateMachine.h"

using namespace DualCoding;

#define LEFT            1
#define RIGHT           0
#define ARM_LEN         180

//static variables used to undo the reach ball motion.
static int hit_dir;
static float x_dis;
static float y_dis;
static float phi_dis;

//position of ball and target
Point ball_pos;
Point target_pos;

//next way point
extern Point next_pos;
extern float next_ori;

/**
@brief
    Given a ball position and where we want to hit to, this class will 
    calculate a pose for the robot to reach the ball and able to hit it 
    towards the target side ways. 
    The robot can hit ball to either left of right, thus it will calculate 
    the pose for both configuration, and pick one that is closer. 
    Some frame transformation is necessary here, but the actual math is simple.
*/
class ReachBall : public XWalkNode, public VRmixin {
public:
    ReachBall() : XWalkNode(), VRmixin() {}
    float dist(float x, float y) {
        return sqrt(x*x + y*y);
    }

    virtual void DoStart() {
   		fmat::Transform T = kine->linkToBase(ArmOffset);
        fmat::Column<3> arm_loc = fmat::SubVector<3>(&T(0,3));       
        Point *arm = new Point(arm_loc);	

        Point diff = target_pos - ball_pos;
        float theta = atan2(diff.coordY(), diff.coordX());

        // frame transformation stuff, I have expanded the matrix 
        // multiplication.
        // hit left
        float arm_x_l = -ARM_LEN*sin(theta)+ball_pos.coordX();
        float arm_y_l = +ARM_LEN*cos(theta)+ball_pos.coordY();
        float robo_x_l = -sin(theta)*arm->coordX()-cos(theta)*arm->coordY()
                    -ARM_LEN*sin(theta)+ball_pos.coordX();
        float robo_y_l = cos(theta)*arm->coordX()-sin(theta)*arm->coordY()
                    +ARM_LEN*cos(theta)+ball_pos.coordY();
        float phi_l = theta - M_PI / 2;
         
        printf("phi %f\n", phi_l);
        // hit right
        float arm_x_r = ARM_LEN*sin(theta)+ball_pos.coordX();
        float arm_y_r = -ARM_LEN*cos(theta)+ball_pos.coordY();
        float robo_x_r = sin(theta)*arm->coordX()+cos(theta)*arm->coordY()
                    +ARM_LEN*sin(theta)+ball_pos.coordX();
        float robo_y_r = -cos(theta)*arm->coordX()+sin(theta)*arm->coordY()
                    -ARM_LEN*cos(theta)+ball_pos.coordY();
        float phi_r = theta + M_PI / 2;

        printf("phi %f\n", phi_r);
            
        float dist_l = dist(robo_x_l, robo_y_l);
        float dist_r = dist(robo_x_r, robo_y_r);

        // pick the smaller distance one
        if (dist_l < dist_r) {
            x_dis = robo_x_l;
            y_dis = robo_y_l;
            phi_dis = phi_l;
            hit_dir = LEFT;
                
            getMC()->setTargetDisplacement(robo_x_l, robo_y_l, phi_l);

            printf("hit left\n");
            //for debugging purposes                        
            Point *temp = new Point();
            temp->coords[0] = robo_x_l;
            temp->coords[1] = robo_y_l;
            Point *temp1 = new Point();
            temp1->coords[0] = arm_x_l;
            temp1->coords[1] = arm_y_l;
            NEW_SHAPE(robot_Pos, PointData, new PointData(localShS, *temp));
            NEW_SHAPE(arm_Pos, PointData, new PointData(localShS, *temp1));
            NEW_SHAPE(ball_Pos, PointData, new PointData(localShS, ball_pos));
            NEW_SHAPE(target_Pos, PointData, new PointData(localShS, target_pos));
        }
        else {
            x_dis = robo_x_r;
            y_dis = robo_y_r;
            phi_dis = phi_r;
            hit_dir = RIGHT;
 
            getMC()->setTargetDisplacement(robo_x_r, robo_y_r, phi_r);
                
            printf("hit right\n");
            //for debugging purposes                        
            Point *temp = new Point();
            temp->coords[0] = robo_x_r;
            temp->coords[1] = robo_y_r;
            Point *temp1 = new Point();
            temp1->coords[0] = arm_x_r;
            temp1->coords[1] = arm_y_r;
            NEW_SHAPE(robot_Pos, PointData, new PointData(localShS, *temp));
            NEW_SHAPE(arm_Pos, PointData, new PointData(localShS, *temp1));
            NEW_SHAPE(ball_Pos, PointData, new PointData(localShS, ball_pos));
            NEW_SHAPE(target_Pos, PointData, new PointData(localShS, target_pos));
        }          
    }
};
   
/**
@brief
    Used to look down at the arm
*/
class LookDown : public HeadPointerNode {
public:
    LookDown() : HeadPointerNode("LookDown") {}
    virtual void DoStart() {
    	getMC()->setJoints(0, 0.167, -1);
	}
};

/**
@brief
    Build a local map to find the ball near arm.
*/
class FindObstacles : public MapBuilderNode {
public:
	FindObstacles() : MapBuilderNode("FindObstacles",MapBuilderRequest::localMap) {}

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

/**
@brief 
    This node uses RRT to move arm to a ready state, so that the arm can
    hit the ball perpendicularly to the desired direction, which will be 
    calculated in reachBall class. This is basically taken from Dave' example
    with some change for the destination of path planning. 
*/
class AdjustArm : public DynamicMotionSequenceNode, public VRmixin {
public:
	AdjustArm() : DynamicMotionSequenceNode("ExecutePath"), VRmixin() {}

	virtual void DoStart() {
		vector<PlannerObstacle*> obstacles;
        Point ball;
		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());
            // padding the size
	    	ob->radius = max(e->getSemimajor(), e->getSemiminor()) / 1.5;
		    cout << "Obstacle at " <<
                ob->centerPoint[0] << "," << ob->centerPoint[1] << " with radius " << ob->radius << endl;
    		obstacles.push_back(ob);
            //////////////
            ball = e->getCentroid();
            /////////////
		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];
        
        fmat::Transform T = kine->linkToBase(ArmOffset);
        fmat::Column<3> arm_loc = fmat::SubVector<3>(&T(0,3));       
        Point *arm = new Point(arm_loc);
        Point arm2ob = ball - *arm;
        float angle = atan2(arm2ob.coordY(), arm2ob.coordX());
        ////////////////////////////////////////////////
        // left
        float t_angle;
        if (hit_dir == LEFT)
            t_angle = angle - 0.4;
        // right
	    else 
		    t_angle = angle + 0.4;
        endSt[0] = t_angle;
        endSt[1] = t_angle;
        endSt[2] = t_angle;
	    ////////////////////////////////////////////////

		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) ) {
			return;
		}

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

	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(1000);
		}
	}
};

/**
@brief
    Swing the arm to specified direction.
*/
class HitBall: public ArmNode {
public:
    HitBall(): ArmNode("PushObjects") {}
	    
    void DoStart() {
        getMC()->setMaxSpeed(0, 3);
	    getMC()->setMaxSpeed(1, 3);
        getMC()->setMaxSpeed(2, 3);
            
        getMC()->setJointValue(1, 0);
	    getMC()->setJointValue(2, 0);

        if (hit_dir == LEFT) {
            getMC()->setJointValue(0, 0.8);
        }
        else {
	        getMC()->setJointValue(0, -0.8);
        }
    }
};

/**
@brief
    Because we are doing everything in local frame, after the robot reach ball
    and hit it, it needs to go back to where it starts. So that we can reuse
    the information during the map building phase. This class is used to 
    restore the pose.
*/
class ResetPose : public XWalkNode {
public:
    ResetPose() : XWalkNode() {}
    virtual void DoStart() {
        printf("reset: %f %f %f\n", -x_dis, -y_dis, -phi_dis);
        getMC()->setTargetDisplacement(-x_dis, -y_dis, -phi_dis);
    }
};

/**
@brief
    Move the robot to the next way point.
*/
class Move2WayPoint: public XWalkNode {
public:
    Move2WayPoint(): XWalkNode() {}

    virtual void DoStart() {
        float next_angle = atan2(next_pos.coordY(), next_pos.coordX());
        printf("=============> turn %f %f %f \n", next_pos.coordX(), next_pos.coordY(), next_angle);
        getMC()->setTargetDisplacement(next_pos.coordX(), next_pos.coordY(), next_angle);
    }
};

/**
@brief
    Rotate the robot at way point, so that it can face the centroid of the
    convex hull.
*/
class RotAtWayPoint: public XWalkNode {
public:
    RotAtWayPoint(): XWalkNode() {}

    virtual void DoStart() {
        getMC()->setTargetDisplacement(0, 0, next_ori);
    }
};

#endif