from microbit import *
import radio
import music

radio.config(channel=1)
radio.on()

#Hvilken modus bit:boten er i. 0 = fjernkontroll, 1 = linjefølger
#Modusen byttes ved å dobbeltrykke begge knappene samtidig på fjernkontrollen
MODE = 0

#Definerer noen hastighetskonstanter
MAX_SPEED, MIN_SPEED = 1023, 100
LEFT_SPEED, RIGHT_SPEED = 0, 0

#Konstant for den maksimale verdien g kan ha i absoluttverdi i alle retninger
g_MAX = 1023
#Konstanter for de maksimale og minimale verdiene x og y kan ha
#Blir regnet ut i funksjonen maksVerdier() ved kjøring av scriptet
X_MAX, Y_MAX = 0, 0
X_MIN, Y_MIN = 40, 40 #Absoluttverdier

#Angir hvilken retning bilen kjører. 0 = framover, 1 = bakover
DIRECTION = -1
#Angir hvilken side bilen svinger mot. 0 = venstre, 1 = høyre
SIDE = -1

#Angir sensitiviteten for svinging.
#Kan være en verdi mellom 1 og 100.
X_SENS = 70
# Angir sensitiviteten for kjøring framover.
# Kan være en verdi mellom 1 og 100.
Y_SENS = 50

#Regner ut den maksimale verdien x og y kan ha basert på sensitiviteten
def maksVerdier():
    global X_MAX
    global Y_MAX
    X_MAX, Y_MAX = (X_SENS*g_MAX)/100, (Y_SENS*g_MAX)/100
    print(X_MAX, Y_MAX)

maksVerdier()

#Regner ut prosent
def prosent(delen, maksimale):
    return round((delen*100)/maksimale)

#Regner ut hastighet fra prosent
def hastighetFraProsent(prosent):
    return round((MAX_SPEED*prosent)/100)

#Regner ut farten til hjulet som skal svinge
def svingFart(xProsent, framFart):
    return round((xProsent*framFart)/100)

def calcSpeed(x, y):
    #Regner ut farten framover basert på akselerometerverdien til y:
    yProsent = prosent(abs(y), Y_MAX)
    framFart = hastighetFraProsent(yProsent)

    #Regner ut svingefarten basert på akselerometerverdien til x:
    xProsent = 100-prosent(abs(x), X_MAX)
    sFart = svingFart(xProsent, framFart)

    setSpeed(framFart, sFart)

#Setter hastighetene på hvert hjul ut fra hvilken retning bilen går
def setSpeed(framFart, sFart):
    global LEFT_SPEED
    global RIGHT_SPEED
    if DIRECTION == -1 and SIDE == -1:
        LEFT_SPEED = MIN_SPEED
        RIGHT_SPEED = MIN_SPEED
    elif DIRECTION == 0:
        #Kjører framover
        if SIDE == 0:
            #Svinger til venstre
            LEFT_SPEED = sFart
            RIGHT_SPEED = framFart
        elif SIDE == 1:
            #Svinger til høyre
            LEFT_SPEED = framFart
            RIGHT_SPEED = sFart
        else:
            #Svinger ikke
            LEFT_SPEED = framFart
            RIGHT_SPEED = framFart
    elif DIRECTION == 1:
        #Subtraherer hastigheten fra maks hastighet
        #(Maksimum hastighet blir minimum hastighet når
        #motorene går motsatt vei rundt)
        sFart = MAX_SPEED-sFart
        framFart = MAX_SPEED-framFart
        #Kjører bakover
        if SIDE == 0:
            #Svinger til venstre
            LEFT_SPEED = sFart
            RIGHT_SPEED = framFart
        elif SIDE == 1:
            #Svinger til høyre
            LEFT_SPEED = framFart
            RIGHT_SPEED = sFart
        else:
            #Svinger ikke
            LEFT_SPEED = framFart
            RIGHT_SPEED = framFart

    #Passer på at farten til hjulene holder seg innenfor grensene
    checkBounds()

#Denne funksjonen skjekker verdiene til LEFT_SPEED og RIGHT_SPEED
#og avgjør om de er innenfor grensene som er satt, for så å
#endre farten om de overstiger eller ligger under grensene.
def checkBounds():
    global LEFT_SPEED
    if LEFT_SPEED > MAX_SPEED:
        LEFT_SPEED = MAX_SPEED
    elif LEFT_SPEED < MIN_SPEED:
        LEFT_SPEED = MIN_SPEED
    global RIGHT_SPEED
    if RIGHT_SPEED > MAX_SPEED:
        RIGHT_SPEED = MAX_SPEED
    elif RIGHT_SPEED < MIN_SPEED:
        RIGHT_SPEED = MIN_SPEED

while True:
    a = button_a.is_pressed()
    b = button_b.is_pressed()
    #Hvis både knapp A og B trykkes samtidig sender vi signaler med fart til bit:boten
    if a and b:
        #Får akselerometerverdiene i de ulike retningene
        x = accelerometer.get_x()
        y = accelerometer.get_y()

        #Finner ut av hvilken retning bit:boten går i
        DIRECTION = -1
        if y < -Y_MIN:
            DIRECTION = 0
        elif y > Y_MIN:
            DIRECTION = 1
        #Finner hvilken side bit:boten svinger til
        SIDE = -1
        if x < -X_MIN:
            SIDE = 0
        elif x > X_MIN:
            SIDE = 1

        calcSpeed(x, y)

        data = str(DIRECTION) + " " + str(LEFT_SPEED) + " " + str(RIGHT_SPEED)
        #Sender signalet med retning og fart til bit:boten
        radio.send(data)
        display.show(Image.TARGET)
        sleep(90)
    else:
        ac = button_a.get_presses()
        bc = button_b.get_presses()
        if ac == 2 and bc == 2:
            music.play(music.BA_DING)
            if MODE == 0:
                MODE = 1
            else:
                MODE = 0
            radio.send(str(MODE))
        display.show(MODE)
        sleep(750)