# Update by Mr F 01/11/26 -
# each move is now in a function.
# each can be called by a TV remote button
# or "*" for all of them
# Even better update by yours truely, the best programming practices in the world, Zach D
import machine
from UKMARS_New import *

from R4D4Tuning import *
#############
# Functions #
#############
LDistance = 0
RDistance = 0

# 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

lwheel.irq(handler=Lwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) 
rwheel.irq(handler=Rwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING)

lmotor = Motor("L")
rmotor = Motor("R")


lmotor.speed(0)  # stop motors in case they are running.
rmotor.speed(0)

LDistance = 0
RDistance = 0
LH = 0
RH = 0


def walls():
    global L, F, R,left,front,right
    #Getting wall values
    (left,front,right)= ReadWALLS()
    
    #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()
    
    return(L, F, R)

def LFR(left,front,right):
    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()
    
    return(L, F, R)

def button_press():
    count = 0
    while Button.value() == 0:
        pass
    
    while Button.value() == 1:
        count += 1
        time.sleep(0.1)
    
    if count > 8:
        return('l')
    else:
        return('s')

def calibration():
    global lpresent, rpresent, fpresent, fstop
    print("Calib")
    
    #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, num = 1): #dist = how far to go forward | one cell = 28
    global LDistance, RDistance, LH, RH,left,front,right
    print("forward",dist,correction,num)

    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*num) or RDistance - r_init < (dist*num):
        (left, front, right)= ReadWALLS()
        #if front wall is close
        if front > fstop:
            print("fstop")
            break
        
        #Detecting if left wall or right wall is present
        if left > lpresent + LH:
            LH = -H
            L = 1
        else:
            L = 0
            LH = H
            
        if right > rpresent + RH:
            R = 1
            RH = -H
        else:
            R = 0
            RH = H
        
        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
            
        # send speeds to motors
        rmotor.speed(rspeed)  
        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()
    print("stop")
    lmotor.speed(0)
    rmotor.speed(0)
    # end of function fwd()
    
    
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
    
#ltarget = left
#rtarget = right 

led.on()
print("Awaiting l/s button press")
blength = button_press()
if blength == 'l':
    print("Long, calibrate present values")
    calibration()
else:
    print("Short press - starting")
led.off()
print("Clearing log")
f = open("log.txt", "w")
f.write("Start of log \n")
#f.flush()
f.close()
print("1 second after button")
time.sleep(1)
#########
#   F1  #
#########
def F1():
    global ltarget,rtarget
    #record ltarget and rtarget from current wall readings
    (left,front,right)= ReadWALLS()
    ltarget = left
    rtarget = right
    print("Targets set")
#########
#   F2  #
#########
def F2():
    global ltarget,rtarget,LDistance,RDistance,fwdshortcell
    #Short cell
    print("Short cell")
    LDistance = 0;    RDistance = 0
    lmotor.speed(20) #was 30
    rmotor.speed(20) # was 30
    while ((LDistance + RDistance)/2) < (fwdshortcell):  # measured 177mm
        pass
    lmotor.speed(0)
    rmotor.speed(0)
#########
#   F3  #
#########  
def F3():
    global ltarget,rtarget,LDistance,RDistance,fwd
    print("forward(112, 1)")
    LDistance = 0;    RDistance = 0
    forward(fwd, 0,4)  # go fwd 4 cells no correction




#########
#   F4  #
#########  
def F4():
    global LDistance,RDistance,diag135R
    LDistance = 0
    RDistance = 0

    print("right 135 degrees")
    lmotor.speed(30)
    rmotor.speed(10)
    while (LDistance - RDistance) < diag135R:
        pass
    lmotor.speed(0)
    rmotor.speed(0)
#########
#   F5  #
#########      
def F5():
    global LDistance,RDistance,diagcell
    lmotor.speed(30)
    rmotor.speed(30)
    LDistance = 0
    RDistance = 0

    print("diagonal")
    while ((LDistance + RDistance)/2) < (diagcell*2):
        pass
    lmotor.speed(0)
    rmotor.speed(0)
