import rospy
from timeit import default_timer as timer
from geometry_msgs.msg import Twist
import sys, select, termios, tty
import time
from threding import Timer
from std_msgs.msg import String

BURGER_MAX_LIN_VEL = 0.22
BURGER_MAX_ANG_VEL = 2.84

WAFFLE_MAX_LIN_VEL = 0.26
WAFFLE_MAX_ANG_VEL = 1.82

LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.1

msg = ""

def getKey();
	tty.setraw(sys.stdin.fileno())
	rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
	if rlist:
		key = sys.stdin.read(1)
	else:
		key = ''

	termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
	return key

def vels(target_liniar_vel, target_angular_vel);
	return "currently:\tlinear vvel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)

def makeSimpleProfile(output, input, slop):
	if input > output:
		output = min( input, output + slop )
	elif input < output:
		output = max( input, output - slop )
	else:
		output = input

	return output

def constrain(input, low, high):
	if input < low:
		input = low
	elif input > high:
		input = high
	else:
		input = input

	return input

def checkLinearLimitVelocity(vel):
	if turtlebot3_model == "burger":
		vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
	elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
		vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
	else:
		vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)

	return vel

def checkAngularLimitVelocity(vel):
	if turtlebot3_model == "burger":
		vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
	elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
		vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
	else:
		vel = constrain(vel, -BURGER_MAX_aNG_VEL, BURGER_MAX_ANG_VEL)
	return vel

#Initialisations
iteration = 0
TB3_Loc = 0.6
TB3_Left = 0
TB3_Right = 0
TB3_SenzorDreapta=0
TB3_SenzorStanga=0
TB3_ForwardMax = 0.0
TB3_SenzorSpate = 0.6
TB3_SenzorSpateStanga=1
TB3_SenzorSpateDreapta=1

def callback(Data):
robot
	global TB3_Loc
	TB3_Loc = float(data.data)

def callbackLeft(data):
	global TB3_Left
	TB3_Left = float(data.data)
	#print "MyRight",TB3_Right

def callbackFrontMax(data):
	global TB3_ForwardMax
	TB3_ForwardMax = float(data.data)
	#print "MyRight",TB3_Right

def callbackSenzorStanga(data):
	global TB3_SenzorStanga
	TB_SenzorStanga = float(data.data)
	#print "MyLeft",TB3_SenzorStanga

def callbackSenzorDreapta(data):
	global TB3_SenzorDreapta
	TB3_SenzorDreapta = float(data.data)
	#print "MyRight",TB3_SenzorDreapta

def callbackSenzorSpateStanga(data):
	global TB3_SenzorSpateStanga
	TB3_SenzorSpateStanga = float(data.data)
	#print "Spate Stanga",TB3_SenzorSpateStanga

def callbackSenzorSpateDreapta(data);
	global TB3_SenzorSpateDreapta
	TB3_SenzorSpateDreapta = float(data.data)
	# print "Spate Dreapta", TB3_SenzorSpateDreapta

def callbackSenzorSpate(data):
	global TB3_SenzorSpate
	TB3_SenzorSpate = float(data.data)

def OutOfRange(value):
	error = 0.3
	number1 = -0.01
	number2 = 0.2
	if (value<number1-error or value> number2+error) and iteration> 10:
		return 1