# May21.py
# Calibration function changed, and now used
#  it uses stored values if SW4 is on, else reads new values
#  and stores them
# Screen messages improved.
# Tuning still needed

moneyForNothing_Remastered1996_ = False
screen = True
lifted = False
'''  incorporated...
  (1) "#" to run auto
  (2) "0" to stop. This needs to be put outside cb()
        trying readWalls()
  (3) check globals Mr Robinson
  (4) TOBY updated reduced to 61
  (5) leftThresh/2 used as threshold for left turn
  (6) "U" and "D" for up arrow/downarrow
  (7) 
  (8) L,R,C decision order not R,L,C
  (9) left turn 0.8  1.25 motor speeds
  (10) corridor() actually called at end
  (11) decode dictionary

# Not yet done....
  (7) Emily, corridor and turnleft to write log
  (12) linear wall sensors
  (13) keep count of runs. Stop run by lifting robot out of maze
  (14) BOH on 1st run
  (15) Read Run 1 log to find other BOHs
  (16) TUNING section (incl right motor tuning )
  (17) FT from front threshold during calibrate
'''
#-----------------------------------------
from machine import Pin, Timer, PWM, ADC, I2C,UART
import utime, time
from ssd1306 import SSD1306_I2C

SW1 = Pin(0, Pin.IN, Pin.PULL_UP)       #Hw1 1st switch +++
SW2 = Pin(1, Pin.IN, Pin.PULL_UP)       #Hw2 2nd switch +++
SW3 = Pin(2, Pin.IN, Pin.PULL_UP)       #Hw4 3rd switch +++
SW4 = Pin(3, Pin.IN, Pin.PULL_UP)
SWVAL = SW1.value()+2*SW2.value()+4*SW3.value()
tuningNum = SWVAL
if screen:
    i2c=I2C(1,sda=Pin(18),scl=Pin(19),freq=200000)
    dsp=SSD1306_I2C(128,32,i2c)#,48
    dsp.fill(0)
else:
    print("SWVAL",SWVAL)
    time.sleep(1)
# Bluetooth serial potr
uart = UART(0, baudrate=9600, tx=Pin(16), rx=Pin(17), bits=8, parity=None, stop=1) # +++

from ir_rx.print_error import print_error  # Optional print of error codes

# Import IR reception class for mini- remote
from ir_rx.nec import NEC_8
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: "U",                   #       UP
    8: "L",  28: "O", 90: "R", # Left, OK, Right
    82: "D",                   #      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,)  # 
        except:
            tr = "?"
        #if (tr == "0"):  #stop  (2)
        #    st()
        if (tr == "?"):  # unknown
            print(tr, data)
            if screen:
                dsp.fill(0)
                dsp.text("code "+ str(data),0,0)
                dsp.text(str(tr),0,10)
                dsp.show()
        else :
            print(tr,end="")
            if tr == "P": print("")
            if screen:
                dsp.fill(0)
                dsp.text("code "+ str(data),0,0)
                dsp.text(str(tr),0,10)
                dsp.show()


    

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


if screen:
    i2c=I2C(1,sda=Pin(18),scl=Pin(19),freq=200000)
    dsp=SSD1306_I2C(128,32,i2c)#,48
    dsp.fill(0)
    
def BT(strg):
    uart.write(bytes(strg,"utf8"))
    print("BT"+strg)

Olderr = 0

lwheel = Pin(6,Pin.IN)
rwheel = Pin(7,Pin.IN)
tst = Pin(14, Pin.OUT) #transistor
led_onboard = Pin(25, Pin.OUT)
led = Pin(25, Pin.OUT)
ledL = Pin(13, Pin.OUT)
ledR = Pin(8, Pin.OUT)
Lsensor = ADC(27)
Msensor = ADC(26)
Rsensor = ADC(28)
button = Pin(22,Pin.IN, Pin.PULL_DOWN)


left_dir = Pin(9, Pin.OUT)
right_dir = Pin(10, Pin.OUT)
left_pwm = PWM(Pin(11))
right_pwm = PWM(Pin(12))
left_pwm.freq(1000)
right_pwm.freq(1000)
LDistance = 0
RDistance = 0
Distance = 0
# TUNING

##############################
#####                     ####
#####      tune()         ####
#####                     ####
##############################
# new 2/5/25 Mr Fisher.
# can respond to switches or TV remote
# takes argument 0 - 4 to choose tuning