#########
#   F6  #
#########       
def F6():
    global LDistance,RDistance,diag45R
    time.sleep(0.5)
    print("right 45 degrees")
    lmotor.speed(30)
    rmotor.speed(13)
    LDistance = 0
    RDistance = 0
    while (LDistance - RDistance) < diag45R:
        pass
    lmotor.speed(0)
    rmotor.speed(0)
    time.sleep(0.5)
    
from ir_rx.nec import NEC_8         # Import IR reception class for mini- remote
from ir_rx.print_error import print_error  # Optional print of error codes

p = Pin(20, Pin.IN)

# Mini NEC-8 
decode = {
    69: "1", 70: "2", 71: "3",
    68: "4", 64: "5", 67: "6",
    7:  "7", 21: "8",  9: "9",
    22: "*", 25: "0", 13: "#", 
    
    24: "+",                   #       UP
    8: "<",  28: "O", 90: ">", # Left, OK, Right
    82: "-",                   #      Down
}

##############################
#####                     ####
#####       cb()          ####
#####                     ####
##############################
tr="?"
last_cb = 0

# call-back from IR receiver interrupt:
def cb(data, addr, ctrl):
    global last_cb,tr
    tr="?"
    time_since = time.ticks_diff(time.ticks_ms(), last_cb)
    last_cb = time.ticks_ms()
    if time_since < 500:  # NEC protocol sends repeat codes.
        #print(".",end="")
        pass
    
    else:
        #print(f"Data = {data}")
        try:
            tr = decode.get(data,)  #
            '''
            if(tr=="U"):
                tr = "+"
            if(tr=="D"):
                tr = "-"
            if(tr=="L"):
                tr = "<"
            if(tr=="R"):
                tr = ">"
            '''    
            print("tr==",tr)
        except:
            tr = "?"
        #if (tr == "0"):  #stop  (2)
        #    st()
        if (tr== None):tr= "?"




ir = NEC_8(p,cb)
ir.error_function(print_error)  # Show debug information
#==========================================================
nextStep = False

while True:
    cmd = tr
    tr = "?"
    if (cmd == "#"):
        if nextStep:
            nextStep = False
            print("next OFF")
            led.off()
        else:
            nextStep = True
            print("next ON")
            led.on()
    
    if (cmd == "*"):
        F1();F2();F3();F4();F5();F6()
        continue
    if (cmd == "1"):
        F1()
        if nextStep:F2()
        continue
    if (cmd == "2"):
        F2()
        if nextStep:F3()
        continue
    if (cmd == "3"):
        F3()
        if nextStep:F4()
        continue
    if (cmd == "4"):
        F4()
        if nextStep:F5()
        continue
    if (cmd == "5"):
        F5()
        if nextStep:F6()
        continue
    if (cmd == "6"):
        F6()
        continue
    time.sleep(0.1)


################################### RUN ############################
F1()        # set targets
F2()        # short cell
time.sleep(0.5)
F3()        # forward 4 cells
F4()		# right 135
F5()        # diagonal
F6()        # right 45 degrees
print("End of run")









1/0 
print("l", LDistance, "R", RDistance)

lmotor.speed(30)
rmotor.speed(30)

time.sleep(1)

lmotor.speed(0)
rmotor.speed(0)
# summary of changes
'''
motors stopped at program start
LH and RH need to be declared
ltarget and rtarget need to be set after button press
  from ReadWalls values
It prints out what its doing, so that you can see what stage its at (when on Thonny)
forward routine prints its parameters
motors speeds needed setting for 135 degree turn
short cell distance needed change
a pause of 1 second after button press allows you to remove finger
fwd from tuning file is used as cell length
num is set to 4, for 4 cell movement
# steering not tested.

'''