from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor, Remote
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

rc = Remote(timeout=5000)
print("Remote name: ", rc.name)
rc.light.on(Color.RED)

shoot = None
try:
    shoot = Motor(Port.F)
    print("found shoot motor")
except:
    print("no shoot motor")

right = Motor(Port.B)
left = Motor(Port.A, Direction.COUNTERCLOCKWISE)
db = DriveBase(left, right, 55, 80) # DriveBase steuert beide Motoren gemeinsam
s_speed, s_accel, t_rate, t_accel = db.settings()
db.settings(s_speed*0.5, s_accel, t_rate, t_accel*2)

while True:
    # Check which buttons are pressed.
    db.settings(s_speed*0.5, s_accel, t_rate, t_accel*2)
    pressed = rc.buttons.pressed()
    #print(str(pressed))
    speed=0
    angle=0
     # Check a specific button.
    if Button.RIGHT_PLUS in pressed:
        speed=1000
        angle+=180
        #db.drive(1000, 180)
        #right.run(1000)
    if Button.LEFT_PLUS in pressed:
        speed=1000
        angle-=180
        #db.drive(1000, -180)
        #left.run(1000)
    if Button.RIGHT_MINUS in pressed:
        speed=-1000
        angle+=180
        #db.drive(-1000, 180)
        #right.run(-1000)
    if Button.LEFT_MINUS in pressed:
        speed=-1000
        angle-=180
        #db.drive(-1000, -180)
        #left.run(-1000)

    if Button.CENTER in pressed:
        if shoot:
            # move forward to shoot
            angle=45
            speed=1000
            shoot.run_angle(-speed, angle)
            # move back
            shoot.run_angle(speed, angle)
        else:
            db.settings(s_speed*2, s_accel, t_rate, t_accel*2)
            speed=-1000
            angle=0

    if Button.LEFT_PLUS in pressed and Button.RIGHT_MINUS in pressed:
        db.turn(-20,wait=False,)
    elif Button.LEFT_MINUS in pressed and Button.RIGHT_PLUS in pressed:
        db.turn(20,wait=False)
    else:
        db.drive(speed, angle)

     # Wait so we can see the result.
    # wait(50)
    if speed == 0 :
        db.stop()