#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from turtlesim.msg  import Pose
from sensor_msgs.msg import LaserScan
from math import pi
from math import atan2

goal = (9.5, 1.5)
perpendicular_compare = 0
state = 0
previous_front = 0


# funkcja wywolywana przy przyjsciu danych ze skanera laserowego
move_x = 1
move_theta = 1
def choose_direction (directions):
	num_in_list = 0
	global angle
	global dir
	global front
	global back
	global turn_left_detector
	global turn_right_detector
	global perpendicular
	global head_min
	head_min = min(min(directions[0:10]), min(directions[350:359]))
	dir = min(directions)
	for direction in directions:
		if (direction == dir):
			print "INDEX", num_in_list
			angle = num_in_list*2*pi/360
			print "LOOP ANGLE", angle
		else:
			num_in_list += 1
	turn_left_detector = directions[45]
	#turn_right_detector = direstions[]
	front = directions[70]
	back = directions[110]
	perpendicular = directions[90]
	return (dir, angle, front, back, turn_left_detector)

def check_front():
	global previous_front
	if (previous_front == 0):
		previous_front = front
		return False
	else:
		if(front > previous_front + 0.5):
			return True
		else:
			previous_front = front
			return False
	

def scan_callback(scan):
	choose_direction(scan.ranges)
	#print "ANGLE", angle
	#print "DIR", dir
	#print scan
	
			
def count_dest_angle(c2y, c1y, c2x, c1x):
	global dest_angle
	dest_angle = atan2 (c2y-c1y, c2x-c1x)
	return dest_angle

angle_to_goal = float(atan2(0.5-0.5, 9.5-0.5))

def drive_to_finish (position_theta):
	global state
	if abs(position_theta - angle_to_goal) > 0.1 and dir > 0.4:
		if position_theta <= angle_to_goal:
			new_vel.angular.z = 0.3
		elif position_theta > angle_to_goal:
			new_vel.angular.z = -0.3
		new_vel.linear.x = 0
		state = 1
		print ("Rotating to goal angle")
	else:
		state = 1
		#print ("Following goal angle")
		print ("DISTOWALL", dir)
		print ("FRONT", front)
		print ("BACK", back)
		if  dir >= 0.4:
			new_vel.angular.z = 0
			new_vel.linear.x = 0.3
			print ("GO AHEAD")
		elif dir < 0.4:
			#jezeli najblizsza odleglosc od sciany jest pod katem 90 lub 270 stopni to jedz do przodu
			# or (round(angle, 2) != (round((pi, 2)*1.5))))
			#(round(angle, 2) != (round((pi*0.5, 2))))
			if (round(front, 1) != round(back, 1) or perpendicular > 0.5):
				new_vel.angular.z = -0.3
				new_vel.linear.x = 0
				print ("ROTATE TO WALL FOLLOW")
			else:
				new_vel.linear.x = 0
				state = 3
					#if (perpendicular > 0.4 or (round(front, 1) != round(back, 1))):
					#	new_vel.linear.x = 0.3
					#	new_vel.angular.z = 0.3
					#
					#	state = 8
def drive_next_wall():
	global state
	if check_front() == False:	
		if (round(front, 2) == round(back, 2)):
			new_vel.linear.x = 0.4
			new_vel.angular.z = 0
			state = 3
			print ("LINE FOLLOW")
		else:
			if front >= back:
				new_vel.angular.z = 0.2
				new_vel.linear.x = 0.2
				state = 3
				print ("EQUALISE FROM FRONT TO BACK")
			else:
				new_vel.angular.z = -0.2
				new_vel.linear.x = 0.2
				state = 3
				print ("EQUALISE FROM BACK TO FRONT")
		return state
	elif check_front() == True:
		new_vel.angular.z = 0
		new_vel.linear.x = 0
		state = 4
		print ("LEFT TURN DETECTED")
def turn_left():
	global state
	#if (head_min < 1):
	if (perpendicular > 0.4 or (round(front, 1) == round(back, 1))):
		new_vel.linear.x = 0.04
		new_vel.angular.z = 0.04
		print ("TURN LEFT")
	else:
		state = 3
		print ("NEXT TO WALL AGAIN")

# funkcja wywolywana przy przyjsciu danych o lokalizacji robota
def odom_callback(odom):
	global new_vel
	global distance_to_goal
	#global perpendicular_compare
	pose = Pose()
	pose.x = odom.pose.pose.position.x
	pose.y = odom.pose.pose.position.y
	pose.theta = tf.transformations.euler_from_quaternion([odom.pose.pose.orientation.x,odom.pose.pose.orientation.y,odom.pose.pose.orientation.z,odom.pose.pose.orientation.w])[2]
	#print "Pozycja x: ",odom.pose.pose.position.x
	#print "Pozycja y: ",odom.pose.pose.position.y
	print "Pozycja theta: ",pose.theta
	print "Kat do celu:", angle_to_goal
	curr_angle_to_goal = count_dest_angle(9.5, pose.y, 1.5, pose.x)
	#print "ANGLE ABS", abs(angle_to_goal - curr_angle_to_goal)
	#print "ANGLE", angle
	#print "FRONT", front

	if state == 0 or state == 1:
		drive_to_finish(pose.theta)
	elif state == 3:
		drive_next_wall()
	elif state == 4:
		turn_left()


if __name__== "__main__":
	global new_vel
	new_vel = Twist()
	rospy.init_node('wr_zad', anonymous=True)
	print("ready")
	rospy.Subscriber( '/odom' , Odometry, odom_callback)
	pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
	rospy.Subscriber( '/scan' , LaserScan, scan_callback)
	
	rate=rospy.Rate(10) # 10Hz
	while not rospy.is_shutdown():
		pub.publish(new_vel)#wyslanie predkosci zadanej
		rate.sleep()

	print("END")