void GrabPuckServer::controlLoop()
{
	double vel_x = 0, vel_y = 0, vel_phi = 0;
	ROS_DEBUG("%s", GrabPuckStates::toString(state_).c_str());
	double percentage = 0;
	if (!loaded_ && (state_ == grab_puck_states::ALIGNING_LATERAL || state_ == grab_puck_states::HEADING_TOWARD_PUCK || state_ == grab_puck_states::GRABBING_PUCK))
	{
		robotino_vision::FindObjects srv;
		srv.request.color = Colors::toCode(color_);
		if (!find_objects_cli_.call(srv))
		{	
			ROS_ERROR("Puck not found!!!");
			state_ = grab_puck_states::LOST;
			return;
		}
		std::vector<float> distances = srv.response.distances;
		std::vector<float> directions = srv.response.directions; 
		int num_objects = srv.response.distances.size();
		if (num_objects > 0 && state_ != grab_puck_states::GRABBING_PUCK)
		{
			int closest_index = 0;
			for (int i = 0; i < num_objects; i++)
			{
				if (distances[i] < distances[closest_index])
				{
					closest_index = i;
				}
			}
			double max_error_lateral = 50, max_error_frontal = 40;
			double error_lateral = directions[closest_index];
			if (error_lateral > max_error_lateral)
			{
				 error_lateral = max_error_lateral;
			}
			double error_frontal = distances[closest_index];
			if (error_frontal > max_error_frontal)
			{
				 error_frontal = max_error_frontal;
			}
			float tolerance_lateral = 0.1, tolerance_frontal = 35;
			double K_error_lateral = 0.3, K_error_frontal = 0.005;
			double percentage_f, percentage_0, tolerance, max_error, error;
			if (fabs(error_lateral) > tolerance_lateral) // 0% a 49%
			{
				state_ = grab_puck_states::ALIGNING_LATERAL;
				vel_y = -K_error_lateral * error_lateral;
				percentage_0 = 0;
				percentage_f = 49;
				tolerance = tolerance_lateral;
				max_error = max_error_lateral;
				error = fabs(error_lateral);
			}
			else if (fabs(error_frontal) > tolerance_frontal) // 50% a 69%
			{
				state_ = grab_puck_states::HEADING_TOWARD_PUCK;
				vel_x = K_error_frontal * error_frontal;
				percentage_0 = 50;
				percentage_f = 69;
				tolerance = tolerance_frontal;
				max_error = max_error_frontal;
				error = fabs(error_frontal);
			}
			else 
			{
				vel_x = .2;
				state_ = grab_puck_states::GRABBING_PUCK;
				grabbing_start_ = ros::Time::now();
			}
			percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
		}
		else if (state_ == grab_puck_states::GRABBING_PUCK) // 70% a 79%
		{
			state_ = grab_puck_states::GRABBING_PUCK;
			vel_x = .2;
			double percentage_0 = 70, percentage_f = 79;
			double elapsed_time = (ros::Time::now() - grabbing_start_).toSec();
			if (elapsed_time > GRABBING_DEADLINE)
			{
				state_ = grab_puck_states::LOST;
				ROS_ERROR("Grabbing state deadline expired!!!");
			}
			percentage = percentage_0 + (percentage_f - percentage_0) * elapsed_time / GRABBING_DEADLINE;
		}		
	}
	else 
	{		
		if (state_ == grab_puck_states::GRABBING_PUCK)
		{
			ros::Duration d(.5);
			d.sleep();
			delta_x_ = -getOdometry_X();
			resetOdometry();
			state_ = grab_puck_states::ROTATING;
			percentage_ = 79;
		}
		double max_error_linear = 1.25;
		double error_linear = getOdometry_X() - delta_x_;
		if (error_linear > max_error_linear)
		{
			 error_linear = max_error_linear;
		}
		double max_error_angular = PI;
		double error_angular = PI - getOdometry_PHI();
		if (error_angular > max_error_angular)
		{
			 error_angular = max_error_angular;
		}
		float tolerance_linear = 0.1, tolerance_angular = 0.2;
		double K_error_linear = 1, K_error_angular = 0.6;
		double percentage_f, percentage_0, tolerance, max_error, error;
		if (fabs(error_angular) > tolerance_angular) // 80% a 89%
		{
			state_ = grab_puck_states::ROTATING;
			vel_phi = K_error_angular * error_angular;
			percentage_0 = 80;
			percentage_f = 89;
			tolerance = tolerance_angular;
			max_error = max_error_angular;
			error = fabs(error_angular);
			
		}
		else if (fabs(error_linear) > tolerance_linear) // 90% a 99%
		{
			state_ = grab_puck_states::GOING_BACK_TO_ORIGIN;
			vel_x = K_error_linear * error_linear;
			percentage_0 = 91;
			percentage_f = 99;
			tolerance = tolerance_linear;
			max_error = max_error_linear;
			error = fabs(error_linear);			
		}
		percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
	}
	if (vel_x == 0 && vel_y == 0 && vel_phi == 0) // 100%
	{
		if(!loaded_ramp_)
		{
			result_.goal_achieved = false;
			result_.message = "Grab fail!!!";
			server_.setAborted(result_, result_.message);
			ROS_ERROR("%s", result_.message.c_str());
		}
		else
		{
			state_ = grab_puck_states::FINISHED;
			percentage_ = 100;
		}
	}
	if (percentage > percentage_)
	{
		percentage_ = percentage;
	}
	setVelocity(vel_x, vel_y, vel_phi);
	publishVelocity();
	publishFeedback();
}
void StorePuckServer::controlLoop()
{
	double vel_x = 0, vel_y = 0, vel_phi = 0;
	ROS_DEBUG("%s", StorePuckStates::toString(state_).c_str());
	double percentage = 0;
	if (mode_ == store_modes::LASER_SCAN)
	{
		robotino_motion::AlignGoal frontal_alignment, lateral_alignment;
		frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
		frontal_alignment.distance_mode = 1; // NORMAL distance mode
		//lateral_alignment.alignment_mode = 9; // RIGHT_LASER alignment mode
		//lateral_alignment.distance_mode = 1; // NORMAL distance mode
		align_client_.sendGoal(frontal_alignment);
		state_ = store_puck_states::ALIGNING_FRONTAL;
		align_client_.waitForResult();

		ROS_INFO("Store_number_: %s", StoreStoreNumbers::toString(store_number_).c_str());
		switch(store_number_)
		{
			case store_store_numbers::NEAR:
				{
				while(laser_front_ > 0.13)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
				//setVelocity(0.0, 0.0, 0.0);
				//publishVelocity();
				}
				break;
			case store_store_numbers::MIDDLE:
				{
					printf("entrou no MIDDLE");
					while(laser_front_ > 0.78)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
				//setVelocity(0.0, 0.0, 0.0);
				//publishVelocity();
				}
				break;
			case store_store_numbers::FAR_AWAY:
				{
					printf("entrou no FAR_AWAY");
					while(laser_front_ > 1.43)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
					//setVelocity(0.0, 0.0, 0.0);
					//publishVelocity();
				}
				break;
			case store_store_numbers::VOLTAR_PRA_CASA:
				{
					frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
					frontal_alignment.distance_mode = 1; // NORMAL distance mode
					align_client_.sendGoal(frontal_alignment);
					state_ = store_puck_states::ALIGNING_FRONTAL;
					align_client_.waitForResult();

					while(laser_front_ > 0.13)
					{
						setVelocity(0.15, 0, 0);
						publishVelocity();
					}
					while(laser_left_ > 0.3)
					{
						setVelocity(0, 0.1, 0);
						publishVelocity();
					}
					flag_aux_ = true;
				}
				break;
			default:
				ROS_ERROR("Store Number not supported yet!!!");
		}

		//laserDistanceFront(store_number_);

		if(flag_aux_ == false)
		{
			frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
			frontal_alignment.distance_mode = 1; // NORMAL distance mode
			align_client_.sendGoal(frontal_alignment);
			state_ = store_puck_states::ALIGNING_FRONTAL;
			align_client_.waitForResult();

			resetOdometry();
			while(getOdometry_PHI() < PI / 2)
			{
				setVelocity(0, 0, 0.5);
				publishVelocity();
			}
			frontal_alignment.alignment_mode = 8; // FRONT_LASER alignment mode
			frontal_alignment.distance_mode = 1; // NORMAL distance mode
			align_client_.sendGoal(frontal_alignment);
			state_ = store_puck_states::ALIGNING_FRONTAL;
			align_client_.waitForResult();

			state_ = store_puck_states::STORING_PUCK;
			while(laser_front_ > 0.33)
			{
				setVelocity(0.15, 0, 0);
				publishVelocity();
			}
			state_ = store_puck_states::LEAVING_PUCK;
			while(laser_front_ < 0.8)
			{
				setVelocity(-0.2, 0, 0);
				publishVelocity();
			}
			if(laser_right_ < 0.6 || laser_left_ < 0.6)
			{
				while(laser_right_ < 0.6)
				{
					setVelocity(0, 0.2, 0);
					publishVelocity();
				}
				while(laser_left_ < 0.6)
				{
					setVelocity(0, -0.2, 0);
					publishVelocity();
				}
			}
		}
	}
	else if (mode_ == store_modes::VISION)
	{
		if (state_ != store_puck_states::LEAVING_PUCK && state_ != store_puck_states::GOING_BACK_TO_ORIGIN)
		{
			robotino_vision::FindInsulatingTapeAreas srv;
			find_areas_cli_.waitForExistence();
			if (!find_areas_cli_.call(srv))
			{	
				ROS_ERROR("Area not found!!!");
				state_ = store_puck_states::LOST;
				return;
			}
			std::vector<float> distances = srv.response.distances;
			std::vector<float> directions = srv.response.directions; 
			int num_areas = srv.response.distances.size();
			if (num_areas > 0 && state_ != store_puck_states::STORING_PUCK)
			{
				int closest_area_index = 0;
				for (int i = 0; i < num_areas; i++)
				{
					if (distances[i] < distances[closest_area_index] && fabs(directions[i]) <fabs(directions[closest_area_index]))
					{
						closest_area_index = i;
					}
				}
				double max_error_lateral = 50, max_error_frontal = 40;
				double error_lateral = directions[closest_area_index];
				if (error_lateral > max_error_lateral)
				{
					 error_lateral = max_error_lateral;
				}
				double error_frontal = distances[closest_area_index];
				if (error_frontal > max_error_frontal)
				{
					 error_frontal = max_error_frontal;
				}
				float tolerance_lateral = 0.1, tolerance_frontal = 30;
				double K_error_lateral = 0.3, K_error_frontal = 0.003;
				double percentage_f, percentage_0, tolerance, max_error, error;
				if (fabs(error_lateral) > tolerance_lateral) // 0% a 49%
				{
					state_ = store_puck_states::ALIGNING_LATERAL;
					vel_y = -K_error_lateral * error_lateral;
					percentage_0 = 0;
					percentage_f = 49;
					tolerance = tolerance_lateral;
					max_error = max_error_lateral;
					error = fabs(error_lateral);
				}
				else if (fabs(error_frontal) > tolerance_frontal) // 50% a 69%
				{
					state_ = store_puck_states::HEADING_TOWARD_AREA;
					vel_x = K_error_frontal * error_frontal;
					percentage_0 = 50;
					percentage_f = 69;
					tolerance = tolerance_frontal;
					max_error = max_error_frontal;
					error = fabs(error_frontal);
				}
				else 
				{
					vel_x = .14;
					state_ = store_puck_states::STORING_PUCK;
					storing_start_ = ros::Time::now();
				}
				percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
			}
			else if (state_ == store_puck_states::STORING_PUCK) // 70% a 89%
			{
				vel_x = .1;
				double percentage_0 = 70, percentage_f = 89;
				double elapsed_time = (ros::Time::now() - storing_start_).toSec();
				if (elapsed_time > STORING_DEADLINE)
				{
					state_ = store_puck_states::LEAVING_PUCK;
				}
				percentage = percentage_0 + (percentage_f - percentage_0) * elapsed_time / STORING_DEADLINE;
			}
		}
		else
		{
			if (state_ == store_puck_states::LEAVING_PUCK)
			{
				delta_x_ = -getOdometry_X();
				vel_x = -.14;
				resetOdometry();
				state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
				percentage_ = 89;
			}
			double max_error_linear = 1.25;
			double error_linear = delta_x_ -  getOdometry_X();
			if (error_linear > max_error_linear)
			{
				 error_linear = max_error_linear;
			}
			float tolerance_linear = 0.01;
			double K_error_linear = 1.2;
			double percentage_f, percentage_0, tolerance, max_error, error;
			if (fabs(error_linear) > tolerance_linear) // 90% a 99%
			{
				state_ = store_puck_states::GOING_BACK_TO_ORIGIN;
				vel_x = K_error_linear * error_linear;
				percentage_0 = 91;
				percentage_f = 99;
				tolerance = tolerance_linear;
				max_error = max_error_linear;
				error = fabs(error_linear);			
			}
			percentage = percentage_0 + (percentage_f - percentage_0) * (max_error - error) / (max_error - tolerance);
		}
	}
	else
	{
		ROS_ERROR("Storage Mode not supported yet!!!");
		return;
	}
	if (vel_x == 0 && vel_y == 0 && vel_phi == 0) // 100%
	{
		state_ = store_puck_states::FINISHED;
		percentage_ = 100;
	}
	if (percentage > percentage_)
	{
		percentage_ = percentage;
	}
	setVelocity(vel_x, vel_y, vel_phi);
	publishVelocity();
	publishFeedback();
}