import math
import os

import bhi160
import buttons
import display
import utime

WIDTH = 159
HEIGHT = 79
CENTER_X = WIDTH // 2
CENTER_Y = HEIGHT // 2
LINE_HEIGHT = 20
LEN = min(WIDTH, HEIGHT) // 2
RED = (255, 100, 100)
GREEN = (100, 255, 100)
BLUE = (0, 0, 255)


def update(sensor, disp):
    vecs = sensor.read()

    if not (vecs):
        return

    vec = vecs[-1]
    azimut = vec.x * math.pi * 2 / 360
    width = int(LEN * math.sin(azimut))
    height = int(LEN * math.cos(azimut))
    disp.clear()
    disp.print("N", posx=0, posy=0, fg=RED)
    disp.print("S", posx=0, posy=LINE_HEIGHT, fg=GREEN)
    disp.print("exit>", posx=WIDTH - 70, posy=HEIGHT - LINE_HEIGHT, bg=BLUE)
    disp.line(CENTER_X, CENTER_Y, CENTER_X - width, CENTER_Y - height, col=RED)
    disp.line(CENTER_X, CENTER_Y, CENTER_X + width, CENTER_Y + height, col=GREEN)
    disp.update()


def main():
    with bhi160.BHI160Orientation() as sensor:
        with display.open() as disp:
            while True:
                update(sensor, disp)
                utime.sleep_ms(100)
                pressed = buttons.read(buttons.BOTTOM_RIGHT)

                if pressed:
                    os.exit()


if __name__ == "__main__":
    main()