from hub import port, motion_sensor as ms
import motor_pair as mp
import distance_sensor as ds
import color_sensor as cs
import sys
import runloop
import time

BLACK = 0
GREEN = 6
WHITE = 10
RED = 9

async def turn_right(angle=85):
    mp.move_tank(mp.PAIR_1, 200, -200)
    await runloop.sleep_ms(int(angle * 7))# 6 ms per degree → tune this!
    mp.stop(mp.PAIR_1)

async def turn_left(angle=85):
    mp.move_tank(mp.PAIR_1, -200, 200)
    await runloop.sleep_ms(int(angle * 7))
    mp.stop(mp.PAIR_1)

# drive forward X centimeters
async def drive_cm(cm, speed=200):
    # convert cm → ms (you must tune this constant!)
    mp.move_tank(mp.PAIR_1, -speed, -speed)
    await runloop.sleep_ms(cm * 70)
    mp.stop(mp.PAIR_1)

async def avoid_obstacle(speed=100):
    mp.stop(mp.PAIR_1)
    await runloop.sleep_ms(500)
    mp.move_tank(mp.PAIR_1, -speed, -speed)

    await turn_right(85)
    await drive_cm(15)
    await turn_left(85)
    await drive_cm(38)
    await turn_left(85)
    while cs.reflection(port.C) > 25:
        mp.move_tank(mp.PAIR_1, -speed, -speed)
    while cs.color(port.B) != BLACK:
        mp.move_tank(mp.PAIR_1, 10, -110)

async def move_on_color():
    won = False
    speed = 100
    saw_green_left = False
    saw_green_right = False
    last_seen_black = ""

    old = time.ticks_ms()
    while not won:
        

        # Farbcodes lesen
        left_color = cs.color(port.B)
        right_color = cs.color(port.F)
        front_sens = cs.reflection(port.C)

        gradient = ms.tilt_angles()[2]
        # Farbcodes
        BLACK = 0
        GREEN = 6
        WHITE = 10
        RED = 9

        # Distanzsensor
        ds_front = ds.distance(port.D)

        if gradient < -120:
            speed = -120
        elif gradient > 50:
            speed = -50
        else:
            speed = -100

        # --- LOGIK NUR MIT FARBEN ---
        if left_color == RED or right_color == RED:
            won = True


        elif ds_front < 75 and ds_front > -1:
            await avoid_obstacle()

        # Linker Sensor sieht Grün → Linksabbiegen
        elif left_color == GREEN and right_color != GREEN:
            saw_green_left = True
            old = time.ticks_ms()


        elif right_color == GREEN and left_color != GREEN:
            saw_green_right = True
            old = time.ticks_ms()

        # Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung)
        elif left_color == GREEN and right_color == GREEN:
            saw_green_right = False
            saw_green_left = False
            mp.move_tank(mp.PAIR_1, -speed, speed)
            time.sleep_ms(2300)

        elif saw_green_left and left_color == BLACK:
            saw_green_left = False
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(1200)
            mp.move_tank(mp.PAIR_1, speed, -speed)
            time.sleep_ms(1000)
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(300)

        # Rechter Sensor sieht Grün → Rechtsabbiegen
        elif saw_green_right and right_color == BLACK:
            saw_green_right = False
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(1200)
            mp.move_tank(mp.PAIR_1, -speed, speed)
            time.sleep_ms(1000)
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(300)

        # Schwarz-Logik
        elif front_sens > 25 and last_seen_black == "left":
            while cs.reflection(port.C) > 25: # front_sensor
                if cs.color(port.B) == BLACK and cs.color(port.F) == WHITE: # left_color
                    last_seen_black = "left"
                    
                if cs.color(port.F) == BLACK and cs.color(port.B) == WHITE: # right_color
                    last_seen_black = "right"
                    break
                old = time.ticks_ms()
                mp.move_tank(mp.PAIR_1, -125, -20)


        elif front_sens > 25 and last_seen_black == "right":
            while cs.reflection(port.C) > 25:
                if cs.color(port.F) == BLACK and cs.color(port.B) == WHITE:
                    last_seen_black = "right"

                if cs.color(port.B) == BLACK and cs.color(port.F) == WHITE:
                    last_seen_black = "left"
                    break
                old = time.ticks_ms()
                mp.move_tank(mp.PAIR_1, -20, -125)

        elif right_color == BLACK or left_color == BLACK:
            old = time.ticks_ms()
            if right_color == BLACK and left_color == BLACK:
                mp.move_tank(mp.PAIR_1, speed, speed)
                time.sleep_ms(20)
                

            # Linker Sensor ist Schwarz, rechter Weiß → sanft nach links
            elif left_color == BLACK and right_color == WHITE:
                last_seen_black = "left"

            # Rechter Schwarz, linker Weiß → sanft nach rechts
            elif right_color == BLACK and left_color == WHITE:
                last_seen_black = "right"

        # Beide Weiß, Front Schwarz → geradeaus
        elif cs.color(port.B) == WHITE and cs.color(port.F) == WHITE and front_sens < 25:
            if time.ticks_diff(time.ticks_ms(), old) > 1000:
                last_seen_black = ""
                old = time.ticks_ms()
            mp.move_tank(mp.PAIR_1, speed, speed)
            runloop.sleep_ms(50)

async def main():
    mp.pair(mp.PAIR_1, port.A, port.E)
    await move_on_color()

    sys.exit()

runloop.run(main())