import time
from telemetrix_rpi_pico import telemetrix_rpi_pico
import kuukar.kuukar_leds as kuukar_leds
import kuukar.kuukar_collision as kuukar_collision
import kuukar.kuukar_motion as kuukar_motion
import kuukar.kuukar_lcd as kuukar_lcd
import kuukar.kuukar_sensors as kuukar_sensors
import kuukar.kuukar_environment as kuukar_environment
from flask import Flask, request

from kuukar.kuukar_config import SERIAL_MCU, SERIAL_DRV

app = Flask(__name__)

mcu = telemetrix_rpi_pico.TelemetrixRpiPico(com_port=SERIAL_MCU)
drv = telemetrix_rpi_pico.TelemetrixRpiPico(com_port=SERIAL_DRV)
sensors = kuukar_sensors.sensors(mcu)
leds = kuukar_leds.leds(mcu)
motion = kuukar_motion.motion(mcu, drv, leds, sensors)
lcd = kuukar_lcd.lcd(motion, sensors)
environment = kuukar_environment.environment(lcd, leds, sensors)
collision = kuukar_collision.collision(mcu, lcd, leds)
motion.turn(30,3000)

while True:
    temp = sensors.get_temperature()
    humid = sensors.get_humidity_pct()
    dist1 = sensors.sonar_get_distance(0)
    dist2 = sensors.sonar_get_distance(1)
    dist3 = sensors.sonar_get_distance(2)
    print(f'temp: {temp}, humidity: {humid}, dist1: {dist1}, dist2: {dist2}, dist3: {dist3}')
    time.sleep(1)

@app.route('/drive')
def drive():
    speed = int(request.args.get('speed'))
    motion.drive(speed)


@app.route("/stop")
def stop():
    motion.stop()


@app.route('/turn')
def turn():
    speed = int(request.args.get('speed'))
    duration = int(request.args.get('duration'))
    motion.turn(speed, duration)


@app.route('/set_ambient_leds')
def set_ambient_leds():
    r = int(request.args.get('r'))
    g = int(request.args.get('g'))
    b = int(request.args.get('b'))
    leds.set_ambient_led(r, g, b)


@app.route('/set_mode')
def set_mode():
    mode = str(request.args.get('mode'))
    if mode == "AI":
        motion.roam_start()
    elif mode == "MANUAL":
        motion.roam_stop()