def tune(number):
    global motorcomp, SPspeed, FT,TOBY
    global mult, Kd, speed, LSPD, RSPD
    global velociraptor_mongoliensis
    global manospondylus_gigas,tuningNum

    tuningNum = number
    
    print("tune("+str(number)+")")
    if (number == "0"): number = 0
    if (number == "1"): number = 1
    if (number == "2"): number = 2
    if (number == "3"): number = 3
    if (number == "4"): number = 4
    
    motorcomp = 1.075# motor compensation
    SPspeed = 8000     # right spin speed
     # (4) left wheel encoder 90 degree spin   #TUNING
    FT = 9000          # front threshold
    # spin
# tune 0
    if number == 0:
        mult = 2
        Kd = 2
        speed = 8000
        LSPD = 0.8
        RSPD = 2
        velociraptor_mongoliensis = 80 # FWD before Left
        manospondylus_gigas = 10        # kink Right
        TOBY = 64                      # Spin right
        
    elif number == 1:
        mult = 2
        Kd = 2
        speed = 10000
        LSPD = 0.7
        RSPD = 2
        velociraptor_mongoliensis = 80
        manospondylus_gigas = 0
        TOBY = 64
        
    elif number == 2:
        mult = 2.5 #1
        Kd = 2
        speed = 12500
        LSPD = 0.75
        RSPD = 2
        velociraptor_mongoliensis = 50
        manospondylus_gigas = 5
        TOBY = 72
    else:       # number = 3 or more
        mult = 3 #1
        Kd = 2
        speed = 15000
        LSPD = 0.87
        RSPD = 2
        velociraptor_mongoliensis = 30
        manospondylus_gigas = 10
        TOBY = 64
        
    if screen:
        dsp.fill(0)
        dsp.text(str(number),100,0)
        dsp.text("Sp "+ str(speed),0,0)
        dsp.text("Kp "+ str(mult)+" Kd "+ str(Kd),0,10)
        dsp.text("Ls "+ str(LSPD)+" Rs "+ str(RSPD),0,20)
        dsp.show()
        time.sleep(0.4)
        BT("TU"+str(number))
tune(SWVAL)
print("Speed",speed)

R2 = 1#right turn
D2 = 2#U turn
C3 = 3#left turn
PO = 4#corridor

##############################
#####                     ####
#####     motors()        ####
#####                     ####
##############################
def motors(left,right):
    #print("motors("+str(left)+","+str(right)+")")
    right *= motorcomp #1.103    #TUNING
    if left<0:
        left = -left
        left_dir.on()
    else:
        left_dir.off()
    if right<0:
        right = -right
        right_dir.on()
    else:
        right_dir.off()
    
    left  = min(left,65001)
    right = min(right,65001)
    left_pwm.duty_u16(int(left)) 
    right_pwm.duty_u16(int(right)) 

##############################
#####                     ####
#####    clearLOG()       ####
#####                     ####
##############################
def clearLOG():
    f = open("log.txt", "w")
    f.close()
    if screen:
        dsp.fill(0)
        dsp.text("log gone, never ",0,0)
        dsp.text("to return",0,10)
        dsp.show()
    time.sleep(1)
    
def writeLOG(msg):
    print("msg",msg)
    f = open("log.txt", "a")
    f.write(msg + "\n")
    f.close()
    
def jotLOG(msg):
    print("mr clapson",msg)
    f = open("log.txt", "a")
    f.write(str (msg))
    f.close()
    
    

if screen:
    dsp.fill(0)
    dsp.text("speed:"+str(speed),0,0)
    dsp.text("FT:"+str(FT),0,10)
    dsp.text("mult:"+str(mult),0,20)
    dsp.show()
time.sleep(1)

if SW4.value() == 1:
    dsp.fill(0)
    dsp.text("Saved Calib",0,10)
    dsp.show()

rwd = .001     #TUNING
def tobynotgood():
    motors(-speed,-speed)#reverse
    time.sleep(1)
    stop()
    
def malcolm(dist):
    stopat = LDistance + RDistance + dist
    motors(speed,speed)
    while (LDistance + RDistance) < stopat:
        pass
    stop()

def Mr_Fisher(angle):
    oldAngle=LDistance - RDistance
    
    BT("MF"+str(angle)+" ")  # Bluetooth
    
    #while (LDistance - RDistance)< (angle + oldAngle):
    motors(10000,-10000)
    time.sleep(angle/200)
    stop()


