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

# Implementation of Basic operations and misc. ##############################################

# await mp.move_for_degrees(mp.PAIR_1, 360, 45, velocity=280)

# motor.run_for_degrees(portNum, 200, 300)
# await runloop.sleep_ms(2000)

# mp.move_tank(mp.PAIR_1, 280, -280)
# await runloop.sleep_ms(2000)

# WANTED_COLOR = color.BLACK

# Productive Code ###########################################################################

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(30)
    await turn_left(85)
    await drive_cm(15)
    await turn_right(85)
old = 0
async def move_on_color():
    won = False
    old = time.ticks_ms()
    speed = 100

    while not won:

        # Farbcodes lesen
        left_color = cs.color(port.B)
        right_color = cs.color(port.F)
        
        gradient = ms.tilt_angles()[2]
        # Farbcodes
        BLACK = 0
        GREEN = 6
        WHITE = 10
        RED = 9
        straight = False
        turn_way_if_lost_right = True
        # Distanzsensor
        ds_front = ds.distance(port.D)

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

        # --- LOGIK NUR MIT FARBEN ---
                # Finish line logik
        if left_color == RED or right_color == RED:
            won = True
            break

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

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

        # Linker Sensor sieht Grün → Linksabbiegen
        elif left_color == GREEN and right_color != GREEN:
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(700)
            mp.move_tank(mp.PAIR_1, -speed, speed)
            time.sleep_ms(1050)
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(300)

        # Rechter Sensor sieht Grün → Rechtsabbiegen
        elif right_color == GREEN and left_color != GREEN:
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(700)
            mp.move_tank(mp.PAIR_1, speed, -speed)
            time.sleep_ms(1050)
            mp.move_tank(mp.PAIR_1, speed, speed)
            time.sleep_ms(300)

        elif right_color == BLACK or left_color == BLACK:
            if right_color == BLACK and left_color == BLACK:
                mp.move_tank(mp.PAIR_1, speed, speed)
        # Linker Sensor ist Schwarz, rechter Weiß → sanft nach links
            elif left_color == BLACK and right_color == WHITE:
                mp.move_tank(mp.PAIR_1, speed-100, speed)

        # Rechter Schwarz, linker Weiß → sanft nach rechts
            elif right_color == BLACK and left_color == WHITE:
                mp.move_tank(mp.PAIR_1, speed, speed-100)  

        # Beide Weiß → geradeaus (vermutlich neben der Linie)
        elif left_color == WHITE and right_color == WHITE:
            straight = True
            mp.move_tank(mp.PAIR_1, speed, speed)
            runloop.sleep_ms(50)

        if straight:
            if time.ticks_diff(time.ticks_ms(), old) > 2750:
                old = time.ticks_ms()
                if turn_way_if_lost_right:
                    mp.move_tank(mp.PAIR_1, speed, 0)
                    await runloop.sleep_ms(200)
                    turn_way_if_lost_right = not turn_way_if_lost_right
                else:
                    mp.move_tank(mp.PAIR_1, 0, speed)
                    await runloop.sleep_ms(200)
                    turn_way_if_lost_right = not turn_way_if_lost_right

        else:
            old = time.ticks_ms()

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

runloop.run(main())