#! /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 = 5.6
max_turn = 90
count=0
right_avg_arr = [0,0,0,0,0,0,0,0,0,0]
left_avg_arr = [0,0,0,0,0,0,0,0,0,0]

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[:250])
	left_average = mean(sensor_data[250:])
	result = left_average - right_average
	#print "Right: " + str(right_average)
	#print "Left: " + str(left_average)
	#print "Result" + str(result)
	result = result / max_range
	result = result * max_turn
	#print "Turn Result" + str(result)
	return result

def callback(sensor_data):
	#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
		
	
#	sector_first= mean(data_sns[50:120])
#	sector_sec = mean(data_sns[120:200])
#	sector_front = mean(data_sns[200:300])
#	sector_third = mean(data_sns[300:380])
#	sector_forth = mean(data_sns[380:450])
#
#	sector_array = []
#	sector_

	

	
	arc_start = 100
	arc_end = 400
	detection_distance = 0.5
	for i in range(len(data_sns[arc_start:arc_end])-4):
                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:
		                                        base_data.linear.x = 0
							base_data.angular.z = turn(data_sns)
							#print "Turning"

	global count	
	right_avg = mean(data_sns[:200])
	left_avg = mean(data_sns[301:])
	right_avg_arr[count] = right_avg
	left_avg_arr[count] =  left_avg
	#print left_avg
	#print right_avg
	if count == 9:
		print "#################################################################"
		tot_right = 0
		tot_left = 0
	
		for x in right_avg_arr:
			tot_right = tot_right + x
		res_right = tot_right/len(right_avg_arr)

		for y in left_avg_arr:
			tot_left = tot_left + y
		res_left = tot_left/len(left_avg_arr)

		if res_right > 4.5 or res_left > 4.5:
			diff_means = res_right - res_left
			print diff_means	
			if (diff_means >= 0):
				base_data.angular.z = -45
			else:
				base_data.angular.z = 45	
		count = 0
	count = count+1
		
		
	#rand = random.randint(0,200)
	#print rand
	#if rand==5:
	#	print "RANDOM"
	#	base_data.angular.z = 22
	

	pub.publish(base_data)

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()

	