oldState = None
def Mr_Robinson(newState):
    return
    global oldState, Distance, LDistance, RDistance  #(3)
    if oldState == None:
        oldState = newState
        jotLOG(newState)#start line in log
    else:
        if oldState != newState:
            writeLOG(" "+str( LDistance + RDistance - Distance ) )
            jotLOG(newState)
            Distance = LDistance + RDistance
    
##############################
#####                     ####
#####   Lwheelirq()       ####
#####                     ####
##############################
def Lwheelirq(which):
    global LDistance,IR            # count of wheel markers
    IR= "L"
    #if intenb:
    LDistance += 1              # increase count

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

# 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)#while True:
#    led.value(button.value())
#while True:
 #   ledL.value(lwheel.value())
  #  ledR.value(rwheel.value())
   # print(LDistance,RDistance)
    #time.sleep(1)

    

def stop():
    motors(0,0)

def st():
    stop()
    1/0
##############################
#####                     ####
#####   readWalls()       ####
#####                     ####
##############################
def readWalls():
    global lifted
    if (tr == "0"):  #stop  (2)
        print("STOP")
        st()
    tst.value(0)
    time.sleep(rwd)
    LMin = Lsensor.read_u16()
    MMin = Msensor.read_u16()
    RMin = Rsensor.read_u16()

    tst.value(1)
    time.sleep(rwd)
    MMax = Msensor.read_u16()
    RMax = Rsensor.read_u16()
    LMax = Lsensor.read_u16()

    tst.value(0)
    Lside = LMax - LMin
    Mside = MMax - MMin
    Rside = RMax - RMin

    if (Lside <400) & (Mside <400) & (Rside <400 ):
        if lifted== False: print("Lift"); BT("Lft")
        lifted = True
        if screen:
            dsp.fill(0)
            dsp.text("Lifted  "+str(tuningNum), 0,0)
            dsp.show()

    else:
        lifted = False
        
    return Lside, Mside, Rside

##############################
#####                     ####
#####   spinRight90()     ####
#####                     ####
##############################    
def spinRight90():
    global LDistance
    Fullspeed = True
    #LDistance = 0
    x = LDistance+TOBY
    #dsp.fill(0)
    #dsp.text("turning right", 0,0)
    #dsp.show()
    #right_dir.on()     # reverse right motor
    #left_dir.off()     # forward left motor
    #left_pwm.duty_u16(speed)   #reset
    #right_pwm.duty_u16(int(speed*1.2))
    motors(SPspeed,-SPspeed*1.2)
    # slow down early...
    while LDistance<x:
        if Fullspeed and LDistance>x-10:           #TUNING
            Fullspeed= False
            motors(SPspeed-1000,-(SPspeed-1000))    #TUNING
        pass
    motors(0,0)
    print("EndRSp")

    
    
    
##############################
#####                     ####
#####     turnLeft()      ####
#####                     ####
##############################

def turnLeft():
    BT("TL ")  # Bluetooth
    ledL.on()
    (l,f,r) = readWalls()
    # spin a bit
    Mr_Fisher(manospondylus_gigas)
    malcolm(velociraptor_mongoliensis)
    if screen:
        dsp.fill(0)
        dsp.text("turning left", 0,0)
        dsp.show()
    motors(speed*LSPD,speed*RSPD)
    while True:
        (l,f,r) = readWalls()
        if lifted:stop();return
        if l > leftThresh*0.5: #"and  r < rightThresh"
            break
        
    ledL.off()
    stop()
    BT("TLx ")  # Bluetooth    
##############################
#####                     ####
#####       BOH()         ####
#####                     ####
##############################
def BOH():
    print("bat out of hell")
    speed2 = speed + 2000
    #speed2 = 15000
    (l,f,r) = readWalls()
    KD = 60
    
    while True:
        Olderr = 0
        (l,f,r) = readWalls()
        error = l - leftThresh
        error = error*mult*(speed2/speed)
        deriv = (error - Olderr)*KD
        sDiff = error + deriv
        Olderr = error
        lSpeed = max(speed2+sDiff,0)
        lSpeed = min(lSpeed,65000)
        rSpeed = max(speed2-sDiff,0)
        rSpeed = min(rSpeed,65000)
        (l,f,r) = readWalls()
        #left_pwm.duty_u16(int(lSpeed))   #reset
        #right_pwm.duty_u16(int(rSpeed))
        motors(lSpeed,rSpeed)
            
        if f > FT/4.5:
            print(f)
            writeLOG("BOH ")
            stop()
            return
          

