from hub import port, motion_sensor as ms
import motor_pair as mp
import distance_sensor as ds
import color_sensor as cs
import color
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=90):
    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=90):
    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():
    mp.stop(mp.PAIR_1)
    await runloop.sleep_ms(500)
    mp.move_tank(mp.PAIR_1, -100, -100)

    await turn_right(90)
    await drive_cm(20)
    await turn_left(90)
    await drive_cm(40)
    await turn_left(90)
    await drive_cm(20)
    await turn_right(90)

async def move_on_color():
    while True:

        # Farbcodes lesen
        left_color = cs.color(port.C)
        right_color = cs.color(port.E)
        gradient = ms.tilt_angles()[2]
        # Farbcodes
        BLACK = 0
        GREEN = 6
        WHITE = 10

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

        # --- LOGIK NUR MIT FARBEN ---
        # Beide Sensoren sehen Grün (z.B. Kreuzung / Markierung)
        if ds_front < 75:
            await avoid_obstacle()

        elif left_color == GREEN and right_color == GREEN:
            mp.move_tank(mp.PAIR_1, -100, 100)
            time.sleep_ms(2300)

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

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

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

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

        # Beide Weiß → geradeaus (vermutlich neben der Linie)
        elif left_color == WHITE and right_color == WHITE:
            mp.move(mp.PAIR_1, 0, velocity=150)

        # TODO: ---- Weiß auf beiden Seiten = Linie verloren ----
        # wenn links == weiß UND rechts == weiß:
        #    langsam drehen bis schwarz gefunden

        elif gradient < -150:
            mp.move(mp.PAIR_1, 0, velocity=350)

        elif gradient > 50:
            mp.move(mp.PAIR_1, 0, velocity=100)   

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

    sys.exit()

runloop.run(main())