# 0.434 mm per ecoder count

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

import machine
from UKMARS_New import *
from tvr_lib import Remote
from values import lpresent

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

DONT_STOP = True

led = machine.Pin("LED", machine.Pin.OUT)
ledL.off()
ledR.off()

# 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

# Hysterosis
H = 200
LH = 0
RH = 0

# Acceleration
acc = 1
deacc_dist = 100

mult = 0.0016  # old value = 0.0015 amount of steering in corridor
mult_steering = 0.0006

# forward speeds
fwd = 415  # 410        # one normal cell forward
no_stop_fwd = 420       # forward with no stop
fwd_correction = 240

# left turn
b_turn_fwd = 15  # 12
turn_amount = 245 #240
a_turn_fwd = 150  # 180 

# u_turn
uturn_amount = 250  # 250
uturn_fwd = 130  # 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(v=0, c=0, s=0):
    s = int(s)
    global lpresent, rpresent, fpresent, fstop, slow, fast
    
    if SW4.value() == 0:
        # 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)
        
        # logging 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()
        
    else:
        if c == '+':
            if v == 'l': lpresent += s
            elif v == 'f': fpresent += s
            elif v == 'r': rpresent += s
            elif v == 's': fstop += s
            elif v == 'S': slow += s
            elif v == 'F': fast += s
            
        elif c == '-':
            if v == 'l': lpresent -= s
            elif v == 'f': fpresent -= s
            elif v == 'r': rpresent -= s
            elif v == 's': fstop -= s
            elif v == 'S': slow -= s
            elif v == 'F': fast -= s
            
        elif c == 'V':
            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.write("slow = "+str(slow)+"\n")
            f.write("fast = "+str(fast)+"\n")
            f.close()


def forward(dist, correction = 0, num=1, stop_at_end=True): # dist = how far to go forward | one cell = 28
    """moves the robot forward"""
    
    global LDistance, RDistance, LH, RH, intenb, speed
    
    print("forward", LDistance)
    
    dist -= (LDistance + RDistance) / 2
    
    # resets encoder 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 or True:
        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
        print(L,R)
        # 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 or True:
            # speed up
            if target_speed > speed:
                speed += acc
                
            # slowing down
            if num == 1:
                if dist - (deacc_dist - 50) <= LDistance:
                    speed = target_speed * 0.75
            else:
                if dist * num - deacc_dist <= LDistance:
                    speed = target_speed * 0.5
        
        ledL.off()
        ledR.off()
        
        # Steering
        if L and R:
            error = (left - ltarget + rtarget - right) / 2
            error *= mult
            
            lspeed = speed + error    # calculate motor speeds
            rspeed = speed - error
            
            ledL.on()
            ledR.on()
        
        elif L == 1:
            error = left - ltarget     # subtract target
            error = error * mult      # amount of steering
    
            lspeed = speed + error    # calculate motor speeds
            rspeed = speed - error
            
            ledL.on()
        
        elif R == 1:
            error = right - rtarget     # subtract target
            error = error * mult      # amount of steering
        
            lspeed = speed - error    # calculate motor speeds
            rspeed = speed + error
            
            ledR.on()
        
        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
        
        print("LWheel: "+str(LDistance)+"RWheel :"+str(RDistance))
    
    #reverts speed back to speed before acceleration
    if num > 1 or True:
        speed = target_speed
    
    if stop_at_end:  # stop at the end of the movement
        lmotor.stop()
        rmotor.stop()
    led.off()
    
    angle = LDistance - RDistance
    


