# 02/06/2023
# movementV4_1.py    Mr Fisher

#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

#########
# Setup #
#########

#imports
import machine
import gc
from UKMARS_New import *
from tvr_lib import Remote
if SW3.value() == 0:
    import maze_solver
    # need to set target_cell=120,  cells_to_visit=256 for 16x16 maze  
    # try target_cell=160  for simple tests
    solver = maze_solver.Solver(conn=False)   # default small maze target_cell=165,  cells_to_visit=36,  
    # these next 2 lines aren't needed I think
    solver.setp("240UUUUUNRNRNRNRNRNEDEDEDEDEDE")
    solver.setp("240")
    solver.solve()

#gc.enable()   #Garbage collection not needed # new V4_1:
#Setup for maze solving#

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

#Used in for final solve
finalsolve = False
pcommand = ''
#Value from whell sensors
LDistance=0
RDistance=0
command = 0 #Used to get inputs from remotes and solver code
speed = 20
fast = 30             #new V4_1 for long straights
slow = 20             #new V4_1

#Used to determin what wall all present
lpresent = 1100       # new V4_1: was 2700
lnotpresent = 0
fpresent = 1800       # new V4_1: was 2500
rpresent = 1100       # new V4_1: was 2700
fstop = 3300

fwd = 380

lturn_amount = 245
lturn_fwd = 220

rturn_amount = 245
rturn_fwd = 200

uturn_amount = 250    # new V4_1: was 170 but see line 293
uturn_fwd = 130       # # new V4_1:

#Steup for wall follwing#

#Setup for motors
RightMotor = rmotor
LeftMotor = lmotor
mult = 0.0005        # amount of steering in corridor
delay = 0.18		#time delay for left_delay
frontvalue = 14000	#front sensor value for breaking corridor
leftvalue = 3400	#left sensor value for breaking corridor


############################
# Wall Following Functions #
############################

def corridor():   # go down the corridor
    global left, front, right

    while True:                   # this version doesn't stop
        (left,front,right)= ReadWALLS()  # read all 3 walls
        target = 6000             # required left wall distance
                
        # statements to end the loop (not used till step4)
        if (left < leftvalue):
            RightMotor.stop()
            LeftMotor.stop()
            break                 # exit loop if no left wall
        if (front > frontvalue):
            RightMotor.stop()
            LeftMotor.stop()
            break                 # exit loop if front wall
        
        error = left - target     # subtract target
        error = error * mult      # amount of steering
    
        lspeed = speed + error    # calculate motor speeds
        rspeed = speed - error
        RightMotor.speed(rspeed)  # send speeds to motors
        LeftMotor.speed(lspeed)
        
        time.sleep(0.01)# do a hundred readings per second
    
    print("exit", front, left)

def left_delay():
    global left
    #delay before
    lspeed = 32
    rspeed = 30
    RightMotor.speed(rspeed)
    LeftMotor.speed(lspeed)
    time.sleep(delay)  # adjsut to suit
    #turn
    lspeed = 15      # adjust to suit
    rspeed = 40     # adjust to suit
    RightMotor.speed(rspeed)
    LeftMotor.speed(lspeed)
    (left,front,right)= ReadWALLS()  # read all 3 walls
    while left < leftvalue:
        (left,front,right)= ReadWALLS()  # read all 3 walls
        time.sleep(0.01)
    RightMotor.stop()
    LeftMotor.stop()
    
def right_delay():   
    global front
    lspeed = 30      # adjust to suit
    rspeed = -30     # adjust to suit
    RightMotor.speed(rspeed)
    LeftMotor.speed(lspeed)
    (left,front,right)= ReadWALLS()  # read all 3 walls
    while front > 7000:
        (left,front,right)= ReadWALLS()  # read all 3 walls
        time.sleep(0.01) # adjsut to suit
        led.on()
    led.off()
    RightMotor.stop()
    LeftMotor.stop()

##########################
# Maze Solving Functions #
##########################

# Setup for left wheel sensor
def Lwheelirq(which):
    global LDistance            # count of wheel markers
    ledL.value(lwheel.value())  # light the sensor bar LED accordingly
    LDistance += 1              # increase count

# Setup for left wheel sensor
def Rwheelirq(which):
    global RDistance            # count of wheel markers
    ledR.value(rwheel.value())  # light the sensor bar LED accordingly
    RDistance += 1              # increase count

#Function for getting inputs from a remote
def remote():
    global remote
    led.off()
    while True:
        char = remote.get()
        if char:
            led.on()
            return char

 
