#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from math import exp, sqrt, pi, isnan, isinf
import time
import random

speed = 0.2
max_range = 3
max_turn = 45
angle = 1
count = 0
turn_left = [0,0,0,0,0,0,0,0,0,0]
turn_right = [0,0,0,0,0,0,0,0,0,0]
already_turned = True

def mean(sensor_values):
	total = 0
	for data in sensor_values:
		total += data
	result = total / len(sensor_values)
	return result

def turn(sensor_data):
	right_average = mean(sensor_data[:200])
	left_average = mean(sensor_data[300:])
	result = left_average - right_average
	print "Right: " + str(right_average)
	print "Left: " + str(left_average)
	print "Result" + str(result)
	
	if result < 0:
		print "11111"
		result = -(angle)
	
	else:
		print "22222"
		result = angle
	return result

	

pastTime = 0
def callback(sensor_data):
	currentTime = time.time()
	if (currentTime - pastTime) > 0.1:
		print "calledback"

		data_sns = []

		for num, data in enumerate(sensor_data.ranges):
			if isnan(data):
				data_sns.append(0)
			elif isinf(data):
																																																																																																																																																						      			data_sns.append(max_range)
			else:
				data_sns.append(data)

		print(data_sns[200:300])

		base_data = Twist()

		base_data.linear.x = speed	
		print "linear" + str(base_data.linear.x)
	
		arc_start = 125
		arc_end = 375
		detection_distance = 0.5
		for i in range(len(data_sns[arc_start:arc_end]) - 5):
		        if data_sns[arc_start+i] < detection_distance:
		                if data_sns[arc_start+i+1] < detection_distance:
		                        if data_sns[arc_start+i+2] < detection_distance:
		                                if data_sns[arc_start+i+3] < detection_distance:
						        if data_sns[arc_start+i+4] < detection_distance:
						                if data_sns[arc_start+i+5] < detection_distance:
						                        base_data.linear.x = 0
									print "linear" + str(base_data.linear.x)
									turn_value = turn(data_sns)
									base_data.angular.z = turn_value
									print "angular" + str(turn_value)
					
									break
									#print "Turning"
		


		#global count
		#global already_turned

		#if already_turned:
		#	if count<9:
		#		turn_left[count] = mean(data_sns[50:150])
		#		turn_right[count] = mean(data_sns[350:450])
		#		count = count+1
		#	if count == 9:
		#		turn_left[count] = mean(data_sns[50:150])
		#		turn_right[count] = mean(data_sns[350:450])
		#		count = 0
		#	if mean(turn_left) > 4.7:
		#		
		#		base_data.angular.z = -30
		#		#already_turned = False
		#		print "LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL"
		#		print mean(turn_left)
		#	elif mean(turn_right) > 4.7:
		#		print "RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR"
		#		print mean(turn_right)
		#		base_data.angular.z = 30
				#already_turned = False

			
				

				


		pub.publish(base_data)
		global pastTime
		pastTime = currentTime
		print "end"

if __name__ == '__main__':
	rospy.init_node('reactive_mover_node')
	rospy.Subscriber('base_scan', LaserScan, callback)
	pub = rospy.Publisher('cmd_vel', Twist, queue_size=100)
	rospy.spin()

	
