# see updated version
#08/11/24
#import main_updated
from machine import Pin, Timer, PWM, ADC, I2C
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)

if SW1.value() == 0:
    mult = 5
    speed = 6000
    SpinRightThresh = 100
    spinLeftThresh = 100
    lMotorTurnMult = 0.7
    RMotorLTurnMult = 1.5
    Rcount = 0
    forwardTime = 0.6
    FT = 850
    rSpinDelay = 0.075
else:
    mult = 5.5
    speed = 10000
    SpinRightThresh = 100
    spinLeftThresh = 100
    lMotorTurnMult = 0.4
    RMotorLTurnMult = 1.5
    Rcount = 0
    forwardTime = 0.3
    FT = 350 #front reading when i start a right spin
    rSpinDelay = 0

i2c=I2C(1,sda=Pin(18),scl=Pin(19),freq=200000)
dsp=SSD1306_I2C(128,32,i2c)#,48

#dsp.text('Hello',0,0)
#dsp.text('World',0,8)
#dsp.show()


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)
but =Pin(22, Pin.PULL_DOWN)
SW1 = Pin(0, Pin.IN, Pin.PULL_UP)
SW2 = Pin(1, Pin.IN, Pin.PULL_UP)
SW3 = Pin(2, Pin.IN, Pin.PULL_UP)       
SW4 = Pin(3, Pin.IN, Pin.PULL_UP)

#lwheel = Pin(4,Pin.IN)   #motor sensor
#rwheel = Pin(5,Pin.IN)
#LDistance = 0
#RDistance = 0
#intenb = True

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)

#while True:
#    led.value(button.value())


def spinRight():
    while True:
        (l,f,r) = readWalls()
        if f < SpinRightThresh and l < leftThresh:
            time.sleep(rSpinDelay)
            break
        right_dir.on()
        left_dir.off()
        left_pwm.duty_u16(speed)   #reset
        right_pwm.duty_u16(speed)
    right_dir.off()
    left_dir.off()
    left_pwm.duty_u16(0)   #reset
    right_pwm.duty_u16(0)

def spinLeft():
    while True:
        (l,f,r) = readWalls()
        if f < SpinRightThresh: #"and  r < rightThresh"
            time.sleep(rSpinDelay)
            break
        right_dir.off()
        left_dir.on()
        left_pwm.duty_u16(speed)   #reset
        right_pwm.duty_u16(speed)
    stop()

def stop():
    right_dir.off()
    left_dir.off()
    left_pwm.duty_u16(0)   #reset
    right_pwm.duty_u16(0)
        

def readWalls():
    tst.value(0)
    time.sleep(0.005)
    LMin = Lsensor.read_u16()
    MMin = Msensor.read_u16()
    RMin = Rsensor.read_u16()

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

    tst.value(0)
    Lside = LMax - LMin
    Mside = MMax - MMin
    Rside = RMax - RMin
    return Lside, Mside, Rside
#main loop
while True:
    (leftThresh,frontThresh,rightThresh)=readWalls()
    print(leftThresh,frontThresh,rightThresh)
    frontThresh = FT
    time.sleep(0.1)
    if button.value() == 1:
        break
print("BP1")
while button.value() == 1:
    pass
print("BR1")
while button.value() == 0:
    pass
print("BP2")
oldL = leftThresh
while Rcount < 400:
    (l,f,r) = readWalls()
    
    if f > frontThresh:
        if l > (leftThresh - 200):
            spinRight()
        else:
            spinLeft()
        Rcount += 1
        (l,f,r) = readWalls()
    sDiff = l - leftThresh
    sDiff = sDiff*mult
    lSpeed = max(speed+sDiff,0)
    lSpeed = min(lSpeed,65000)
    rSpeed = max(speed-sDiff,0)
    rSpeed = min(rSpeed,65000)
    (l,f,r) = readWalls()
    left_pwm.duty_u16(int(lSpeed))   #reset
    right_pwm.duty_u16(int(rSpeed))
    oldL = l
    
"""#if (l < spinLeftThresh):
     #   led_onboard.on()
        #stop()#
        left_pwm.duty_u16(speed)   #reset
        right_pwm.duty_u16(speed)
        time.sleep(forwardTime)
        left_pwm.duty_u16(int(speed*lMotorTurnMult))   #reset
        right_pwm.duty_u16(int(speed*RMotorLTurnMult))
        while l < leftThresh:
            (l,f,r) = readWalls()
            pass"""
        
    
    
    
    
    
left_pwm.duty_u16(0)   #reset
right_pwm.duty_u16(0)