def forward(dist, turn): #dist = how far to go forward | one cell = 28
    dist_corrected = False   # new V4_1:only correct distance once
    global LDistance, RDistance
    LDistance=0
    RDistance=0
    (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 < dist or RDistance < dist:
        #gc.collect()  # new V4_1:#Garbage collection not needed
        (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 turn == 0 and dist_corrected == False and (LDistance + 235) < dist:
            if L != OldL or R != OldR:
                dist = LDistance + 235
                dist_corrected = True
                led.on()
            else:
                led.off()
        
        #Stearing
        if 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
            
        RightMotor.speed(rspeed)  # send speeds to motors
        LeftMotor.speed(lspeed)
            
        #if there is left wall stear to left target
        #if there is right wall stear to right target
        #no walls? go straught ahead
        
        if turn == 0:
            OldL = L
            OldR = R
        
    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.2)          # new V4_1:
    forward(lturn_fwd, 1)
    lmotor.speed(0)
    rmotor.speed(0)


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.2)     # new V4_1:
    forward(rturn_fwd, 1)
    lmotor.speed(0)
    rmotor.speed(0)


def u_turn():
    global LDistance, RDistance
    LDistance=0
    RDistance=0
    (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.1)
    if front >= fpresent:        # New V4_1
        go_back()
        forward(fwd, 1)
    else:
        forward(uturn_fwd,1)     # new V4_1:
    lmotor.speed(0)
    rmotor.speed(0)


def go_back():
    global LDistance, RDistance
    LDistance=0
    RDistance=0
    rmotor.speed(-15)
    lmotor.speed(-15)
    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

#How to start the robot
if SW4.value() == 0:
    #With remote
    command = remote.get()
    while command != 'P':
        (left,front,right)= ReadWALLS()
        time.sleep(0.1)
        print("T L:",left,"F:",front,"R:",right)
        command = remote.get()
        print(command)
        if left > lpresent:
            ledL.on()
        
        else:
            ledL.off()
        
        if front> fpresent:
            led2.on()
        
        else:
            led2.off()
        
        if right > rpresent:
            ledR.on()
        
        else:
            ledR.off()
else:
    #With button
    while Button.value() == 0:
        (left,front,right)= ReadWALLS()
        time.sleep(0.1)
        print("B 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(0.5)
(left,front,right)= ReadWALLS()
#used for steering
ltarget = left
ftarget = front
rtarget = right     

f = open("log.txt", "w")
f.write("Starting\n\n")
f.close()

while True:
    if SW1.value() == 1:
        #Wall follower 
        corridor()
        (left,front,right)= ReadWALLS()
        if left < leftvalue:
            left_delay()    
        else:
            right_delay()      

    else:
        #Maze solver
        
        #Getting wall values
        (left,front,right)= ReadWALLS()
        
        #Working out what walls are present
        #1 = present / 0 = not present
        if left > lpresent:
            L = 1
        else:
            L = 0
            
        if front > fpresent:
            F = 1
        else:
            F = 0
            
        if right > rpresent:
            R = 1
        else:
            R = 0
        led.on()
        
        irqvals=machine.disable_irq()
        f = open("log.txt", "a")
        f.write(str(L)+","+str(F)+","+str(R)+"\n")
        f.close()
        machine.enable_irq(irqvals)
        
        if finalsolve == False:
            if SW3.value() == 1:
                #Waiting for instructions from the remote
                command = remote.get()
                led.on()
                while command == '?':
                    #gc.collect()   #Garbage collection not needed
                    command = remote.get()
                led.off()
                
            else:
                #Use solver code
                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 what walls are present
            
            f = open("log.txt", "a")
            f.write(str(command)+"\n\n")
            f.close()
        
        if type(command) == list:
            finalsolve = True
            for i in range(len(command)):
                pcommand = command[i]
                
                if pcommand[0] == '+' and len(pcommand) > 1:
                    # new V4_1:  run forward() many times
                    numcells = pcommand[1:]
                    while numcells >1:
                        speed = fast
                        forward(fwd,0)
                        numcells -= 1
                    speed = slow
                    #forward(28*pcommand[1:])
                    forward(fwd,0)
                            
                else:           
                    if pcommand == '>':
                        right_turn()
                        
                    elif pcommand == '<':
                        left_turn()
                        
                    elif pcommand == 'O':
                        u_turn()    # was go_back() in V4
                        #go_back()
                        
        else:
            #Deciding how to move
            if command == '+':
                forward(fwd, 0)
                
            elif command == '>':
                right_turn()
                
            elif command == '<':
                left_turn()
                
            elif command == 'O':
                u_turn()
            # new V4_1:    digits go forward n times
            elif command.isdigit():
                    
                numcells = int(command)
                while numcells >1:
                    speed = fast
                    forward(fwd,0)
                    numcells -= 1
                speed = slow
                #forward(28*pcommand[1:])
                forward(fwd,0)