import time, socket, buttons, virtualtimers as vt, easydraw as draw, network, ugfx

try:
	import orientation
	orientation.default()
except:
  pass

draw.msg("Connecting...", "Hoverboard", True)
nw = network.WLAN(network.STA_IF)
nw.active(True)
nw.connect("hoverboard","secret123")

timeout = 120
while not nw.isconnected():
	time.sleep(0.1)
	timeout = timeout - 1
	if (timeout < 1):
	  	draw.msg("???")
		nw.connect("hoverboard","secret123")
		#draw.msg("Failed.")
		#app.home()

teller = 0
def hbwrite(steer, speed):
	global teller
	teller = teller + 1
	if teller > 255:
		teller = 0
	data = bytes([0x00, 0x42])+(steer).to_bytes(2, "little")+(speed).to_bytes(2, "little")+bytes([teller])+bytes([0x42, 0x00])
	try:
		sock.sendto(data, ("192.168.4.1",42))
	except:
		pass

speed = 0
steer = 0
speedValue = 0
steerValue = 0
speedMp = 1
bw = 0
q = 0

def key_a(pressed):
	global speed, speedMp, bw, steer
	print("A",pressed)
	if pressed:
		speed = 40
		speedMp = 1
		bw = 0
	else:
		speed = 0
        bw = 0

def key_b(pressed):
	global speed, speedMp, bw, steer, speedValue
	print("B",pressed)
	if pressed:
		bw = 1
		speed = 0
		speedMp = 1
	else:
		speed = 0
		speedValue = 0
		bw = 0
		
def key_left(pressed):
	global speed, speedMp, bw, steer
	print("LEFT",pressed)
	if pressed:
		steer = -40
	else:
		steer = 0
		
def key_right(pressed):
	global speed, speedMp, bw, steer
	print("RIGHT",pressed)
	if pressed:
		steer = 40
	else:
		steer = 0
		
buttons.attach(buttons.BTN_A, key_a)
buttons.attach(buttons.BTN_B, key_b)
buttons.attach(buttons.BTN_LEFT, key_left)
buttons.attach(buttons.BTN_RIGHT, key_right)

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

vt.activate(100)

def display():
	global speedValue, steerValue
	ugfx.clear()
	ugfx.string(5, 13*0, str(speedValue),"Roboto_Regular12",ugfx.BLACK)
	ugfx.string(5, 13*2, str(steerValue),"Roboto_Regular12",ugfx.BLACK)
	ugfx.flush()
	return 500

vt.new(1,display)

while True:
	if (bw and (speedValue > 100)):
		speedValue = speedValue - 100
	steerValue = steerValue + steer
	speedValue = speedValue + speed #+ round(speed/speedMp)
	if (not q):
		speedValue = round(speedValue*0.97)
	steerValue = round(steerValue*0.9)
	if (steerValue < 50) and (steerValue > -50):
		if steerValue > 0:
			steerValue -= 1
		if steerValue < 0:
			steerValue += 1
	if (speedValue < 50) and (speedValue > -50):
		if speedValue > 0:
			speedValue -= 1
		if speedValue < 0:
			speedValue += 1
	if (steerValue < -400):
		steerValue = -400
	if (steerValue > 400):
		steerValue = 400
	if (speedValue <= 0):
		speedValue = bw * -300
	if (speedValue > 1000):
		speedValue = 1000
	hbwrite(steerValue, speedValue)
	print(steerValue, speedValue, steer, speed, bw, speedMp)
	time.sleep(0.025)