##############################
#####                     ####
#####    calibrate()      ####
#####                     ####
##############################
# returns 3 wall values
# if SW4 is on, it returns stored values
def calibrate():
    global FT
    if (SW4.value() == 1):
        try:
            import calib
            if screen:
                dsp.fill(0)
                dsp.text("Saved calib",0,0)
                dsp.show()
                time.sleep(1)
            return( (calib.leftThresh,calib.frontThresh,calib.rightThresh) )
        except:
            pass
    # read averaged values from walls
    dsp.fill(0)
    dsp.text("Calibrating 99",0,0)
    dsp.text("SW4"+str(SW4.value()),0,10)
    dsp.show()
    
    x=1
    (leftThresh,frontThresh,rightThresh) = (0,0,0)
    AverageAmount = 0.8   # amount of old reading
    NewAmount = 1-AverageAmount  # amount of new reading
    print("Calibrating")
    BT("Calib ")  # Bluetooth
    while button.value() == 0:
        (l,f,r)=readWalls()
        leftThresh  = (leftThresh*AverageAmount)  + (l*NewAmount)
        frontThresh = (frontThresh*AverageAmount) + (f*NewAmount)
        rightThresh = (rightThresh*AverageAmount) + (r*NewAmount)
        print(leftThresh,frontThresh,rightThresh,LDistance)
        
        if leftThresh > 7000:
            ledL.on()
        else:
            ledL.off()
        if rightThresh > 6000:
            ledR.on()
        else:
            ledR.off()
        if frontThresh > FT:
            led.on()
        else:
            led.off()
        #frontThresh = FT
        FT = frontThresh  #(17)

        time.sleep(0.1)
    # end of calibration
    # save readings to file
    BT("Cs ")  # Bluetooth
    print("Saving")
    f = open("calib.py", "w")
    f.write("leftThresh  = " + str(int(leftThresh)) + "\n")
    f.write("frontThresh = " + str(int(frontThresh))+ "\n")
    f.write("rightThresh = " + str(int(rightThresh))+ "\n")
    f.close()
    print("Saved")
    if screen:
        dsp.fill(0)
        dsp.text("Live calib",0,0)
        dsp.show()
    return( (leftThresh,frontThresh,rightThresh) )

        
# Main program strts here .......

##############################
#####                     ####
#####         MAIN        ####
#####                     ####
##############################
#main loop
stop()
led.off()

### OLD Calibration code.....
'''
if screen:
    dsp.fill(0)
    dsp.text("Calibrating",0,0)
    dsp.show()
    BT("\nCalib ")  # Bluetooth
while True:
    (leftThresh,frontThresh,rightThresh)=readWalls()
    print(leftThresh,frontThresh,rightThresh)
    if leftThresh > 7000:
        ledL.on()
    else:
        ledL.off()
    if rightThresh > 6000:
        ledR.on()
    else:
        ledR.off()
    if frontThresh > FT:
        led.on()
    else:
        led.off()
    #frontThresh = FT
    FT = frontThresh  #(17)

    time.sleep(0.1)
    if button.value() == 1:
        break
#print("BP1")

BT("Cx "+str(leftThresh)+" "+str(frontThresh)+" "+str(rightThresh))  # Bluetooth

while button.value() == 1:
    pass
print("BR1")
'''

(leftThresh,frontThresh,rightThresh) = calibrate()
FT = frontThresh  #(17)

# Await hand wave
if screen:
    dsp.fill(0)
    dsp.text("Wave hand",0,0)
    dsp.show()
(l,f,r) = readWalls()

while f < 1500:
    (l,f,r) = readWalls()
    print(f)
    time.sleep(0.1)

led_onboard.on()

while f > 2000:
    (l,f,r) = readWalls()
    print(f)
    time.sleep(0.1)
led_onboard.off()

### END OF OLD Calibration code.....