def forward_simple(dist, nofront = False, finalsolve = False, stop_at_end=True):
    global LDistance, RDistance, intenb, speed, mult
    
    # Reset encoder values
    intenb = False
    time.sleep(0.0001)
    LDistance = 0
    RDistance = 0
    intenb = True
    
    if stop_at_end == False:
        speed = slow - 5
        
    while dist > LDistance or dist > RDistance:
            
        L, F, R = walls()
        
        #fstop
        if not nofront:
            if front > fstop:
                break
        
        ledL.off()
        ledR.off()
        
        # Steering
        
        if L and R:
            error = (left - ltarget + rtarget - right) / 2
            error *= mult
            
            lspeed = speed + error    # calculate motor speeds
            rspeed = speed - error
            
            ledL.on()
            ledR.on()
        
        elif L == 1:
            error = left - ltarget     # subtract target
            error = error * mult_steering      # amount of steering
    
            lspeed = speed + error    # calculate motor speeds
            rspeed = speed - error
            
            ledL.on()
        
        elif R == 1:
            error = right - rtarget     # subtract target
            error = error * mult_steering      # amount of steering
        
            lspeed = speed - error    # calculate motor speeds
            rspeed = speed + error
            
            ledR.on()
        
        else:
            lspeed = speed
            rspeed = speed
            
        lmotor.speed(lspeed)
        rmotor.speed(rspeed)
    
    if stop_at_end:
        lmotor.speed(0)
        rmotor.speed(0)
    '''
    angle = (LDistance - RDistance)* 0.6
    if abs(angle) > 5 and not finalsolve:
        if angle > 0:
            LDistance = 0
            lmotor.speed(-speed*0.5)
            while LDistance < angle:
                pass
            lmotor.speed(0)
        else:
            RDistance = 0
            rmotor.speed(-speed*0.5)
            while RDistance < -angle:
                pass
            rmotor.speed(0)
    '''
    intenb = False
    LDistance = 0
    RDistance = 0
    intenb = True
    
    if stop_at_end == False:
        speed = slow
  
    
def left_turn(finalsolve = False, stop_at_end=True):
    global LDistance, RDistance, intenb
    
    # forward before turn
    forward_simple(b_turn_fwd, True, stop_at_end=stop_at_end)
    if stop_at_end:
        time.sleep(0.05)

    rmotor.speed(speed)
    lmotor.speed(0)
    while RDistance < turn_amount:
        pass
    
    if stop_at_end:
        lmotor.speed(0)
        rmotor.speed(0)
        time.sleep(0.05)
    
    # forward after turning
    forward_simple(a_turn_fwd, finalsolve = finalsolve, stop_at_end=stop_at_end)
    if stop_at_end:
        time.sleep(0.05)


def right_turn(finalsolve = False, stop_at_end=True):
    """turns 90 degrees right"""
    global LDistance, RDistance, intenb


    # forward before turning
    forward_simple(b_turn_fwd, True, stop_at_end=stop_at_end)
    if stop_at_end:
        time.sleep(0.05)
    
    lmotor.speed(speed)
    rmotor.speed(0)
    while LDistance < turn_amount:
        pass  # wait until we've turned far enough
    
    if stop_at_end:
        lmotor.speed(0)
        rmotor.speed(0)
        time.sleep(0.05)

    # forward after turning
    forward_simple(a_turn_fwd, finalsolve=finalsolve, stop_at_end=stop_at_end)
    if stop_at_end:
        time.sleep(0.05)


def u_turn(go_fwd, finalsolve = False):
    """spins 180 and, if a wall is there, backs up to the wall behind it"""
    
    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*6/7:
        pass
    
    rmotor.speed(-15)
    lmotor.speed(15)
    left = 0
    right = 1
    while left > right:
        left, front, right = ReadWALLS()

    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, finalsolve = finalsolve)
        else:
            forward_simple(uturn_fwd, finalsolve = finalsolve) 
        
    else:
        forward_simple(uturn_fwd, finalsolve = finalsolve)  # 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 pause(wait_time=3.0):
    """pauses the robot for a moment"""
    
    lmotor.speed(0)
    rmotor.speed(0)
    
    time.sleep(wait_time)
    
def move(a, finalsolve = False):
    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(no_stop_fwd, 0, num_cells, stop_at_end=not (finalsolve and DONT_STOP))
        speed = slow
    
    elif a[0] == '+':
        if finalsolve:
            forward(no_stop_fwd, 0, stop_at_end=not (finalsolve and DONT_STOP))
        else:
            forward(fwd, 1, stop_at_end=not (finalsolve and DONT_STOP))
        
    elif a == '>': right_turn(finalsolve, stop_at_end=not (finalsolve and DONT_STOP))
        
    elif a == '<': left_turn(finalsolve, stop_at_end=not (finalsolve and DONT_STOP))
        
    elif a == 'O': u_turn(1, finalsolve) #normal u_turn
    
    elif a == 'U': u_turn(2, finalsolve) #wall follow u_turn
        
    elif a == '-': u_turn(0, finalsolve) #u_turn without going foirward
    
    elif a == 'w': u_turn(2, finalsolve) #u_turn with go_back()
    
    elif a == 'p': pause()
    
    else:
        lmotor.speed(0)  # stop the motors
        rmotor.speed(0)
  
  
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
    slow = 18
    fast = 35
    
speed = slow
    
# 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")