import math
import time
from hardware import Hardware
from robot import Robot
from gait import Creep
import samd

samd.disable_autoreload()

robot = Robot(Hardware())
robot.unfold()
gait = Creep(robot)
gait.dx = 0

dr = math.pi / 80
while 1:
    try:
        distance = robot.hardware.sensor()
        if distance == 0:
            robot.hardware.eye_color(4)
            gait.dx = 0
            i = 10
            j = 0
            while distance == 0:
                robot.rotate_by(dr)
                i += 1
                if i > 20:
                    i = 0
                    dr = -dr
                    j += 1
                    if j > 4:
                        j = 0
                        robot.hardware.eye_color(0)
                        robot.fold()
                        while distance == 0:
                            time.sleep(1)
                            distance = robot.hardware.sensor()
                        robot.unfold()
                time.sleep(0.2)
                distance = robot.hardware.sensor()
        elif distance < 200:
            robot.hardware.eye_color(1)
            gait.dx = -1
        elif distance < 250:
            robot.hardware.eye_color(5)
            gait.dx = 0
            time.sleep(1)
            distance2 = robot.hardware.sensor()
            if abs(distance - distance2) < 5:
                robot.rotate_by(dr)
                time.sleep(0.1)
                distance = robot.hardware.sensor()
                robot.rotate_by(-dr)
                time.sleep(0.1)
                robot.rotate_by(-dr)
                time.sleep(0.1)
                distance2 = robot.hardware.sensor()
                robot.rotate_by(dr)
                time.sleep(0.1)
                if distance2 > distance:
                    dr = -dr
                robot.hardware.eye_color(6)
                gait.rotation = dr
                gait.run(10)
                gait.rotation = 0
            else:
                distance = distance2
        else:
            robot.hardware.eye_color(3)
            gait.dx = 1
        gait.run(1)
    except ValueError:
        robot.home()