# 28/09/2023
# movementV5_2.py 

#Switchs#
# SW1 0:Maze    1:Wall-Follower
# SW2 0:TVR     1:Bluetooth
# SW3 0:Solver  1:Human w/remote
# SW4 0:TVR'P'  1:Button Start

###########
# Imports #
###########

import machine
from UKMARS_New import *
from tvr_lib import Remote
if SW1.value() == 0:
    import solver_m_lite_2
    solver = solver_m_lite_2.Solver(target_cell=165, conn=False)   # default small maze target_cell=165
    solver.solve()

#############
# Variables #
#############

led = machine.Pin("LED", machine.Pin.OUT)

#Setting up the motors
lmotor = Motor("L")
rmotor = Motor("R")

finalsolve = False #Used for final solve
pcommand = ''
LDistance=0  #Value from left wheel incoder
RDistance=0  #Value from right wheel incoder
command = 0  #Used to get inputs from remotes and solver code
mult = 0.0006 # amount of steering in corridor
speed = 25
fast = 40
slow = 25


fwd = 450
fast_fwd = 415
fwd_correction = 240

lturn_amount = 245
lturn_fwd = 210 #205

rturn_amount = 245
rturn_fwd = 210 #205

uturn_amount = 265
uturn_fwd = 140       

#############
# Functions #
#############

# Setup for left wheel sensor
def Lwheelirq(which):
    global LDistance            # count of wheel markers
    LDistance += 1              # increase count

# Setup for left wheel sensor
def Rwheelirq(which):
    global RDistance            # count of wheel markers
    RDistance += 1              # increase count

#Function for getting inputs from a remote
def remote():
    global remote
    while True:
        char = remote.get()
        if char:
            return char
        
def button_press():
    global blength
    count = 0
    while Button.value() == 0:
        pass
    
    while Button.value() == 1:
        count += 1
        time.sleep(0.1)
    
    if count > 8:
        blength = 'l'
    else:
        blength = 's'
        
def calibration():
    global lpresent, rpresent, fpresent, fstop
    
    #lpresent
    ledL.on()
    while Button.value() == 0:
        pass
    (left, front , right) = ReadWALLS()
    lpresent = left * 0.75
    ledL.off()
    
    time.sleep(1)
    
    #rpresnt
    ledR.on()
    while Button.value() == 0:
        pass
    (left, front , right) = ReadWALLS()
    rpresent = right
    ledR.off()
    
    time.sleep(1)
    
    #fpresent
    led2.on()
    while Button.value() == 0:
        pass
    (left, front , right) = ReadWALLS()
    fpresent = front
    led2.off()
    
    time.sleep(1)
    
    #fstop
    led.on()
    while Button.value() == 0:
        pass
    (left, front , right) = ReadWALLS()
    fstop = front
    
    time.sleep(1)
    
    #loging values
    f = open("values.py", "w")
    f.write("lpresent = "+str(lpresent)+"\n")
    f.write("fpresent = "+str(fpresent)+"\n")
    f.write("rpresent = "+str(rpresent)+"\n")
    f.write("fstop = "+str(fstop)+"\n")
    f.close()   
    
 
def forward(dist, correction): #dist = how far to go forward | one cell = 28
    
    global LDistance, RDistance     

    l_init = LDistance
    r_init = RDistance
    
    (left, front, right)= ReadWALLS()
    # time.sleep(0.01)
    
    if left > lpresent:
        OldL = 1
    else:
        OldL = 0
            
    if right > rpresent:
        OldR = 1
    else:
        OldR = 0
        
    lmotor.speed(30)
    rmotor.speed(30)
    
    while LDistance - l_init < dist or RDistance - r_init < dist:
        (left, front, right)= ReadWALLS()
        # time.sleep(0.01)
        
        #if front wall is close
        if front > fstop:
            break
        
        #Detecting if left wall or right wall is present
        if left > lpresent:
            L = 1
        else:
            L = 0
            
        if right > rpresent:
            R = 1
        else:
            R = 0
        
        
        if correction == 1 and (LDistance - l_init + fwd_correction) < dist or (RDistance - r_init + fwd_correction) < dist:  #only reduces value of dist
            if L < OldL or R < OldR:  #If wall has disappeared
                led.on()
                dist = LDistance - l_init + fwd_correction
                correction = 0

            else:
                led.off()
        
        #Steering
        if L and R:
            error = (left - ltarget + rtarget - right) / 2
            error *= mult
            
            lspeed = speed + error    # calculate motor speeds
            rspeed = speed - error
        
        elif L == 1:
            error = left - ltarget     # subtract target
            error = error * mult      # amount of steering
    
            lspeed = speed + error    # calculate motor speeds
            rspeed = speed - error
        
        elif R == 1:
            error = right - rtarget     # subtract target
            error = error * mult      # amount of steering
        
            lspeed = speed - error    # calculate motor speeds
            rspeed = speed + error
        
        else:
            lspeed = speed
            rspeed = speed
            
        rmotor.speed(rspeed)  # send speeds to motors
        lmotor.speed(lspeed)
            
        #if there is left wall steer to left target
        #if there is right wall steer to right target
        #no walls? go straught ahead
        
        OldL = L
        OldR = R
            
    led.off()
    
    lmotor.speed(0)
    rmotor.speed(0)
    
    
