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

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

import math
import time


def shutdown():
    # stop turtlebot
    rospy.loginfo("Stop TurtleBot")
    # a default Twist has linear.x of 0 and angular.z of 0.  So it'll stop TurtleBot
    pub.publish(Twist())
    # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
    rospy.sleep(1)


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

    for i in range(15):
        if msg.ranges[i] > 0.5:
            move.linear.x = 0.3
            move.angular.z = 0.0
            print("Moving forward")

        elif msg.ranges[i] <= 0.5:
            move.linear.x = 0.0
            move.angular.z = 0.3
            print("Obstacle in front")

    for i in range(345,359):
        if msg.ranges[i] > 0.5:
            move.linear.x = 0.3
            move.angular.z = 0.0
            print("Moving forward")

        elif msg.ranges[i] <= 0.5:
            move.linear.x = 0.0
            move.angular.z = -0.3
            print("Obstacle in front")

    pub.publish(move)


rospy.init_node('obstacle_avoidance')  # Initiate node
rospy.loginfo("To stop TurtleBot CTRL + C")
rospy.on_shutdown(shutdown)                             #Specifies what the turtlebot should do once Ctrl+C is pressed

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

rospy.spin()