# 0.434 mm per ecoder count

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

import machine
from UKMARS_New import *

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

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

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

#Encode Variables
intenb = True
LDistance = 0  #Value from left wheel incoder
RDistance = 0  #Value from right wheel incoder

#Speeds
speed = 18  #25
fast = 35  #35
slow = speed  # 20

#Hysterosis
H = 200
LH = 0
RH = 0

#Acceleration
acc = 1
deacc_dist = 100

mult = 0.0006  #old value = 0.0005 amount of steering in corridor

#forawrd
fwd = 420  #420
fast_fwd = 425  #430
fwd_correction = 240

#left_turn
l_bturn_fwd = 12  # 18
lturn_amount = 286
lturn_fwd = 210  # 210 

#right_turn
r_bturn_fwd = 12
rturn_amount = 286  # 245
rturn_fwd = 210  # 210

#u_tuen
uturn_amount = 252  #245
uturn_fwd = 135 #135      

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

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

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

        
def calibration():
    global lpresent, rpresent, fpresent, fstop
    
    #lpresent
    ledL.on()
    while Button.value() == 0:
        pass
    (left, front , right) = ReadWALLS()
    lpresent = left
    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 bluetooth_calibration():
    pass
 
def forward(dist, correction, num = 1): #dist = how far to go forward | one cell = 28
    global LDistance, RDistance, LH, RH, intenb, speed
    
    #resets encode values
    intenb = False
    LDistance = 0
    RDistance = 0
    intenb = True
            
    L, F, R = walls()
    
    #Correction - Walls when starting
    if L:
        OldL = 1
    else:
        OldL = 0
            
    if R:
        OldR = 1
    else:
        OldR = 0
    
    #Acceleration - sets starting speed
    if num > 1:
        target_speed = speed
        speed = 10  #8
    
    while LDistance < dist*num or RDistance < dist*num:
        left, front, right = ReadWALLS()
        if front > fstop:
            break
        
        #hysterosis        
        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
        
        #correction
        if correction == 1:
            if L < OldL or R < OldR:
                if LDistance + fwd_correction <= dist or RDistance + fwd_correction <= dist:
                    led.on()
                    dist = LDistance + fwd_correction
                    correction = 0
        
        #acceleration for forward longer than 1 cell
        if num > 1:
            #speed up
            if target_speed > speed:
                speed += acc
                
            #slowing down                
            if dist * num - deacc_dist <= LDistance:
                speed = target_speed * 0.75
        
        #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 straight ahead
        
        #wall previously for correction
        OldL = L
        OldR = R
    
    #reverts speed back to speed before acceleration
    if num > 1:
        speed = target_speed
        
    lmotor.stop()
    rmotor.stop()
    led.off()
    

def forward_simple(dist, nofront = False):
    global LDistance, RDistance, intenb, speed
    
    #Reset encoder values
    intenb = False
    LDistance = 0
    RDistance = 0
    intenb = True
    
    while dist > LDistance or dist > RDistance:
            
        L, F, R = walls()
        
        #fstop
        if not nofront:
            if front > fstop:
                break
        
        #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
            
        lmotor.speed(lspeed)
        rmotor.speed(rspeed)
    
    lmotor.stop()
    rmotor.stop()
    
    intenb = False
    LDistance = 0
    RDistance = 0
    intenb = True
  
    
def left_turn():
    global LDistance, RDistance, intenb
    
    #forward before turn
    forward_simple(l_bturn_fwd, True)
    time.sleep(0.05)
    
    #turning
    rmotor.speed(speed + 5)
    lmotor.speed(0)
    while RDistance < lturn_amount:
        pass
    lmotor.speed(0)
    rmotor.speed(0)
    time.sleep(0.05)
    
    #forward after turning
    forward_simple(lturn_fwd)
    time.sleep(0.05)


def right_turn():
    global LDistance, RDistance, intenb
    
    #forward before turning
    forward_simple(r_bturn_fwd, True)
    time.sleep(0.05)
    
    #turning
    lmotor.speed(speed + 5)
    rmotor.speed(0)
    while LDistance < rturn_amount:
        pass    
    lmotor.speed(0)
    rmotor.speed(0)
    time.sleep(0.05)
    
    #forward after turning
    forward_simple(rturn_fwd)
    time.sleep(0.05)


def u_turn(go_fwd):
    global LDistance, RDistance, intenb
    #resets encoder values
    intenb = False
    LDistance=0
    RDistance=0
    intenb = True
    
    (left,front,right)= ReadWALLS()
    
    #180 Spin
    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 wall before turning does go_back, if not goes forward to be a start of cell
    if not at end phase 2 goes forward one cell, if aa end of phase 2 doesn't go forward
    if using wall follow just goes forward to be a start of cell
    '''
    
    if front >= fpresent:        # New V4_1
        if go_fwd != 2:
            go_back()
            if go_fwd == 1:
                forward_simple(fwd)
        else:
            forward_simple(uturn_fwd) 
        
    else:
        forward_simple(uturn_fwd)     # new V4_1:
    
    lmotor.speed(0)
    rmotor.speed(0)


def go_back():
    global LDistance, RDistance
    intenb = False
    LDistance = 0
    RDistance = 0
    intenb = True
    
    #backs into wall
    rmotor.speed(-20)
    lmotor.speed(-20)
    
    #waits until robot not moving anyfurther back
    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)
    
    
def move(a):
    global speed
    #when in final solve, two character commands can be sent e.g +3
    if len(a) > 1 and a[0] == '+' and int(a[1:]) > 1:
        num_cells = int(a[1:])
        speed = fast
        forward(fast_fwd, 0, num_cells)
        speed = slow
    
    elif a[0] == '+': forward(fwd, 0)
        
    elif a == '>': right_turn()
        
    elif a == '<': left_turn()
        
    elif a == 'O': u_turn(1) #normal u_turn
        
    elif a == '-': u_turn(0) #u_turn without going foirward
    
    elif a == 'w': u_turn(2) #u_turn with go_back()
  
  
def walls():
    global 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
    

########
# Main #
########
    
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
    
# 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)

print("movement started")