#! usr#!/usr/bin/env python

import rospy
from math import pi
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from time import sleep

def shutdown():
    """
    This function runs the shutdown procedure of the program when Ctrl + C is pressed
    """
    rospy.loginfo("Stopping TurtleBot...")   #prints to hidden log file
    pub.publish(Twist())    #sends an empty velocity object to the robot's motor in the /cmd_vel topic
    rospy.sleep(1)  #sleeps the program for 1 second to prevent interference from other nodes, and to give time for unfinished commands to finish



def callback(msg):
    print('=========================')
    #print('s1 [0]')
    #print(msg.ranges[0])  #distance at angle 0, which is directly in front of robot

    #print('s2 [90]')
    #print(msg.ranges[90])  #distance at angle 90 degrees

    #print('s3 [180]')
    #print(msg.ranges[180])  #distance at angle 180 degrees

    print('s4 [270]')
    print(msg.ranges[270])  #distance at angle 270 degrees

    #print('s5 [359]')
    #print(msg.ranges[359])  #distance at angle 359 degrees


    if 0.11 < msg.ranges[270] < 0.25:
        move.linear.x = 0.1
        move.angular.z = 0.0
        print('Moving forward')
        pub.publish(move)
        sleep(0.1)
    if msg.ranges[270] == 0.0 or msg.ranges[270] > 0.25:
        move.linear.x = 0.0
        move.angular.z = -1 * pi
        print('Turning')
        pub.publish(move)
        sleep(0.2)
        move.angular.z = 0.0
        pub.publish(move)
    if 0.01 < msg.ranges[0] == 0.1:
        move.linear.x = -0.1
        move.angular.z = 0.0
        print('Backward')
        pub.publish(move)
        sleep(0.2)




        #print('Obstable in front')





rospy.init_node('obstacle_avoidance')                          #Initiate node
sub = rospy.Subscriber('/scan', LaserScan, callback)    #Create subscriber to laser/scan topic
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
move = Twist()

rospy.spin()