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
from pybricks.hubs import PrimeHub

config = {
    "PyBrick Hub" : {
        "remote" : "blue",
        "color" : Color.BLUE
    },
    "Hub Rot" : {
        "remote" : "red",
        "color" : Color.RED
    }
}

hub = PrimeHub()
hub_name = hub.system.name()
hub_config = config[hub_name]
print("Hub config: ", hub_config)
rc = Remote(timeout=5000, name=hub_config["remote"])
rc.light.on(hub_config["color"])

shoot = None
try:
    shoot = Motor(Port.F)
    print("found shoot motor")
    # erhöhe Beschleunigung des Shoot Motors von 2000 auf 8000
    shoot.control.limits(1000, 8000, 199)
    print("Shoot limits (speed, accel, torque): " + str(shoot.control.limits()))
    shoot.control.limits
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=30
            speed=1000
            shoot.run_angle(speed, angle)
            # move back
            shoot.run_angle(-speed, angle)
            #shoot.run(speed)

        # run fast
        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()