#!/usr/bin/env python

#######################################################################
#                            Aufgabe 2	                              #
#######################################################################

from linuxWiimoteLib import *
frim iot_car import Iot_car

# initialize wiimote
wiimote = Wiimote()

#Insert address and name of device here
# hcitool scan 
#
device = ('', '')

connected = False

car = Iot_car(testmode=True)

# AUFGABE d
def set_led(remote):
    global car
    # percent
    p = car.speed_cruise_control / car.V_MAX
    if p > 0 and p <= 0.25:
        remote.SetLEDs(True, False, False, False)
    elif p > 0.25 and p <= 0.5:
        remote.SetLEDs(True, True, False, False)
    elif p > 0.5 and p <= 0.75:
        remote.SetLEDs(True, True, True, False)
    else:
        remote.SetLEDs(True, True, True, True)

try:
	print "Press any key on wiimote to connect"
	while (not connected):
		connected = wiimote.Connect(device)

	print "succesfully connected"

	wiimote.SetAccelerometerMode()

	wiistate = wiimote.WiimoteState
	while True:
		# re-calibrate accelerometer
		if (wiistate.ButtonState.Home):
			print 're-calibrating'
			wiimote.calibrateAccelerometer()
			sleep(0.1)			

        # Set acceleration and deceleration
        if wiistate.ButtonState.One:
            car.activate_cruise_control_forward()
        else:
            car.deactivate_cruise_control()
        if wiistate.ButtonState.Two:
            car.activate_cruise_control_backward()
        else:
            car.deactivate_cruise_control()
            
        # streeting
        if wiistate.ButtonState.Up:
            # streeting left        
            car.car.streeting_left()
        if wiistate.ButtonState.Down:
            # streeting right
            car.streeting_right()
        if not wiistate.ButtonState.Down and not wiistate.ButtonState.Up:
            car.reset_streeting()
            
        # reset button
        if wiistate.ButtonState.Minus:
            car.reset()
            car.cruise_control_reset()
            
        # set led states
        set_led(wiimote)
            
        delta = 1.0 / 50
        print("({},{} --> {})".format(car.speed_cur, car.angle_cur, (car.speed_cur - car.speed_last) / delta)) + ", Servo_active: " + str(car.is_testmode_servo_active) + "CC Speed: " str(car.speed_cruise_control)
        sleep(1 / 50)
        
except KeyboardInterrupt:
	print "Exiting through keyboard event (CTRL + C)"
	exit(wiimote)