# Write your code here :-) ok
from microbit import *
import radio
import time
import math


leftLineSensor = pin11  # Enklere navn for linjesensoren på venstre side
rightLineSensor = pin5  # Enklere navn for linjesensoren på høyre side
requiredX = 400  # Hvor mye må man tilte venstre/høyre før display skal vise venstre/høyre? (Verdier 0-1000)
requiredY = 100  # Hvor mye må man tilte framover/bakover før display skal vise fram/bakover? (Verdier 0-1000)
minX = 0  # Hvor mye må man tilte venstre/høyre før vila begynner å svinge? (Verdier 0-1000)
maxX = 400  # Hvor mye må man tilte venstre/høyre for å svinge maksimalt? (Verdier 0-1000)
minY = 100  # Hvor mye må man tilte framover/bakover før bila begynner å gasse? (Verdier 0-1000)
maxY = 500  # Hvor mye må man tilte framover/bakover for å gasse maksimalt? (Verdier 0-1000)

# Hver verdi av y skal tilsvare en verdi av fart.
# Senere i programmet regnes det ut en speedMultiplier er mellom 0 og 1.
# Når y er mellom minY og maxY er farta et sted mellom 0 og 1.
# Her legges det en basis for en lineær funksjon med punkter (minY, 0) og (maxY, 1)
# Følgende kode beregner stigningstall og konstantledd for denne funksjonen.
slopeY = 1/(maxY-minY)  # Finner stigningstall
bY = -slopeY*minY # Finner konstantledd

# Når x er i området [-maxX, -minX] eller [minX, maxX]:
# Forholdet mellom farta til det tregeste hjulet og det kjappeste hjulet er mellom 0 og 1.
# Her legges det en basis for en lineær funksjon med punkter (minX, 0.5) og (maxX, 1)
# Følgende kode beregner stigningstall og konstantledd for denne funksjonen.
slopeX = 0.5/(maxX-minX)
bX = -slopeX*minX

mode = "none"  # mode kan være none, control eller linefollower. Starter med none.
timeout = 0  # Bilen skal stoppe etter for lang tid uten mottatt signal.
imageUndefined = Image("90009:" # Lager et bilde av et kryss
                "09090:"
                "00900:"
                "09090:"
                "90009")
imageLinefollower = Image("90000:" # Lager et bilde av en L
                "90000:"
                "90000:"
                "90000:"
                "99999")


def funcLSpeed(var):  # Lager enklere funksjonsnavn for fart til venste hjul for å lettere kunne lese koden.
    pin0.write_analog(var)
def funcLDir(var):  # Samme for retning til venstre hjul
    pin8.write_digital(var)
def funcRSpeed(var):  # Samme for fart til høyre hjul
    pin1.write_analog(var)
def funcRDir(var):  # Samme for retning til høyre hjul
    pin12.write_digital(var)
def drive(lDir, lSpeed, rDir, rSpeed):  # Definerer en enkel funksjon for å få bilen til å kjøre en viss retning
    funcLDir(lDir)  # Setter venstre hjul retning til lDir
    funcRDir(rDir)  # Setter høyre hjul retning til rDir
    if lDir == 0:  # Hvis venstre hjul kjører framover:
        funcLSpeed(math.floor(lSpeed*1023))  # funcLSpeed tar en verdi mellom 0 og 1023
    else:  # Hvis venstre hjul kjører bakover:
        funcLSpeed(math.floor((1-lSpeed)*1023))  # verdi mellom 0 og 1023, men her er 0 mest og 1023 minst
    if rDir == 0:  # Hvis høyre hjul kjører framover:
        funcRSpeed(math.floor(rSpeed*1023))  # funcRSpeed tar en verdi mellom 0 og 1023
    else:  # Hvis høyre hjul kjører bakover:
        funcRSpeed(math.floor((1-rSpeed)*1023))  # verdi mellom 0 og 1023, men her er 0 mest og 1023 minst


