#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/Twist.h>

#define GIRO_IZQDA 1
#define GIRO_DCHA 2
#define RECTO 3
#define MAX_VEL 4
#define STOP 5
#define MED_VEL 6

int id = 0;
ros::Publisher action_pub;
geometry_msgs::Twist set_vel;

void objectCallback(const std_msgs::Float32MultiArrayPtr &object)
{
   if (object->data.size() > 0)
   {
      id = object->data[0];

      switch (id)
      {
      case GIRO_IZQDA:
         set_vel.linear.x = 0;
         set_vel.angular.z = 1;
         break;
      case RECTO:
         set_vel.linear.x = 1;
         set_vel.angular.z = 0;
         break;
      case GIRO_DCHA:
         set_vel.linear.x = 0;
         set_vel.angular.z = -1;
         break;
      case STOP:
         set_vel.linear.x = 0;
         set_vel.angular.z = 0;
         break;
      case MAX_VEL:
         set_vel.linear.x = 2;
         set_vel.angular.z = 0;
         break;
      default: // other object
         set_vel.linear.x = 0;
         set_vel.angular.z = 0;
      }
      action_pub.publish(set_vel);
   }
   else
   {
      // No object detected
      set_vel.linear.x = 0;
      set_vel.angular.z = 0;
      action_pub.publish(set_vel);
   }
}

int main(int argc, char **argv)
{

   ros::init(argc, argv, "action_controller");
   ros::NodeHandle n("~");
   ros::Rate loop_rate(50);
   ros::Subscriber sub = n.subscribe("/objects", 1, objectCallback);
   action_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
   set_vel.linear.x = 0;
   set_vel.linear.y = 0;
   set_vel.linear.z = 0;
   set_vel.angular.x = 0;
   set_vel.angular.y = 0;
   set_vel.angular.z = 0;
   while (ros::ok())
   {
      ros::spinOnce();
      loop_rate.sleep();
   }
}