#!/usr/bin/env python
import rospy
from geometry_msgs.msg  import Twist
from geometry_msgs.msg import Pose
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
import itertools
import socket
from math import pow,atan2,sqrt

class turtlebot():

    def __init__(self):
        #Creating our node,publisher and subscriber
        rospy.init_node('my_controller', anonymous=True)
        self.velocity_publisher = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=10)
        self.pose_subscriber = rospy.Subscriber('/odom', Odometry, self.callback)
        self.odom = Odometry()
        self.pose = Pose()
        self.rate = rospy.Rate(10)
        self.kz = 2.0
        self.kx = 0.2

    #Callback function implementing the pose value received
    def callback(self, data):
        self.odom = data
        self.pose = self.odom.pose.pose
        self.pose.position.x = round(self.pose.position.x, 6)
        self.pose.position.y = -round(self.pose.position.y, 6)

    def get_distance(self, goal_x, goal_y):
        distance = sqrt(pow((goal_x - self.pose.position.x), 2) + pow((goal_y - self.pose.position.y), 2))
        return distance

    def move2goal(self):
        goal_pose = Pose()
        goal_pose.position.x = 2.0#input("Set your X goal:")
        goal_pose.position.y = 0.0#input("Set your Y goal:")
        distance_tolerance = 0.05#input("Set your tolerance:")
        vel_msg = Twist()

        # self.pose.position.x self.pose.position.y & condicao de parada
        while self.get_distance(goal_pose.position.x, goal_pose.position.y) >= distance_tolerance and not rospy.is_shutdown():

            #Porportional Controller
            #linear velocity in the x-axis:
            vel_msg.linear.x = .5# + 2 * sqrt(pow((goal_pose.position.x - self.pose.position.x), 2) + pow((goal_pose.position.y - self.pose.position.y), 2))
            vel_msg.linear.y = 0
            vel_msg.linear.z = 0

            #angular velocity in the z-axis:
            vel_msg.angular.x = 0
            vel_msg.angular.y = 0
            angle = atan2(goal_pose.position.y - self.pose.position.y, goal_pose.position.x - self.pose.position.x)
            (roll,pitch,yaw) = euler_from_quaternion([self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w])
            theta = -yaw

            gama = theta
            phi = angle
            sgn = 1
            if ((theta > 1.57 or theta < -1.57) and (angle > 1.57 or angle < -1.57)):
                sgn = -sgn
                if theta > 0: gama = -(theta - 3.1416)
                else: gama = -(theta + 3.1416)
                if angle > 0: phi = -(angle - 3.1416)
                else: phi = -(angle + 3.1416)

            #elif (theta < -1.57 and angle < -1.57): # 3Quad
			#	gama = -(theta + 3.1416)
			#	phi = -(angle + 3.1416)

            error = sgn*(phi - gama)
            vel_msg.angular.z = self.kz * error

            print ("-----------------------")
            print ("angle : " + str(phi))
            print ("theta : " + str(gama))
            print ("error : " + str(error))
            print ("linear.x  : " + str(vel_msg.linear.x))
            print ("angular.z : " + str(vel_msg.angular.z))
            print ("x : " + str(self.pose.position.x))
            print ("y : " + str(self.pose.position.y))
            #Publishing our vel_msg
            self.velocity_publisher.publish(vel_msg)
            self.rate.sleep()

        #Stopping our robot after the movement is over
        vel_msg.linear.x = 0
        vel_msg.angular.z =0
        self.velocity_publisher.publish(vel_msg)
        #rospy.spin()

if __name__ == '__main__':

    #Testing our function
    x = turtlebot()
    try:
        x.move2goal()

    except rospy.ROSInterruptException: pass
    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        x.velocity_publisher.publish(twist)
