# 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)
slopeY = 1/(maxY-minY)  # TODO: fullfør kommentering
bY = -slopeY*minY
slopeX = 0.5/(maxX-minX)
bX = -slopeX*minX
mode = "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 å lettere kunne lese koden.
    pin0.write_analog(var)  # Parameteren heter "var" fordi "dir" og "speed" brukes senere
def funcLDir(var):
    pin8.write_digital(var)
def funcRSpeed(var):
    pin1.write_analog(var)
def funcRDir(var):
    pin12.write_digital(var)
def drive(lDir, lSpeed, rDir, rSpeed):
    funcLDir(lDir)
    funcRDir(rDir)
    if lDir == 0: # Venstre kjører framover
        funcLSpeed(math.floor(lSpeed*1023))
    else:
        funcLSpeed(math.floor((1-lSpeed)*1023))
    if rDir == 0:
        funcRSpeed(math.floor(rSpeed*1023))
    else:
        funcRSpeed(math.floor((1-rSpeed)*1023))


radio.on() #Skrur på radio
radio.config(channel=23) #Setter radiokanal
while True:
    time.sleep(0.01)
    inData = radio.receive_bytes()
    if inData is not None:
        timeout = 0
        inData = str(inData, 'utf-8').split()
        mode = inData[0]
        if mode == "none":
            display.show(Image.CONFUSED)
        elif mode == "linefollower":
            display.show(imageLinefollower)
            leftSensor = leftLineSensor.read_digital()
            rightSensor = rightLineSensor.read_digital() # 1 eller 0
            if leftSensor is not None and rightSensor is not None:
                if leftSensor == 1:
                    if rightSensor == 1:
                        #Midt på linje
                        display.show(Image.ARROW_N)
                        drive(0, 0.4, 0, 0.4)
                    else:
                        #Høyre for linje
                        display.show(Image.ARROW_W)
                        drive(1, 0.2, 0, 0.2)
                else:
                    if rightSensor == 1:
                        #Venstre for linje
                        display.show(Image.ARROW_E)
                        drive(0, 0.2, 1, 0.2)
                    else:
                        #Linje ikke funnet
                        display.show(Image.SAD)
                        drive(1, 0.3, 1, 0.3)
            else:
                #Ugyldig inndata
                display.show(Image.ANGRY)
        elif mode == "control":
            x = int(inData[1])
            y = int(inData[2])
            speedMultiplier = slopeY*abs(y)+bY
            if speedMultiplier > 1:
                speedMultiplier = 1
            elif speedMultiplier < 0:
                speedMultiplier = 0
            leftRatio = slopeX*x+bX
            rightRatio = -slopeX*x+bX
            if leftRatio > 1:
                leftRatio = 1
            elif leftRatio < 0.5:
                leftRatio = 0.5
            if rightRatio > 1:
                rightRatio = 1
            elif rightRatio < 0.5:
                rightRatio = 0.5
            if leftRatio > rightRatio:
                rightWheelMultiplier = 2-(leftRatio*2)
                leftWheelMultiplier = 1
            elif rightRatio > leftRatio:
                leftWheelMultiplier = 2-(rightRatio*2)
                rightWheelMultiplier = 1
            else:
                leftWheelMultiplier = 1
                rightWheelMultiplier = 1
            leftSpeed = leftWheelMultiplier*speedMultiplier
            rightSpeed = rightWheelMultiplier*speedMultiplier
            if y <= 0:
                direction = 0
            else:
                direction = 1
            drive(direction, leftSpeed, direction, rightSpeed)
            #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)
                elif x < -requiredX: #Hvis styrer bakover mot venstre:
                    display.show(Image.ARROW_SW)
                else: #Hvis styrer bakover med lite venstre/høyre:
                    display.show(Image.ARROW_S)
            elif y < -requiredY: #Hvis styrer framover:
                if x > requiredX: #Hvis styrer framover mot høyre:
                    display.show(Image.ARROW_NE)
                elif x < -requiredX: #Hvis styrer framover mot venstre:
                    display.show(Image.ARROW_NW)
                else: #Hvis styrer framover med lite venstre/høyre:
                    display.show(Image.ARROW_N)
            else: #Hvis lite framover/bakover:
                if x > requiredX: #Hvis styrer nesten kun mot høyre:
                    display.show(Image.ARROW_E)
                elif x < -requiredX: #Hvis styrer nesten kun mot venstre:
                    display.show(Image.ARROW_W)
                else: #Hvis lite framover/bakover og lite venstre/høyre:
                    display.show(Image.HEART_SMALL)
    else:
        timeout += 1
        if timeout >= 10:
            display.show(imageUndefined)
            drive(0, 0, 0, 0)