def left_turn():
    global LDistance, RDistance
    LDistance=0
    RDistance=0
    
    rmotor.speed(30)
    lmotor.speed(0)
    
    while RDistance < lturn_amount:
        pass
    
    lmotor.speed(0)
    rmotor.speed(0)
    time.sleep(0.05)          # new V4_1:
    
    forward(lturn_fwd, 0)
    
    lmotor.speed(0)
    rmotor.speed(0)
    
    (left,front,right)= ReadWALLS()  # New V4_1


def right_turn():
    global LDistance, RDistance
    LDistance=0
    RDistance=0
    
    lmotor.speed(30)
    rmotor.speed(0)
    
    while LDistance < rturn_amount:
        pass
    
    lmotor.speed(0)
    rmotor.speed(0)
    
    time.sleep(0.05)     # new V4_1:
    forward(rturn_fwd, 0)
    
    lmotor.speed(0)
    rmotor.speed(0)
    
    (left,front,right)= ReadWALLS()  # New V4_1


def u_turn(go_fwd):
    global LDistance, RDistance
    LDistance=0
    RDistance=0
    
    (left,front,right)= ReadWALLS()  # New V4_1
    (left,front,right)= ReadWALLS()  # New V4_1
    # time.sleep(0.01)                 # new V4_1:
    
    rmotor.speed(-20)
    lmotor.speed(20)
    
    while RDistance < uturn_amount:
        pass
    
    lmotor.speed(0)      # new V4_1: stop mptor befor pause
    rmotor.speed(0)
    time.sleep(0.05)
    
    if front >= fpresent:        # New V4_1
        go_back()
        if go_fwd == 1:
            forward(fwd, 0)
        
    else:
        forward(uturn_fwd,0)     # new V4_1:
    
    lmotor.speed(0)
    rmotor.speed(0)
    
    (left,front,right)= ReadWALLS()  # New V4_1


def go_back():
    global LDistance, RDistance
    LDistance=0
    RDistance=0
    rmotor.speed(-20)
    lmotor.speed(-20)
    while True:
        OLD = LDistance
        time.sleep(0.05)
        if ((LDistance-OLD) < 2):
            time.sleep(0.2)           # new V4_1: reduced
            break  
    lmotor.speed(0)
    rmotor.speed(0)


########
# Main #
########
    
# set up interrupt handler for each pin
lwheel.irq(handler=Lwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) 
rwheel.irq(handler=Rwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING)

#bluetooth or TV remote
if SW2.value() == 1:
    remote = Remote(0,9600)  # Bluetooth
else:
    remote = Remote(0,2400)  # TV Remote

button_press()
if blength == 'l':
    calibration()

else:
    try:
        from values import *
    
    except:
        lpresent = 1300    # new V4_1: was 2700
        fpresent = 2000     # new V4_1: was 2500
        rpresent = 1500    # new V4_1: was 2700
        fstop = 3000
            
led.off()
        

while Button.value() == 0:
    (left, front, right)= ReadWALLS()
    time.sleep(0.1)
    print("L:", left, "F:", front, "R:", right)
    
    if left > lpresent:
        ledL.on()
    else:
        ledL.off()
    
    if front > fpresent:
        led2.on()
    else:
        led2.off()
    
    if right > rpresent:
        ledR.on()
    else:
        ledR.off()

time.sleep(1)
(left,front,right)= ReadWALLS()

#used for steering
ltarget = left
rtarget = right     

f = open("log.txt", "w")
f.close()

