#include <stdio.h>
#include <vector>
#include <math.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <numeric>
#include <interactive_markers/interactive_marker_server.h>
#include <interactive_markers/menu_handler.h>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
#include "std_msgs/String.h"
#include <trajectory_planner/coordinates.h>

//namepaces
using namespace visualization_msgs;

// Global Vars
boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
ros::NodeHandle* p_n;
ros::Publisher coor_pub;
trajectory_planner::coordinates message;



void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	if( feedback->mouse_point_valid )
	{
		//mouse_point_ss << " at " << feedback->mouse_point.x
			//<< ", " << feedback->mouse_point.y
			//<< ", " << feedback->mouse_point.z
			//<< " in frame " << feedback->header.frame_id;
	}

	switch ( feedback->event_type )
	{
		case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
			message.x=feedback->pose.position.x; message.y=feedback->pose.position.y; message.theta=2.0*asin(feedback->pose.orientation.z);

		message.header.stamp = ros::Time::now();
		message.header.frame_id = "/world";

		coor_pub.publish(message);
			
			break;
	}

	server->applyChanges();
}


Marker makeBox( InteractiveMarker &msg )
{
	Marker marker;

	marker.type = Marker::ARROW;
	marker.scale.x = msg.scale;
	marker.scale.y = msg.scale;
	marker.scale.z = msg.scale;
	marker.color.r = 0;
	marker.color.g = 0.7;
	marker.color.b = 0;
	marker.color.a = 0.6;
	return marker;
}


InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
{
	InteractiveMarkerControl control;
	control.always_visible = true;
	control.markers.push_back( makeBox(msg) );
	msg.controls.push_back( control );

	return msg.controls.back();
}


void make6DofMarker( bool fixed )
{
	InteractiveMarker int_marker;
	int_marker.header.frame_id = "/world";
	int_marker.pose.position.x = 0;
	int_marker.pose.position.y = 0;
	int_marker.pose.position.z = 0;
	int_marker.scale = 0.4;

	int_marker.name = "Control target";
	int_marker.description = "Control the final \nposition of the robot";

	// insert a box
	makeBoxControl(int_marker);
	InteractiveMarkerControl control;

	control.orientation.w = 1;
	control.orientation.x = 1;
	control.orientation.y = 0;
	control.orientation.z = 0;
	control.name = "move_x";
	control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
	int_marker.controls.push_back(control);

	control.orientation.w = 1;
	control.orientation.x = 0;
	control.orientation.y = 0;
	control.orientation.z = 1;
	control.name = "move_y";
	control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
	int_marker.controls.push_back(control);

	control.orientation.w = 1;
	control.orientation.x = 0;
	control.orientation.y = 1;
	control.orientation.z = 0;
	control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
	control.name = "rotate_z";
	int_marker.controls.push_back(control);

	server->insert(int_marker);
	server->setCallback(int_marker.name, &processFeedback);
}


int main(int argc, char **argv)
{
	ros::init(argc, argv, "APgenerator");
	ros::NodeHandle n("~");
	ros::Rate loop_rate(10);
	p_n=&n;

	coor_pub = n.advertise<trajectory_planner::coordinates>("/msg_coordinates", 1000);
	
	
	server.reset( new interactive_markers::InteractiveMarkerServer("APgenerator/im","",false) );
	make6DofMarker( true );
	server->applyChanges();
	
	while(ros::ok())
	{
		
			
		
		loop_rate.sleep();
		ros::spinOnce();
	}

	server.reset();
}