radio.on()  # Skrur på radio
radio.config(channel=23)  # Setter radiokanal
while True:  # Hovedløkke
    time.sleep(0.01)  # Klokkefrekvens
    inData = radio.receive_bytes()  # Sjekker siste radiomelding i bufferen
    if inData is not None:  # Hvis en melding er mottatt:
        timeout = 0  # Tid siden siste melding resettes
        inData = str(inData, 'utf-8').split()  # Radiomeldingen inneholder en string med mellomrom. Splitter opp i en array.
        mode = inData[0]  # Første ord i meldinga spesifiserer modus (none, control, linefollower)
        if mode == "none":  # Hvis fjernkontrolleren ikke har valgt modus enda:
            display.show(Image.CONFUSED)  # Displayer spørsmålstegn
        elif mode == "linefollower":  # Hvis modus er linefollower
            display.show(imageLinefollower)  # Henter bilde av L for linefollower
            leftSensor = leftLineSensor.read_digital()  # Får en verdi 0 eller 1 om venstre sensor oppfatter en konstrast
            rightSensor = rightLineSensor.read_digital() # Får en verdi 0 eller 1 om høyre sensor oppfatter en kontrast
            if leftSensor is not None and rightSensor is not None:  # Hvis det ikke er noe feil med komponentene:
                if leftSensor == 1:  # Hvis ser linje til venstre:
                    if rightSensor == 1:  # Hvis ser linje til venstre og høyre:
                        # Midt på linje
                        display.show(Image.ARROW_N)  # Pil framover
                        drive(0, 0.4, 0, 0.4)  # Kjør framover
                    else:  # Hvis kun ser linje til venstre:
                        # Høyre for linje
                        display.show(Image.ARROW_W)  # Pil venstre
                        drive(1, 0.2, 0, 0.2)  # Kjør til venstre
                else:  # Hvis ikke ser linje til venstre:
                    if rightSensor == 1:  # Hvis kun ser linje til høyre:
                        # Venstre for linje
                        display.show(Image.ARROW_E)  # Pil høyre
                        drive(0, 0.2, 1, 0.2)  # Kjør til høyre
                    else:  # Hvis hverken ser linje til venstre eller høyre:
                        # Linje ikke funnet
                        display.show(Image.SAD)  # Surfjes
                        drive(1, 0.3, 1, 0.3)  # Kjør bakover
            else:  # Ikke mottatt data fra sensorene
                # Noe er feil med bila, eller microbiten er ikke koblet til bila
                display.show(Image.ANGRY)  # Surfjes
        elif mode == "control":  # Ganske selvsagt
            x = int(inData[1])  # Andre ord i radiomeldinga er x-rotasjon til fjernkontrolleren
            y = int(inData[2])  # Tredje ord i radiomeldinga er y-rotasjon til fjernkontrolleren
            speedMultiplier = slopeY*abs(y)+bY  # Beregner basefart fra rotasjon (mellom 0 og 1)
            if speedMultiplier > 1:  # Hvis y > maxY:
                speedMultiplier = 1  # Sett fart til 1
            elif speedMultiplier < 0: # Hvis y < minY:
                speedMultiplier = 0  # Sett fart til 0
            leftRatio = slopeX*x+bX  # Bruker funksjonsuttrykk til å finne hvor stor ratio venstre hjul skal ha
            rightRatio = -slopeX*x+bX  # Bruker funksjonsuttrykk til å finne hvor stor ratio høyre hjul skal ha
            if leftRatio > 1:   # Hvis x > maxX:
                leftRatio = 1  # Sett venstre hjuls ratio til 1
            elif leftRatio < 0.5:  # Hvis x < minX:
                leftRatio = 0.5  # Sett venstre hjuls ratio til 0.5
            if rightRatio > 1:  # Hvis x < -maxX:
                rightRatio = 1  # Sett høyre hjuls ratio til 1
            elif rightRatio < 0.5:  # Hvis x > -minX:
                rightRatio = 0.5  # Sett høyre hjuls ratio til 0.5
            if leftRatio > rightRatio:  # Hvis det skal svinges til høyre:
                rightWheelMultiplier = 2-(leftRatio*2)  # Setter en multiplier for hastighet til høyre hjul
                leftWheelMultiplier = 1  # Venstre hjuls hastighet = speedMultiplier
            elif rightRatio > leftRatio:  # Hvis det skal svinges til venstre:
                leftWheelMultiplier = 2-(rightRatio*2)  # Setter en multiplier for hastighet til venstre hjul
                rightWheelMultiplier = 1  # Høyre hjuls hastighet = speedMultiplier
            else:  # Hvis x er i [-minX, minX]:
                # Begge hjulene har samme fart
                leftWheelMultiplier = 1
                rightWheelMultiplier = 1
            leftSpeed = leftWheelMultiplier*speedMultiplier  # Kombinerer venstre hjuls fart med generell fart (blir mellom 0 og 1)
            rightSpeed = rightWheelMultiplier*speedMultiplier  # Kombinerer høyre hjuls fart med generell fart (blir mellom 0 og 1)
            if y <= 0:  # Hvis fjernkontrolleren vendes framover:
                direction = 0  # Retning framover
            else:  # Hvis fjernkontrolleren vendes bakover:
                direction = 1  # Retning bakover
            drive(direction, leftSpeed, direction, rightSpeed)  # Kjør bila med verdiene som er funnet.
            # Følgende kode bestemmer når ulike piler vises på displayet.
            if y > requiredY:  # Hvis styrer bakover
                if x > requiredX:  # Hvis styrer bakover mot høyre:
                    display.show(Image.ARROW_SE)  # Pil sørøst
                elif x < -requiredX:  # Hvis styrer bakover mot venstre:
                    display.show(Image.ARROW_SW)  # Pil sørvest
                else:  # Hvis styrer bakover med lite venstre/høyre:
                    display.show(Image.ARROW_S)  # Pil sør
            elif y < -requiredY:  # Hvis styrer framover:
                if x > requiredX:  # Hvis styrer framover mot høyre:
                    display.show(Image.ARROW_NE)  # Pil nordøst
                elif x < -requiredX:  # Hvis styrer framover mot venstre:
                    display.show(Image.ARROW_NW)  # Pil nordvest
                else:  # Hvis styrer framover med lite venstre/høyre:
                    display.show(Image.ARROW_N)  # Pil nord
            else:  # Hvis lite framover/bakover:
                if x > requiredX:  # Hvis styrer nesten kun mot høyre:
                    display.show(Image.ARROW_E)  # Pil øst
                elif x < -requiredX:  # Hvis styrer nesten kun mot venstre:
                    display.show(Image.ARROW_W)  # Pil vest
                else:  # Hvis lite framover/bakover og lite venstre/høyre:
                    display.show(Image.HEART_SMALL)  # Hjerte som metafor for korleis kjærleiken ikkje går nokon veg
    else:  # Hvis radiomelding ikke ble mottatt:
        timeout += 1  # Øk tid siden forrige melding ble mottatt
        if timeout >= 10:  # Hvis det har gått for lang tid uten noen radiomelding mottatt:
            display.show(imageUndefined)  # Display kryss
            drive(0, 0, 0, 0)  # Stopp bila