#!/usr/bin/env pybricks-micropython

#Team Members:
#Matthew Zheng, 730092528
#Hyrum Draughon, 730089364

from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
                                 SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from math import pi

def findSpeed(distance, time):
    """
    distance: distance in meters to travel
    time: travel time in seconds
    returns speed in degress/second
    """
    wheelCircumference = DIAMETER*pi
    numRotations = (distance*100)/wheelCircumference
    degrees = numRotations*360
    rotSpeed = degrees/time
    return rotSpeed

left = Motor(Port.B)
right = Motor(Port.C)

touch = TouchSensor(Port.S3)
ultra = UltrasonicSensor(Port.S4)

DIAMETER = 5.3975 #centimeters

while not 32 in brick.buttons():
    wait(10)
#Move forward 60-120 cm to wall

seconds = 10
speed = findSpeed(1.289, seconds)

left.run(speed)
right.run(speed)

while (not touch.pressed()):
    continue

left.stop(Stop.BRAKE)
right.stop(Stop.BRAKE)

#Back up and turn right, go forward

#back up and turn right

#Keep running and stay within 30cm, until past the wall

speed2 = findSpeed(3, 30) #Estimate 30 seconds to move at most 3 meters

def runOnWall():
    right.run(speed2)
    left.run(speed2)

    while ((ultra.distance() < 20) and (not touch.pressed())):
        continue
    if ((ultra.distance() >= 20) and (ultra.distance() <= 30)):
        #stop and turn left
    elif (touch.pressed()):
        #stop and turn right (45 degrees)

    if (ultra.distance() > 30):
        #stop and turn left to original orientation (about 90 degrees)
    else:
        runOnWall()

runOnWall()

#Turn left and go straight 0.7 meters
seconds
speed3 = findSpeed(0.7, seconds)

left.run_time(speed3, seconds*1000, Stop.COAST, False)
right.run_time(speed3, seconds*1000, Stop.COAST, True)