/*!
	 * \brief Executes a valid trajectory.
	 *
	 * Move the arm to a goal configuration in Joint Space
	 * \param goal JointTrajectoryGoalConstPtr
	 */
	void executeTrajectory(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
	{
		ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
		if(!executing_)
		{
			while(current_operation_mode_ != "velocity")
			{
				ROS_INFO("waiting for arm to go to velocity mode");
				usleep(100000);
			}
			traj_ = goal->trajectory;

			if(traj_.points.size() == 1)
			{
				traj_generator_->moveThetas(traj_.points[0].positions, q_current);
			}

			else
			{
				//Insert the current point as first point of trajectory, then generate SPLINE trajectory
				trajectory_msgs::JointTrajectoryPoint p;
				p.positions.resize(7);
				p.velocities.resize(7);
				p.accelerations.resize(7);
				for(unsigned int i = 0; i<7; i++)
				{
					p.positions.at(i) = q_current.at(i);
					p.velocities.at(i) = 0.0;
					p.accelerations.at(i) = 0.0;
				}
				std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
				it = traj_.points.begin();
				traj_.points.insert(it,p);
				traj_generator_->moveTrajectory(traj_, q_current);
			}

			executing_ = true;
			startposition_ = q_current;

			//TODO: std::cout << "Trajectory time: " << traj_end_time_ << " Trajectory step: " << traj_step_ << "\n";
		}

		else //suspend current movement and start new one
		{
		}

		while(executing_)
		{
			sleep(1);
		}

		as_.setSucceeded();
	}
 bool srvCallback_setAcc(cob_trajectory_controller::SetFloat::Request &req, cob_trajectory_controller::SetFloat::Response &res)
 {
     ROS_INFO("Setting acceleration to %f", req.value.data);
     traj_generator_->SetPTPacc(req.value.data);
     res.success.data = true;
     return true;
 }
	/*!
	 * \brief Run the controller.
	 *
	 * The Controller generates desired velocities for every joints from the current positions.
	 *
	 */
	void run()
	{
		if(executing_)
		{
			watchdog_counter = 0;
			if (as_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
			{
				/// set the action state to preempted
				executing_ = false;
				traj_generator_->isMoving = false;
				ROS_INFO("Preempted trajectory action");

				return;
			}

			std::vector<double> des_vel;
			if(traj_generator_->step(q_current, des_vel))
			{
				if(!traj_generator_->isMoving) //Trajectory is finished
				{
					executing_ = false;
				}

				brics_actuator::JointVelocities target_joint_vel;
				target_joint_vel.velocities.resize(7);
				for(unsigned int i=0; i<7; i++)
				{
					std::stringstream joint_name;
					joint_name << "arm_" << (i+1) << "_joint";
					target_joint_vel.velocities[i].joint_uri = joint_name.str();
					target_joint_vel.velocities[i].unit = "rad";
					target_joint_vel.velocities[i].value = des_vel.at(i);
				}

				/// send everything
				joint_vel_pub_.publish(target_joint_vel);
			}

			else
			{
				ROS_INFO("An controller error occured!");
				executing_ = false;
			}

		}

		else
		{
			/// WATCHDOG TODO: don't always send
			if(watchdog_counter < 10)
			{
				sensor_msgs::JointState target_joint_position;
				target_joint_position.position.resize(7);
				brics_actuator::JointVelocities target_joint_vel;
				target_joint_vel.velocities.resize(7);
				for (unsigned int i = 0; i < 7; i += 1)
				{
					std::stringstream joint_name;
					joint_name << "arm_" << (i+1) << "_joint";
					target_joint_vel.velocities[i].joint_uri = joint_name.str();
					target_joint_position.position[i] = 0;
					target_joint_vel.velocities[i].unit = "rad";
					target_joint_vel.velocities[i].value = 0;
				}

				joint_vel_pub_.publish(target_joint_vel);
				joint_pos_pub_.publish(target_joint_position);
			}

			watchdog_counter++;
		}
	}
 void run()
 {
     if(executing_)
     {
         failure_ = false;
         watchdog_counter = 0;
         //if (as_follow_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
         if (!ros::ok() || current_operation_mode_ != "velocity")
         {
             // set the action state to preempted
             executing_ = false;
             traj_generator_->isMoving = false;
             //as_.setPreempted();
             failure_ = true;
             return;
         }
         if (as_follow_.isPreemptRequested())
         {
             //as_follow_.setAborted()
             failure_ = true;
             preemted_ = true;
             ROS_INFO("Preempted trajectory action");
             return;
         }
         std::vector<double> des_vel;
         if(traj_generator_->step(q_current, des_vel))
         {
             if(!traj_generator_->isMoving) //Finished trajectory
             {
                 executing_ = false;
                 preemted_ = false;
             }
             brics_actuator::JointVelocities target_joint_vel;
             target_joint_vel.velocities.resize(DOF);
             for(int i=0; i<DOF; i++)
             {
                 target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
                 target_joint_vel.velocities[i].unit = "rad";
                 target_joint_vel.velocities[i].value = des_vel.at(i);
             }
             
             //send everything
             joint_vel_pub_.publish(target_joint_vel);
         }
         else
         {
             ROS_INFO("An controller error occured!");
             failure_ = true;
             executing_ = false;
         }
     }
     else
     {  //WATCHDOG TODO: don't always send
         if(watchdog_counter < 10)
         {
             brics_actuator::JointVelocities target_joint_vel;
             target_joint_vel.velocities.resize(DOF);
             for (int i = 0; i < DOF; i += 1)
             {
                 target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
                 target_joint_vel.velocities[i].unit = "rad";
                 target_joint_vel.velocities[i].value = 0;
             }
             joint_vel_pub_.publish(target_joint_vel);
             ROS_INFO("Publishing 0-vel (%d)", DOF);
         }
         watchdog_counter++;
     }
 }
 void spawnTrajector(trajectory_msgs::JointTrajectory trajectory)
 {
     if(!executing_ || preemted_)
     {
         //set component to velocity mode
         cob_srvs::SetOperationMode opmode;
         opmode.request.operation_mode.data = "velocity";
         srvClient_SetOperationMode.call(opmode);
         ros::Time begin = ros::Time::now();
         while(current_operation_mode_ != "velocity")
         {
             ROS_INFO("waiting for component to go to velocity mode");
             usleep(100000);
             //add timeout and set action to rejected
             if((ros::Time::now() - begin).toSec() > velocity_timeout_)
             {
                 rejected_ = true;
                 return;
             }  
         }
         
         std::vector<double> traj_start;
         if(preemted_ == true) //Calculate trajectory for runtime modification of trajectories
         {
             ROS_INFO("There is a old trajectory currently running");
             traj_start = traj_generator_->last_q;
             trajectory_msgs::JointTrajectory temp_traj;
             temp_traj = trajectory;
             //Insert the saved point as first point of trajectory, then generate SPLINE trajectory
             trajectory_msgs::JointTrajectoryPoint p;
             p.positions.resize(DOF);
             p.velocities.resize(DOF);
             p.accelerations.resize(DOF);
             for(int i = 0; i<DOF; i++)
             {
                 p.positions.at(i) = traj_start.at(i);
                 p.velocities.at(i) = 0.0;
                 p.accelerations.at(i) = 0.0;
             }
             std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
             it = temp_traj.points.begin();
             temp_traj.points.insert(it,p);
             //Now insert the current as first point of trajectory, then generate SPLINE trajectory
             for(int i = 0; i<DOF; i++)
             {
                 p.positions.at(i) = traj_generator_->last_q1.at(i);
                 p.velocities.at(i) = 0.0;
                 p.accelerations.at(i) = 0.0;
             }
             it = temp_traj.points.begin();
             temp_traj.points.insert(it,p);
             for(int i = 0; i<DOF; i++)
             {
                 p.positions.at(i) = traj_generator_->last_q2.at(i);
                 p.velocities.at(i) = 0.0;
                 p.accelerations.at(i) = 0.0;
             }
             it = temp_traj.points.begin();
             temp_traj.points.insert(it,p);
             for(int i = 0; i<DOF; i++)
             {
                 p.positions.at(i) = traj_generator_->last_q3.at(i);
                 p.velocities.at(i) = 0.0;
                 p.accelerations.at(i) = 0.0;
             }
             it = temp_traj.points.begin();
             temp_traj.points.insert(it,p);
             for(int i = 0; i<DOF; i++)
             {
                 p.positions.at(i) = q_current.at(i);
                 p.velocities.at(i) = 0.0;
                 p.accelerations.at(i) = 0.0;
             }
             it = temp_traj.points.begin();
             temp_traj.points.insert(it,p);
             traj_generator_->isMoving = false ;
             traj_generator_->moveTrajectory(temp_traj, traj_start);
         }
         else //Normal calculation of trajectories
         {
             traj_start = q_current;
             trajectory_msgs::JointTrajectory temp_traj;
             temp_traj = trajectory;
             if(temp_traj.points.size() == 1)
             {
                 traj_generator_->isMoving = false ;
                 traj_generator_->moveThetas(temp_traj.points[0].positions, traj_start);
             }
             else
             {
                 //Insert the current point as first point of trajectory, then generate SPLINE trajectory
                 trajectory_msgs::JointTrajectoryPoint p;
                 p.positions.resize(DOF);
                 p.velocities.resize(DOF);
                 p.accelerations.resize(DOF);
                 for(int i = 0; i<DOF; i++)
                 {
                     p.positions.at(i) = traj_start.at(i);
                     p.velocities.at(i) = 0.0;
                     p.accelerations.at(i) = 0.0;
                 }
                 std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
                 it = temp_traj.points.begin();
                 temp_traj.points.insert(it,p);
                 traj_generator_->isMoving = false ;
                 traj_generator_->moveTrajectory(temp_traj, traj_start);
             }
         }
         
         executing_ = true;
         startposition_ = q_current;
         preemted_ = false;
         
     }
     else //suspend current movement and start new one
     {
     }
     while(executing_)
     {
         if(!preemted_)
         {
             usleep(1000);
         }
         else
         {
             return;
         }
     }
 }