# Update by Mr F 22/9/26 - see end for details
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)
# record 
# Change 1 - await Button to start after calibration
# Change 2 record ltarget and rtarget from current wall readings
(left,front,right)= ReadWALLS()
ltarget = left
rtarget = right
print("Short cell")
lmotor.speed(20) #was 30
rmotor.speed(20) # was 30
while ((LDistance + RDistance)/2) < (fwdshortcell):  # measured 177mm
    pass
lmotor.speed(0)
rmotor.speed(0)
#print("Stop")
#1/0
time.sleep(0.5)

#print("forward(112, 1)")
LDistance = 0
RDistance = 0

forward(fwd, 0,4)  # go fwd 4 cells no correction
print("right 135 degrees")
lmotor.speed(30)
rmotor.speed(10)
while (LDistance - RDistance) < diag135R:
    pass

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

LDistance = 0
RDistance = 0

print("diagonal")
while ((LDistance + RDistance)/2) < (300*2):
    pass
lmotor.speed(0)
rmotor.speed(0)
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.

'''