#SW1: up = wall_follow
#SW2: up = Bluetooth Frequency | down = TV Frequency
#SW3: up = human_contol()  down = maze()
#SW4: up = start with remove/Bluetooth | down = start with hand

################################################################################################
from ssd1306 import SSD1306_I2C
from machine import I2C, Pin
import machine
import time
from UKMARS_New import *
import movementV6 as m
from tvr_lib import Remote

COMMAND_FOLLOWER = False

#Initialises the solver
if SW1.value() == 0:
    import faster_maze_solver_1
    solver = faster_maze_solver_1.Solver(target_cell=165, conn=False)   # default small maze target_cell=165
    solver.solve()
    
#Sets up screen
i2c = I2C(1, sda=Pin(18), scl=Pin(19), freq=200000)
dsp = SSD1306_I2C(128, 32, i2c)

#Sets up onboard LED
led = machine.Pin("LED", machine.Pin.OUT)

prev_command = ''
command = ''  #Used to get inputs from remotes and solver code
finalsolve = False #Used for final solve

################################################################################################
        
def button_press():
    if SW4.value() == 0:
        count = 0
        while Button.value() == 0:
            pass
        
        while Button.value() == 1:
            count += 1
            time.sleep(0.1)
        
        if count > 8:
            return 'l'  # long press
        else:
            return 's'  # short press
        
    else:
        print("Bluetooth")
        command = remote.get()
        while command == '?':
            # print(command)
            command = remote.get()
        
        print('pass', str(command))
        if command != 'P':
            return('b')
        
        else:
            return('s')
     
     
def bluetooth_calibration():
    print("BC")
    remote.send("Select variable or continue\n")
    print("Sent")
    command = remote.get()
    while command == '?':
        time.sleep(0.1)
        command = remote.get()
        
    print(command)
        
    if command != 'P':
        # Getting variable to change
        if command == 'l':
            remote.send(str(m.lpresent) + "\n")
            selected_variable = command
            
        elif command == 'f':
            remote.send(str(m.fpresent) + "\n")
            selected_variable = command
            
        elif command == 'r':
            remote.send(str(m.rpresent) + "\n")
            selected_variable = command
            
        elif command == 's':
            remote.send(str(m.fstop) + "\n")
            selected_variable = command
            
        elif command == 'S':
            remote.send(str(m.slow) + "\n")
            selected_variable = command
            
        elif command == 'F':
            remote.send(str(m.fast) + "\n")
            selected_variable = command
            
        #Recieving increase/decrease step
        remote.send("Select step\n")
        command = remote.get()
        while command == '?':
            time.sleep(0.1)
            command = remote.get()
            
        print(command)
            
        if command == '1':   step = 1
        elif command == '2': step = 5
        elif command == '3': step = 10
        elif command == '4': step = 50
        elif command == '5': step = 200
        elif command == '6': step = 1000
        
        #Sends current value of selected variable
        if selected_variable == 'l': remote.send(str(m.lpresent) + "\n")
        elif selected_variable == 'f': remote.send(str(m.fpresent) + "\n")
        elif selected_variable == 'r': remote.send(str(m.rpresent) + "\n")
        elif selected_variable == 's': remote.send(str(m.fstop) + "\n")
        elif selected_variable == 'S': remote.send(str(m.slow) + "\n")
        elif selected_variable == 'F': remote.send(str(m.fast) + "\n")
        
        command = remote.get()
        while command == '?':
            time.sleep(0.1)
            command = remote.get()
            
        print(command)
            
        while True:    
            while command == '+' or command == '-':
                if command == '+':
                    m.calibration(selected_variable, command, step)
                    
                elif command == '-':
                    m.calibration(selected_variable, command, step)
                    
                else:
                    break
                
                if selected_variable == 'l': remote.send(str(m.lpresent) + "\n")
                elif selected_variable == 'f': remote.send(str(m.fpresent) + "\n")
                elif selected_variable == 'r': remote.send(str(m.rpresent) + "\n")
                elif selected_variable == 's': remote.send(str(m.fstop) + "\n")
                elif selected_variable == 'S': remote.send(str(m.slow) + "\n")
                elif selected_variable == 'F': remote.send(str(m.fast) + "\n")
                
                command = '?'
                command = remote.get()
                while command == '?':
                    command = remote.get()
                    
                print(command)
            
            if command == 'P':
                break
            
            elif command == 'V':
                m.calibration('', command, 0)
                
            elif command == 'l':
                remote.send(str(m.lpresent) + "\n")
                selected_variable = command
                
            elif command == 'f':
                remote.send(str(m.fpresent) + "\n")
                selected_variable = command
                
            elif command == 'r':
                remote.send(str(m.rpresent) + "\n")
                selected_variable = command
                
            elif command == 's':
                remote.send(str(m.fstop) + "\n")
                selected_variable = command
                
            elif command == 'S':
                remote.send(str(m.slow) + "\n")
                selected_variable = command
                
            elif command == 'F':
                remote.send(str(m.fast) + "\n")
                selected_variable = command
                
            elif command == '1':
                step = 1
                
            elif command == '2':
                step = 5
                
            elif command == '3':
                step = 10
                
            elif command == '4':
                step = 50
                
            elif command == '5':
                step = 200
                
            elif command == '6':
                step = 1000
                
            command = '?'
            command = remote.get()
            while command == '?':
                command = remote.get()
            
    else:
        pass

