import bhi160
import display
import utime
import buttons
import math

sensor = bhi160.BHI160Orientation()

center = (80, 40)
radius = 30

plot_red_dir = False
dir = 0
plot_green_dir = False

p = lambda a: a % 360
circ_dist = lambda a, b: min(p(p(a) + p(-b)), p(p(-a) + p(b)))

with display.open() as disp:
    while True:
        samples = sensor.read()
        if len(samples) > 0:
            disp.clear()
            for ii in range(30):
                disp.pixel(
                        int(center[0] + radius * math.cos(math.radians(12 * ii))),
                        int(center[1] + radius * math.sin(math.radians(12 * ii))),
                        col=(100, 100, 100))

            sample = samples[0]
            if plot_red_dir:
                angle = math.radians(-sample.x - dir) - math.pi / 2
                disp.circ(
                        int(center[0] + radius * math.cos(angle)),
                        int(center[1] + radius * math.sin(angle)),
                        3,
                        col=(255, 0, 0))
            if plot_green_dir:
                if circ_dist(-sample.x, dir) < 30:
                    angle = math.radians(3 * (-sample.x - dir)) - math.pi / 2
                    disp.circ(
                            int(center[0] + radius * math.cos(angle)),
                            int(center[1] + radius * math.sin(angle)),
                            2,
                            col=(0, 255, 0))
            disp.line(
                    int(center[0]), int(center[1] - 0.8 * radius),
                    int(center[0]), int(center[1] - 1.2 * radius),
                    col=(100, 100, 100),
                    size=1)

            disp.update()

        # Read button
        button_pressed = False
        v = buttons.read(buttons.BOTTOM_LEFT | buttons.BOTTOM_RIGHT)
        if v == 0:
            button_pressed = False

        if not button_pressed and v & buttons.BOTTOM_RIGHT != 0:
            button_pressed = True
            plot_red_dir = True
            if len(samples) > 0:
                dir = -samples[0].x

        elif not button_pressed and v & buttons.BOTTOM_LEFT != 0:
            button_pressed = True
            plot_green_dir = not plot_green_dir
            utime.sleep(0.1)

        utime.sleep(0.05)