import display
import color
import math
import buttons
import simple_menu
import utime
import urandom
import leds
#import light_sensor
import bhi160
bhi = bhi160.BHI160Orientation(sample_rate=30, sample_buffer_len=60)

display_width = 160
display_height = 80
number_of_modes = 7
mode = 0

disp = display.open()
disp.clear().update()
ground = []
leds.dim_top(1)

STATUS_COLORS = [
    color.RED,
    # Orange
    color.RED * 0.5 + color.YELLOW * 0.5,
    color.YELLOW,
    color.GREEN,
]


def gravity_sketch():
    global step, x, y
    leds.set(10-(mode % number_of_modes), color.BLUE)
    samples = bhi.read()
    if len(samples) == 0:
        utime.sleep(0.25)
        return
    ox, oy, oz, status = samples[-1]
# Orientation sensor certainty indicator
#    leds.dim_bottom(1)
#    leds.set(11, STATUS_COLORS[status])
#    leds.set(12, STATUS_COLORS[status])
    x = round(x-oz/45)
    y = round(y-oy/45)
    disp.pixel( x%160, y%80, col = color.from_hsv((step*2),1,1) )
    x = round(x-oz/45)
    y = round(y-oy/45)
    disp.pixel( x%160, y%80, col = color.from_hsv((step*2),1,1) )
    disp.update()
    step += 1


def gravity_sketch_mirrored():
    global step, x, y
    leds.set(10-(mode % number_of_modes), color.BLUE)
    samples = bhi.read()
    if len(samples) == 0:
        utime.sleep(0.25)
        return
    ox, oy, oz, status = samples[-1]
    x = round(x-oz/45)
    y = round(y-oy/45)
    disp.pixel( x%160, y%80, col = color.from_hsv((step*2),1,1) )
    disp.pixel( 159-x%160, y%80, col = color.from_hsv((step*2),1,1) )
    disp.pixel( x%160, 79-y%80, col = color.from_hsv((step*2),1,1) )
    disp.pixel( 159-x%160, 79-y%80, col = color.from_hsv((step*2),1,1) )
    x = round(x-oz/45)
    y = round(y-oy/45)
    disp.pixel( x%160, y%80, col = color.from_hsv((step*2),1,1) )
    disp.pixel( 159-x%160, y%80, col = color.from_hsv((step*2),1,1) )
    disp.pixel( x%160, 79-y%80, col = color.from_hsv((step*2),1,1) )
    disp.pixel( 159-x%160, 79-y%80, col = color.from_hsv((step*2),1,1) )
    disp.update()
    step += 1


def gravity_wave_1():
    global offset
    leds.set(10-(mode % number_of_modes), color.BLUE)
    disp.clear()
    samples = bhi.read()
    if len(samples) == 0:
        utime.sleep(0.25)
        return
    ox, oy, oz, status = samples[-1]
    for x in range( 0, display_width ):
        y = round(math.sin(((x+offset/2)*oz/180))*40*(oy/90)+40)
        disp.pixel( x, y, col = color.from_hsv((offset/5),1,1) )
    disp.update()
    offset += 10


def gravity_wave_2():
    global offset
#    disp.clear()
    leds.set(10-(mode % number_of_modes), color.BLUE)
    samples = bhi.read()
    if len(samples) == 0:
        utime.sleep(0.25)
        return
    ox, oy, oz, status = samples[-1]
    for x in range( 0, display_width ):
        y = round(math.sin(((x+offset/2)*oz/180))*40*(oy/90)+40)
        disp.pixel( x, y, col = color.from_hsv((offset/5),1,1) )
    disp.update()
    offset += 10


def wave_1():
    global offset
    leds.set(10-(mode % number_of_modes), color.RED)
    light = 1#(light_sensor.read()/20)
    for x in range( 0, display_width ):
        y = int(math.sin((x/20)*(math.cos(offset/4))+offset)*light*40+40)
        disp.pixel( x, y, col = color.from_hsv((offset*18),1,1) )
    disp.update()
    offset += .1


def wave_2():
    global offset
    leds.set(10-(mode % number_of_modes), color.RED)
    for x in range( 0, display_width ):
        y = int(math.sin((x/20)*(math.cos(offset/4))+offset)*40*math.tan(offset)+40)
        disp.pixel( x, y, col = color.from_hsv((offset*72)%360,1,1) )
    disp.update()
    offset += .05


def gravity_wave_3():
    global offset
    leds.set(10-(mode % number_of_modes), color.BLUE)
    samples = bhi.read()
    if len(samples) == 0:
        utime.sleep(0.25)
        return
    ox, oy, oz, status = samples[-1]
    for x in range( 0, display_width ):
        y = round(math.sin((x/20)*(oz/180)*(math.cos(offset/4))+offset)*(oy/90)*40*math.tan(offset)+40)
        disp.pixel( x, y, col = color.from_hsv((offset*72)%360,1,1) )
    disp.update()
    offset += .05


# Unused
def square_spiral():
    step = 0
    x, y = 0, 0
    shorten = 0
    leds.set(10-(mode % number_of_modes), color.BLUE)
    while x < 159:
        disp.pixel( x, y, col = color.from_hsv((step*2),1,1) )
        x += 1
        disp.update()
        step += 1
    while shorten <= 80:
        while y < 79 - shorten:
            disp.pixel( x, y, col = color.from_hsv((step*2),1,1) )
            y += 1
            step += 1
        disp.update()
        while x > 0 + shorten:
            disp.pixel( x, y, col = color.from_hsv((step*2),1,1) )
            x -= 1
            step += 1
        disp.update()
        shorten += 1
        while y > 0 + shorten:
            disp.pixel( x, y, col = color.from_hsv((step*2),1,1) )
            y -= 1
            step += 1
        disp.update()
        while x < 159 - shorten:
            disp.pixel( x, y, col = color.from_hsv((step*2),1,1) )
            x += 1
            step += 1
        disp.update()


def push(func_name):
    global mode, offset, step, x, y
    disp.clear()
    offset = urandom.randint(0,1800)
    step = 0
    x, y = 80, 40
    pause = False
    for ev in simple_menu.button_events(timeout=0.05):
        if ev == buttons.BOTTOM_RIGHT:
            mode += 1
            leds.clear()
            return
        elif ev == buttons.BOTTOM_LEFT:
            pause = not pause
        elif ev == buttons.TOP_RIGHT:
            return
        else:
            if not pause:
                func_name()
            else:
                leds.set(10-(mode % number_of_modes), color.BLUE)
                leds.clear()
                utime.sleep(.5)


def modeselektor():
    while True:
        if mode % number_of_modes == 0:
            push(gravity_sketch)
        if mode % number_of_modes == 1:
            push(gravity_sketch_mirrored)
        if mode % number_of_modes == 2:
            push(gravity_wave_1)
        if mode % number_of_modes == 3:
            push(gravity_wave_2)
        if mode % number_of_modes == 4:
            push(gravity_wave_3)
        if mode % number_of_modes == 5:
            push(wave_1)
        if mode % number_of_modes == 6:
            push(wave_2)


modeselektor()