def log():
    m.intenb = False
    f = open("log.txt", "a")
    f.write(str(L)+","+str(F)+","+str(R)+"\n")
    f.write(str(command)+"\n")
    f.close()
    m.intenb = True
    remote.send(str(L)+","+str(F)+","+str(R)+","+str(command)+"\n")
    
def display():
    global L, F, R
    #clears the log
    m.intenb = False
    f = open("log.txt", "w")
    f.close()
    m.intenb = True
    
    #Waiting for user to start robot
    count1 = 0
    if SW4.value() == 0:
        while count1 < 10:
            #prints wall values
            (left, front, right)= ReadWALLS()
            time.sleep(0.1)
            print("L:", left, "F:", front, "R:", right)
            #print("LWheel: "+str(m.LDistance)+"RWheel :"+str(m.RDistance))
            count1 += 1
            led.toggle()
            if front < 3000:
                count1 = 0
                led.off()
    
    else:
        command = remote.get()
        while command != 'P':
            (left, front, right)= ReadWALLS()
            print("L:", left, "F:", front, "R:", right)
            command = remote.get()
            time.sleep(0.1)            
        

def wall_follow():
    global L, F, R, command
    
    while True:
        L, F, R = m.walls()

        if L == 0:
            #Left Turn
            m.left_turn(stop_at_end = False)#, True)
            command = '<'
                
        elif L == 1 and F == 0:
            m.forward(m.fwd-25, 0, stop_at_end = False)
            command = '+'
            
        elif L == 1 and R == 0 and F == 1:
            #Rgiht Turn
            m.right_turn(stop_at_end = False)#, True)
            command = '>'
        
        elif L == 1 and R == 1 and F == 1:
            #U_Turn
            m.u_turn(2) 
            command = 'O'
            
        log()
 
 
def human_control():
    global L, F, R, command, prev_command
            
    while True:
        command = remote.get()
        while command == '?':
            command = remote.get()
        
        L, F, R = m.walls()
        #log()
        if command == '<' and L == 1 and prev_command != command:
            prev_command = command
            remote.send("Override")
        elif command == '>' and R == 1 and prev_command != command:
            prev_command = command
            remote.send("Override")
        elif command == '+' and F == 1 and prev_command != command:
            prev_command = command
            remote.send("Override")
        elif prev_command == command:
            m.move(command)
        else:
            m.move(command) #move only responds to: + | > | < | O | -
        log()
        
def maze():
    global L, F, R, command, finalsolve
    while True: #for phase 1 and 2
        (L,F,R) = m.walls()
        command = solver.wn(L,F,R)
    
        if type(command) == list:
            finalsolve = True
            log()
            break
        
        else:
            log()
            m.move(command)
            time.sleep(0.1)
            
    led.on()
    while Button.value() == 0:
        pass
    led.off()
    
    time.sleep(2)
    
    if finalsolve:
        commandlist = command
        for i in range(len(commandlist)):
            command = commandlist[i]
            m.move(command, finalsolve)

def command_follower():
    """follows a list of commands stored in the commands.txt file"""
    
    cmd_file = open("commands.txt", "r")
    try:
        cmds = eval(cmd_file.read().split("\n")[0].strip())
    except SyntaxError:
        print("Invalid command list")
        cmds = []
    finally:
        cmd_file.close()
    
    for cmd in cmds:
        m.move(cmd, True)
    
###############################################################################################

#Enters calibration mode or imports wall present values from values.py
try:
    #Sets frequency up frequency for bluetooth or TV Remote
    if SW2.value() == 1:
        remote = Remote(0, 9600)  # Bluetooth
    else:
        remote = Remote(0, 2400)  # TV Remote
        
    led.on()  # hi future toby :)! scroll down a bit...
    blength = button_press()
    if blength == 'l':
        led.off()
        m.calibration()
        
    elif blength == 'b':
        led.off()
        bluetooth_calibration()
    led.off()

    m.intenb = False
    f = open("log.txt", "w")
    remote.send("Hello World\n")
    f.close()
    m.intenb = True
        
    display()

    time.sleep(2)
    left, front, right = ReadWALLS()
    m.ltarget = left
    m.rtarget = right
    
    if COMMAND_FOLLOWER:  # just follow a list of commands
        command_follower()
    
    elif SW1.value() == 1:
        wall_follow()
        
    elif SW3.value() == 1:  # remote
        human_control()
        
    elif SW3.value() == 0: # solver
        maze()
   
except Exception as e:
    m.intenb = False
    f = open("log.txt", "a")
    f.write(str(e))
    remote.send(str(e))
    print(e)
    f.close()
    m.intenb = True
    
    raise e