while True:
    #wall follower
    if SW1.value() == 1:
               
        #Getting wall values
        (left,front,right)= ReadWALLS()
        # time.sleep(0.01)
        
        #Working out what walls are present
        #1 = present / 0 = not present
        if left > lpresent:
            L = 1
            ledL.on()
        else:
            L = 0
            ledL.off()
            
        if front > fpresent:
            F = 1
            led2.on()
        else:
            F = 0
            led2.off()
            
        if right > rpresent:
            R = 1
            ledR.on()
        else:
            R = 0
            ledR.off()
            
        irqvals = machine.disable_irq()
        f = open("log.txt", "a")
        f.write(str(L)+","+str(F)+","+str(R)+"\n")
        f.write(str(left)+","+str(front)+","+str(right)+"\n\n")
        f.close()
        machine.enable_irq(irqvals)
            
        time.sleep(0.01)
            
        if L == 0:
            left_turn()
        
        elif L == 1 and F == 0:
            forward(fwd, 0)
            
        elif L == 1 and R == 0 and F == 1:
            right_turn()
        
        elif L == 1 and R == 1 and F == 1:
            u_turn(0)
            
        else:
            time.sleep(2)
    
    #maze solver
    else:
        #Getting wall values
        (left,front,right) = ReadWALLS()
        # time.sleep(0.001)
        
        #Working out what walls are present
        #1 = present / 0 = not present
        if left > lpresent:
            L = 1
            ledL.on()
        else:
            L = 0
            ledL.off()
            
        if front > fpresent:
            F = 1
            led2.on()
        else:
            F = 0
            led2.off()
            
        if right > rpresent:
            R = 1
            ledR.on()
        else:
            R = 0
            ledR.off()
        
        #Logs what walls are present
        irqvals = machine.disable_irq()
        f = open("log.txt", "a")
        f.write(str(L)+","+str(F)+","+str(R)+"\n")
        f.write(str(left)+","+str(front)+","+str(right)+"\n")
        f.close()
        machine.enable_irq(irqvals)
        
        if finalsolve == False:     #If not on phase 3
            #Remote
            if SW3.value() == 1:
                #Waiting for instructions from the remote
                command = remote.get()
                while command == '?':
                    command = remote.get()
            
            #Solver
            else:
                #Logs any errors
                try:
                    command = solver.wn(L,F,R)
                
                except Exception as e:
                    irqvals=machine.disable_irq()
                    f = open("log.txt", "a")
                    f.write(str(e))
                    f.close()
                    machine.enable_irq(irqvals)
        
            print(command)
        
            #Logging command recived
            irqvals = machine.disable_irq()
            f = open("log.txt", "a")
            f.write(str(command)+"\n")
            f.close()
            machine.enable_irq(irqvals)
        
        #If on phase 3
        if type(command) == list:
            led.off()
            commandlist = command
            finalsolve = True
            
            for i in range(len(command)):
                pcommand = commandlist[i]   #Gets the nessceray command
                
                if pcommand[0] == '+' and len(pcommand) > 1:
                    numcells = int(pcommand[1:])  #number of times needed to move forward
                    new_numcells = numcells
                    
                    while new_numcells > 1:
                        speed = slow
                        forward(fast_fwd, 0)
                        new_numcells -= 1
                    
                    #slows down on the last cell
                    mult = 0.001        # amount of steering in corridor
                    speed = slow
                    forward(fast_fwd, 0)                        
                        
                else:           
                    if pcommand == '>':
                        right_turn()
                        
                    elif pcommand == '<':
                        left_turn()
                        
                    elif pcommand == 'O':
                        u_turn(1)
                        
                    elif pcommand == 's':
                        exit()          
                
        else:
            #Deciding how to move
            if command == '+':
                forward(fwd, 1)
                
            elif command == '>':
                right_turn()
                
            elif command == '<':
                left_turn()
                
            elif command == 'O':
                u_turn(1)
                
            elif command == '-':
                u_turn(0)
                
                #with remote
                if SW4.value() == 1:
                    command = remote.get()
                    while command != 'P':
                        command = remote.get()
                        led.toggle()
                        time.sleep(0.5)    

                #With button
                else:
                    while Button.value() == 0:
                        led.toggle()
                        time.sleep(0.5)
                        
                time.sleep(1)
                command = solver.wn(0, 0, 0)
                finalsolve = True
            
            elif command.isdigit():  #Used with remote. Press the number on remote for how many cells you want to move forward
                numcells = int(command)  #how many cells to move forward
                
                while numcells > 1:
                    speed = fast
                    forward(fwd,0)
                    numcells -= 1
                
                #slows down on last cell
                speed = slow
                forward(fwd,0)