##############################
#####                     ####
#####     corridor()      ####
#####                     ####
##############################
def corridor():
    global tr,  moneyForNothing_Remastered1996_
    MySpeed = speed
    
    if moneyForNothing_Remastered1996_ :
        MySpeed = speed + 2000
        moneyForNothing_Remastered1996_ = False
    
        dsp.fill(0)
        dsp.text("BOH", 0,0)
        dsp.show()
    
    print("corridor")
    BT("\nCor ")  # Bluetooth
    while True:
        if (tr == "1") or (tr == "2") or (tr == "3"):
            stop()
            tune (tr)
            tr = "?"
        global LDistance, RDistance, l, Distance, Olderr, OldL
        (l,f,r) = readWalls() # checking for a wall ahead, or missing left wall
        if f >= FT:
            
            stop()
            if screen:
                dsp.fill(0)
                dsp.text("Front "+ str(f),0,0)
                dsp.show()
            BT("Cx ")  # Bluetooth    
            return
        if l < leftThresh/2:
            stop()
            if screen:
                dsp.fill(0)
                dsp.text("Left "+ str(l),0,0)
                dsp.show()
            BT("Cx ")  # Bluetooth    
            return
        time.sleep(0.01)
        Distance = LDistance + RDistance
        error = l - leftThresh
        deriv = (error - Olderr)*Kd
        sDiff = error + deriv
        sDiff = sDiff*mult
        Olderr = error
        lSpeed = MySpeed+sDiff
        #lSpeed = min(lSpeed,65000)
        rSpeed = MySpeed-sDiff
        #rSpeed = min(rSpeed,65000)
        #left_pwm.duty_u16(int(lSpeed))   #reset
        #right_pwm.duty_u16(int(rSpeed))
        motors(lSpeed,rSpeed)
        oldL = l
        
        
       
##############################
#####                     ####
#####       emily()       ####
#####                     ####
##############################
def emily():
    (l,f,r) = readWalls()
    print("emily")
    BT(" Em")  # Bluetooth
    ledR.on()
    if l > 0: #(leftThresh - 1000):  # why depend on left sensor to spin right??
        led_onboard.off()
        print("stp2")
        spinRight90()
        spincount = 1
        stop()

        (l,f,r) = readWalls()
        if f > FT/2 :
            spinRight90()
            spincount = 2
        if spincount == 1:
            #toby()
            pass
    # no right spin, or after right spin    
    (l,f,r) = readWalls()

    while l > leftThresh:
        motors(speed,-speed)
        (l,f,r) = readWalls()
    stop()
    ledR.off()
    BT("Ex ")  # Bluetooth    

# new MRF-------

if screen:
    dsp.fill(0)
    dsp.text("TVR to start",0,0)
    dsp.text(str(tr),0,10)
    dsp.show()

while True:
    if tr != "?":
        if tr == "O":
            (l,f,r) = readWalls()
            emily()
            
        if tr == "L":
            turnLeft()
            
        if (tr == "1") or (tr == "2") or (tr == "3"):
            tune(tr)
            
        if tr == "7":
            motors(10000,10000)
            time.sleep(0.5)
            stop()
            
              
                
             
        if tr == "8":
            malcolm(velociraptor_mongoliensis)
            if screen:
                dsp.fill(0)
                dsp.text("velo "+ str(velociraptor_mongoliensis),0,0)
                dsp.show()
                
        if tr == "9":
            Mr_Fisher(manospondylus_gigas)
            if screen:
                dsp.fill(0)
                dsp.text("Mano "+ str(manospondylus_gigas),0,0)
                dsp.show()
            
        if tr == "R":
            spinRight90()
        if (tr == "v")|(tr=="D"): #(6)
            spinRight90()
            spinRight90()
        if (tr == "^")|(tr=="U"):  # (6)
            corridor()
        if tr == "#":  # (1)
            break
        if tr == "*":
            moneyForNothing_Remastered1996_ = True
        tr = "?"
        
            
        
oldL = 0
tr = "?"   # clear TV remote commands
while True:
    # check for TVR and re-tune if necessary
    # if tr == "1"  etc.etc.
    # record start of run in logstring
    while True:
        # Or put re-tuning in here...
        if lifted: break
        (l,f,r) = readWalls()
        if l < leftThresh/2:  # sudden drop in left wall
     #    if (l - oldL)<-1000:  # sudden drop in left wall
            print(l,oldL)
            Mr_Robinson(C3)
           
            turnLeft()
            ledL.off()
            
        if f > FT/2:			# start right spin
            emily()
        else:
            corridor()
            #--------- Corridor steering ------------
            Mr_Robinson(PO)
        if lifted:
            stop()
            BT("Liftx")
            break    # end of run if lifted
    BT("Outer")
    stop()
    if screen:
        dsp.fill(0)
        dsp.text("   LIFTED  "+str(tuningNum),0,0)
        dsp.text("button for",0,10)
        dsp.text("  next run",0,20)
        dsp.show()

    while button.value()==0:
        if (tr == "1") or (tr == "2") or (tr == "3"):
            tune(tr)
            tr="?"
        pass
    lifted = False
    #st()
    # write logstring to log file

#left_pwm.duty_u16(0)   #reset
#right_pwm.duty_u16(0)
motors(0,0)