#include "ros/ros.h"

#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include "robotino_msgs/BHAReadings.h"
#include "robotino_msgs/SetBHAPressures.h"
#include <sstream>
#include <vector>
#include <iostream>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"
#include "std_msgs/Float32MultiArray.h"

// declare global vars
void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array);
int Arr[2];


int main(int argc, char **argv)
{
    int targetX=135;
    int targetY=150;
    float posGain=.001;
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Subscriber ball_sub = n.subscribe("/obj_pos", 100, arrayCallback);

    ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1, true);
    ros::Publisher bha_pub = n.advertise<std_msgs::Float32MultiArray>("/bha_pose", 1, true);
    ros::Publisher gripper_pub = n.advertise<std_msgs::String>("/bha_gripper", 100);

    ros::Rate loop_rate(10);

    int count = 0;

    // loop one approach the ball
    while (ros::ok())
    {
        std::cout<< "x: " << abs(targetY-Arr[1]) << std::endl;
        std::cout<< "y: " << abs(targetX-Arr[0]) << std::endl;

        geometry_msgs::Twist cmd_vel_msg_;

        cmd_vel_msg_.linear.x 	= 0;
        cmd_vel_msg_.linear.y  	= 0;
        cmd_vel_msg_.angular.z 	= 0;

        // move the vehicle so that the ball is in the target location on the camera screen.
        if(Arr[0]>0)
        {
            if(abs(targetY-Arr[1])>2)  cmd_vel_msg_.linear.x 	= (targetY-Arr[1])*posGain;
            if(abs(targetX-Arr[0])>2)  cmd_vel_msg_.linear.y  	= (targetX-Arr[0])*posGain;
            std::cout<< "Vel set "<< std::endl;
        }

        // once we have reached the target location within tolerance break from the loop
        if(abs(targetY-Arr[1])<2  &&  abs(targetX-Arr[0])<5)
        {
            break;
        }

        chatter_pub.publish(cmd_vel_msg_);

        // publish the pose
        float pose[6]= { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203}; // straight arm pose

        std_msgs::Float32MultiArray msg;
        for (int i = 0; i < 6; i++)
        {
            msg.data.push_back(pose[i]);
        }

        bha_pub.publish(msg);

        ros::spinOnce();

        loop_rate.sleep();

        ++count;
    }

// seccond step  move the arm to pickup location
    {
        // broadcast new pose
        float pose[6]= {0.803812,0.459433,0.498534, 0.982405, 0.694037,0.855327};
        std_msgs::Float32MultiArray msg;

        for (int i = 0; i < 6; i++)
        {
            msg.data.push_back(pose[i]);
        }

        bha_pub.publish(msg);



        ros::Duration(5).sleep();  // wait 5 secconds before closing gripper

        // close the gripper
        std_msgs::String gripmsg;
        std::stringstream ss;
        ss << "tolclose" << count;
        gripmsg.data = ss.str();
        gripper_pub.publish(gripmsg);

        ros::Duration(10).sleep();  // wait 5 secconds before closing gripper
    }


    // set arm back to origional position
    std::cout<< "loop3------ " << std::endl;
    while (ros::ok())
    {

        float pose[6]= { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203};
        std_msgs::Float32MultiArray msg;
        for (int i = 0; i < 6; i++)
        {
            msg.data.push_back(pose[i]);
        }
        bha_pub.publish(msg);
        ros::spinOnce();

        loop_rate.sleep();

        ++count;
    }

    //end program
    return 0;
}


void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array)
{

    int i = 0;
    // print all the remaining numbers
    for(std::vector<int>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
    {
        Arr[i] = *it;
        i++;
    }

    return;
}