include "ros/ros.h"

include "gazebo_msgs/SetModelState.h"

include "gazebo_msgs/GetModelState.h"

include "gazebo_msgs/GetPhysicsProperties.h"

include <stdio.h>

include <math.h>

include <stdlib.h>

int main(int argc, char** argv)

{

ros::init(argc, argv, "kouna_to_asistola2");

ros::NodeHandle n;

double f, t, th, a, x, y, e, p, csa, sna, u, w, Xfinal[3], Xinit[3];

double pi=3.141592654;

int g = 3; int h = 1; int k = 6;

int i;

geometry_msgs::Pose pose;

geometry_msgs::Twist twist;

twist.linear.x = 0.0; twist.linear.y = 0.0;

Xinit[0]=0; Xinit[1]=0; Xinit[2]=0;

Xfinal[0]=6.0; Xfinal[1]=7.0; Xfinal[2]=86.0;

for(i=1; i<100000; i+=1) {

ros::ServiceClient gmscl=n.serviceClient<gazebo_msgs::getmodelstate>("/gazebo/get_model_state");

ros::ServiceClient gphspro=n.serviceClient<gazebo_msgs::getphysicsproperties>("/gazebo/get_physics_properties");

ros::ServiceClient client = n.serviceClient<gazebo_msgs::setmodelstate>("/gazebo/set_model_state");

gmscl.waitForExistence(); gphspro.waitForExistence(); client.waitForExistence();

gazebo_msgs::GetModelState getmodelstate; getmodelstate.request.model_name ="turtlebot"; gmscl.call(getmodelstate);

gazebo_msgs::GetPhysicsProperties getphysicsproperties; gphspro.call(getphysicsproperties);

pose.position.z = 0.0;
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;


twist.linear.z = 0.0;
twist.angular.x = 0.0;
twist.angular.y = 0.0;


t=0.0001;

ROS_INFO("position.x = %f",pose.position.x); ROS_INFO("position.y = %f",pose.position.y); ROS_INFO("orientation = %f",pose.orientation.z);

x = Xfinal[0] - Xinit[0];

y = Xfinal[1] - Xinit[1];

f = Xfinal[2] - Xinit[2];

p = atan2(y,x); p = p*180/pi;

th = Xfinal[2] - p;

a = th - f;

if(a<0) a=-a;

csa = cos(api/180); sna = sin(api/180);

e = sqrt(xx + yy);

//Υπολογισμός γραμμικής ταχύτητας - u u = (egcsa);

//Υπολογισμός γωνιακής ταχύτητας - ω w = (ka + ((gcsasna(a + h*th))/a));

twist.angular.z = w; twist.linear.x = sqrt(uu - twist.linear.ytwist.linear.y); twist.linear.y = sqrt(uu - twist.linear.xtwist.linear.x);

if(Xinit[0] <= Xfinal[0]) pose.position.x = getmodelstate.response.pose.position.x+twist.linear.x*t)); else pose.position.x = getmodelstate.response.pose.position.x+0.0;

if(Xinit[1] <= Xfinal[1]) pose.position.y = getmodelstate.response.pose.position.y+twist.linear.y*t)); else pose.position.y = getmodelstate.response.pose.position.y+0.0;

if(Xinit[2] <= Xfinal[2]) pose.orientation.z = getmodelstate.response.pose.orientation.z+(twist.angular.z*t); else pose.orientation.z = getmodelstate.response.pose.orientation.z+0.0;

Xinit[0]=pose.position.x; Xinit[1]=pose.position.y; Xinit[2]=pose.orientation.z;

// ros::ServiceClient client = n.serviceClient<gazebo_msgs::setmodelstate>("/gazebo/set_model_state");

gazebo_msgs::SetModelState setmodelstate;
gazebo_msgs::ModelState modelstate;
modelstate.model_name ="turtlebot";
modelstate.pose = pose;
modelstate.twist = twist;

setmodelstate.request.model_state=modelstate;       

 if (client.call(setmodelstate))
  {
   ROS_INFO("BRILLIANT AGAIN!!!");
  }
 else
  {
   ROS_ERROR("Failed to call service for setmodelstate ");
   return 1;
  }

u=0.0; twist.angular.z=0.0;

}

}