#!/usr/bin/env python

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

# class ForceMapper():
#     def __init__(self):
#         self.r = rospy.Rate(250) # 250hz
#         self.thresh = 1
#         self.obstacle = "cryyyyyinging"
#         self.nearWall = 0
#         self.sub = rospy.Subscriber('/scan', LaserScan, self.callBack)
#         self.pub_obs = rospy.Publisher('obstacle', String, queue_size=10)
#         self.start()

#     def start(self):
#         while not rospy.is_shutdown():
#             self.pub_obs.publish(self.obstacle)
#             self.r.sleep()

#     def callBack(self, msg):
#         size = len(msg.ranges)
#         right = msg.ranges[0]
#         front = msg.ranges[size/2]
#         #frontLeft = msg.ranges[size*3/4]
#         #frontRight = msg.ranges[size/4]
#         left = msg.ranges[size-1]
#         print("right: ", right, size)

def callBack(msg):
    pos = Twist()
    pub = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)

    size = len(msg.ranges)
    front  = msg.ranges[size/2]
    right = msg.ranges[size/2 - 90]
    left = msg.ranges[size/2 + 90]
    print("right: ", right, size)
    if front < 1:
        pos.linear.x = -0.1
        pos.angular.z = -0.4
        pub.publish(pos)

    elif right < 1:
        pos.linear.x = 0.3
        pos.angular.z = 0.4
        pub.publish(pos)

    elif left < 1:
        pos.linear.x = 0.3
        pos.angular.z = -0.4
        pub.publish(pos)

    else:
        pos.linear.x = 0.3
        pos.angular.z = 0
        pub.publish(pos)


def main():
    rospy.init_node('ForceMapper')
    rospy.Subscriber("/scan", LaserScan, callBack)
    # Spin until ctrl + c
    rospy.spin()

if __name__ == '__main__':
    main()


    #if object close to front (always pass string)
    #array of three F,L,R
    #If no longer object to right(entrance) turn right to follow wall
    #three parrallel while loops
    #all in feelforce