#!/usr/bin/env python

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

from linuxWiimoteLib import *
from iot_car import Iot_car

# initialize wiimote
wiimote = Wiimote()

#Insert address and name of device here
# hcitool scan 
#
# B8:AE:6E:30:0F:0F    Nintendo RVL-CNT-01-TR
#
device = ('B8:AE:6E:30:0F:0F', 'Nintendo RVL-CNT-01-TR')

connected = False

car = Iot_car(testmode=False)

# 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)
    elif p == 1:
        remote.SetLEDs(True, True, True, True)
    else:
    	remote.SetLEDs(False, False, False, False)

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

    print "succesfully connected"

    wiimote.SetAccelerometerMode()

    wiistate = wiimote.WiimoteState
    step = True
    counter = 0
    print 'entering loop'
    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()
        elif wiistate.ButtonState.Two:
            car.activate_cruise_control_backward()
        else:
            car.deactivate_cruise_control()
            
        # streeting
        if wiistate.ButtonState.Up:
            # streeting left        
            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()
            
        if wiistate.ButtonState.Right and step:
        	car.cruise_control_increase()
        	step = False
        if wiistate.ButtonState.Left and step:
        	car.cruise_control_decrease()
        	step = False
            
        # reset button
        if wiistate.ButtonState.Minus:
            car.stop()
            car.cruise_control_reset()
            
        # set led states
        set_led(wiimote)
        
        counter = counter + 1
        if counter == 50:
        	step = True
        	counter = 0
        	
        delta = 1.0 / 50
        print wiimote.getAccelState()
        #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(0.01)
        
except KeyboardInterrupt:
    print "Exiting through keyboard event (CTRL + C)"
    exit(wiimote)