import rospy
from threading import Thread
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
import math

a = True
x = True
b = False
c = None
d = False
g = True
def callback(data):
    global a, velocity_publisher, vel_msg, b
    if(data.ranges[0] != float('inf') and a == True):
        if(data.ranges[0] >= 0.39):
            vel_msg.linear.x = 0.2
            velocity_publisher.publish(vel_msg)
        else:
            vel_msg.linear.x = 0
            velocity_publisher.publish(vel_msg)
            a = False
            b = True

def odomcallback(data1):
    global velocity_publisher, x, vel_msg, b, c, d, g
    if(b == True):
        c = input()
        b = False
    t1 = +2.0 * (data1.pose.pose.orientation.w * data1.pose.pose.orientation.z + data1.pose.pose.orientation.x * data1.pose.pose.orientation.y)
    t2 = +1.0 - 2.0 * (data1.pose.pose.orientation.y ** 2 + data1.pose.pose.orientation.z**2)
    print math.degrees(math.atan2(t1, t2))
    if(math.degrees(math.atan2(t1, t2)) < 178.0 and c != None and d == False):
        vel_msg.angular.z = 0.5
        velocity_publisher.publish(vel_msg)
    elif(math.degrees(math.atan2(t1, t2)) >178.0 and d == False):
        vel_msg.angular.z = 0.0
        velocity_publisher.publish(vel_msg)
        d = True

    if (data1.pose.pose.position.x > 0.000 and a == False and c != None and d ==True):
        vel_msg.linear.x = 0.2
    elif(x == True and a == False and c != None and d == True):
        vel_msg.linear.x = 0.0
        x = False

    if (g == True and math.degrees(math.atan2(t1, t2)) < 178.0 and x == False):
        vel_msg.angular.z = -0.5
        velocity_publisher.publish(vel_msg)
    elif (g == True and x == False):
        vel_msg.angular.z = 0.0
        velocity_publisher.publish(vel_msg)
        g = False


    velocity_publisher.publish(vel_msg)



if __name__ == '__main__':
    velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    rospy.init_node('listener', anonymous=True)
    vel_msg = Twist()
    rospy.Subscriber("/scan", LaserScan, callback)
    rospy.Subscriber('/odom',Odometry,odomcallback)
    rospy.spin()