##
# @file    main.py
# @brief   Example of a vehicle dashboard
#

import gui
from math import cos, sin, radians
cosd = lambda x: cos(radians(x))
sind = lambda x: sin(radians(x))
lerp = lambda x, xi, xf, ai, af: ai + ((x - xi) * (af - ai) / (xf - xi))

class Filter:
    def __init__(self):
        # 2nd order IIR Gauss LP filter, 2.1 Hz BW, 3 Hz wc, fs = 60 Hz
        self.a = (1.0000000000, -1.4950558121,  0.5668457092)
        self.b = (0.0179474742,  0.0358949485,  0.0179474742)
        # Filter state
        self.w = [0, 0]

    def __call__(self, x):
        # Biquad transposed direct form 2
        w = x - self.a[1] * self.w[0] - self.a[2] * self.w[1]
        y = self.b[0] * w + self.b[1] * self.w[0] + self.b[2] * self.w[1]
        self.w[1] = self.w[0]
        self.w[0] = w
        return y

class Gauge:
    def __init__(self, x, y, d,
            vmin=0,
            vmax=100,
            dmin=180,
            dmax=45,
            ndivs=10,
            vred=None,
            color=gui.RGB(0, 0, 0),
            color_red=gui.RGB(246, 40, 23)):
        self.x = x
        self.y = y
        # d: 'characteristic dimension'
        self.d = d
        self.vmin = vmin
        self.vmax = vmax
        self.dmin = dmin
        self.dmax = dmax
        self.ndivs = ndivs
        self.vred = vred
        self.color = color
        self.color_red = color_red
        self.filter = Filter()
        self.gui_needle = [[0], [0], [0]]
        self.value = vmin
        self.update()

    def set_val(self, x):
        self.value = x
        self.update()

    def update(self):
        self.cur_val = self.filter(self.value)
        angle = lerp(self.cur_val, self.vmin, self.vmax, self.dmin, self.dmax)
        self.gui_needle[0][0] = gui.DL_VERTEX2F(
            self.x + .9 * self.d * cosd(angle),
            self.y - .9 * self.d * sind(angle)
        )
        self.gui_needle[1][0] = gui.DL_VERTEX2F(
            self.x + .05 * self.d * cosd(angle + 90),
            self.y - .05 * self.d * sind(angle + 90)
        )
        self.gui_needle[2][0] = gui.DL_VERTEX2F(
            self.x + .05 * self.d * cosd(angle - 90),
            self.y - .05 * self.d * sind(angle - 90)
        )

    def gui_l(self):
        ln = [gui.SUBLIST]
        # Draw numbers
        fontsize = int(lerp(self.d, 50, 250, 26, 32))
        if fontsize < 26:
            fontsize = 26
        elif fontsize > 31:
            fontsize = 31
        for i in range(self.ndivs + 1):
            angle = lerp(i, 0, self.ndivs, self.dmin, self.dmax)
            numval = int(lerp(i, 0, self.ndivs, self.vmin, self.vmax))
            if self.vred and numval >= self.vred:
                ln.append([gui.DL_COLOR(self.color_red)])
            ln.append([
                gui.CTRL_TEXT,
                int(self.x + .8 * self.d * cosd(angle)),
                int(self.y - .8 * self.d * sind(angle)),
                fontsize,
                gui.OPT_CENTER,
                str(numval)
            ])
        if self.vred:
            ln.append([gui.DL_COLOR(self.color)])
        # Draw ticks
        l = [gui.DL_SAVE_CONTEXT()]
        l.append(gui.DL_BEGIN(gui.PRIM_LINES))
        for i in range(self.ndivs + 1):
            angle = lerp(i, 0, self.ndivs, self.dmin, self.dmax)
            if i != 0:
                # Minor ticks
                angle_prev = lerp(i - 1, 0, self.ndivs, self.dmin, self.dmax)
                for j in range(1, 10):
                    angle_minor = lerp(j, 0, 10, angle_prev, angle)
                    if self.vred:
                        tickval = lerp(
                            angle_minor, self.dmin, self.dmax, self.vmin, self.vmax)
                        if tickval >= self.vred:
                            l.append(gui.DL_COLOR(self.color_red))
                    l.append(gui.DL_VERTEX2F(
                        self.x + self.d * cosd(angle_minor),
                        self.y - self.d * sind(angle_minor)
                    ))
                    f = .99 if j != 5 else .97
                    l.append(gui.DL_VERTEX2F(
                        self.x + f * self.d * cosd(angle_minor),
                        self.y - f * self.d * sind(angle_minor)
                    ))
            # Major ticks
            l.append(gui.DL_VERTEX2F(
                self.x + self.d * cosd(angle),
                self.y - self.d * sind(angle)
            ))
            l.append(gui.DL_VERTEX2F(
                self.x + .92 * self.d * cosd(angle),
                self.y - .92 * self.d * sind(angle)
            ))
        l.append(gui.DL_END())
        if self.vred:
            l.append(gui.DL_COLOR(self.color))
        # Draw needle
        import ft8xx as ft
        # Filled polygon
        l.extend([
            gui.DL_STENCIL_OP(ft.STENCILOP_INCR, ft.STENCILOP_INCR),
            gui.DL_COLOR_MASK(0, 0, 0, 0),
            gui.DL_BEGIN(gui.PRIM_EDGE_STRIP_B),
            self.gui_needle[0],
            self.gui_needle[1],
            self.gui_needle[2],
            self.gui_needle[0],
            gui.DL_END(),
            gui.DL_COLOR_MASK(1, 1, 1, 1),
            gui.DL_STENCIL_FUNC(ft.ALPHAFUNC_EQUAL, 1, 255),
            gui.DL_BEGIN(gui.PRIM_EDGE_STRIP_B),
            self.gui_needle[0],
            self.gui_needle[1],
            self.gui_needle[2],
            self.gui_needle[0],
            gui.DL_END(),
            gui.DL_STENCIL_FUNC(ft.ALPHAFUNC_ALWAYS, 0, 255),
            gui.DL_CLEAR(0, 1, 0),
        ])
        # Outline
        l.extend([
            gui.DL_LINE_WIDTH(1.6),
            gui.DL_BEGIN(gui.PRIM_LINE_STRIP),
            self.gui_needle[0],
            self.gui_needle[1],
            self.gui_needle[2],
            self.gui_needle[0],
            gui.DL_END()
        ])
        # Draw center pivot
        l.extend([
            gui.DL_POINT_SIZE(self.d * .08),
            gui.DL_BEGIN(gui.PRIM_POINTS),
            gui.DL_VERTEX2F(self.x, self.y),
            gui.DL_END()
        ])
        l.append(gui.DL_RESTORE_CONTEXT())
        ln.append(l)
        return ln


import can
import gnss
import gui
import ustruct as us
import vts
import vbox

drift = [0]
led_brightness = [64 << 8]

def sample_cb():
    sample = vbox.get_sample()
    g_vel.set_val(sample.speed_gnd_mps * 3.6)

def can_cb():
    while True:
        msg = can.get_msg(can.CAN0)
        if msg is None:
            break
        elif msg.id == 0x411:
            # Steering angle + RPM
            rpm = us.unpack_from('>f', bytearray(msg.data), 4)[0] / 1000
            g_rpm.set_val(rpm)
        elif msg.id == 0x420:
            # Drift angle
            drift[0] = us.unpack('>f', bytearray(msg.data))[0]

def vsync_cb(l):
    def led_fade(x, center):
        fade_len = .05
        if x < center - fade_len:
            return 0
        elif x > center + fade_len:
            return led_brightness[0] >> 8
        else:
            return int((led_brightness[0] >> 8)
                * lerp(x, center - fade_len, center + fade_len, 0, 1))

    g_vel.update()
    g_rpm.update()
    # Shift lights
    rpm = g_rpm.cur_val
    leds = [0] * 12
    # LED 0: green
    leds[10] = led_fade(rpm, 4.5)
    # LED 1: yellowish green
    leds[6] = int(led_fade(rpm, 5) * .35)
    leds[7] = int(led_fade(rpm, 5) * .65)
    # LED 2: yellow
    leds[3] = leds[4] = led_fade(rpm, 5.5) // 2
    # LED 3: red
    leds[0] = led_fade(rpm, 6.5)
    vts.leds(*leds)
    gui.redraw()

can.test_mode(can.CAN0, 0)
# Listen on IDs 0x410, 0x411, 0x420, 0x421
can.add_rx_id(can.CAN0, 0x410, 0x7ce)
can.set_callback(can.CAN0, can_cb)
while (gnss.init_status() > 0):
    pass
vbox.init(vbox.VBOX_SRC_GNSS_BASIC)
vbox.set_new_data_callback(sample_cb)


g_rpm = Gauge(200, 335, 190, vmax=8, dmin=225, dmax=75, ndivs=8, vred=6.5)
g_vel = Gauge(520, 280, 270, vmax=220, dmin=225, dmax=-45, ndivs=11, vred=218)
gui.show([
    [gui.EVT_VSYNC, vsync_cb],
    [gui.PARAM_CLRCOLOR, gui.RGB(0, 0, 0)],
    [
        # Gradient
        0xffffff0b,
        0,
        gui.RGB(0, 0, 96),
        (480 << 16) | 200,
        gui.RGB(0, 0, 16),
        # End gradient
        gui.DL_COLOR_RGB(255, 255, 255),
        gui.DL_BEGIN(gui.PRIM_POINTS),
        gui.DL_POINT_SIZE(200),
        gui.DL_VERTEX2F(200, 335),
        gui.DL_POINT_SIZE(280),
        gui.DL_VERTEX2F(520, 280),
        gui.DL_END(),
        gui.DL_COLOR_RGB(0, 0, 0)
    ],
    [gui.PRIM_RECTS, [
        gui.DL_VERTEX2F(440, 350),
        gui.DL_VERTEX2F(600, 450),
    ]],
    [gui.DL_COLOR_RGB(255, 255, 255)],
    [gui.CTRL_TEXT, 520, 410, 30, gui.OPT_CENTERX, 'Drift'],
    [gui.CTRL_NUMBER, 580, 350, 32, gui.OPT_RIGHTX, '%.1f', drift],
    [gui.CTRL_TEXT, 585, 360, 26, 0, 'o'],
    [gui.DL_COLOR_RGB(0, 0, 0)],
    g_rpm.gui_l(),
    [gui.CTRL_TEXT, 200, 355, 28, gui.OPT_CENTERX, 'RPM'],
    [gui.CTRL_TEXT, 200, 375, 28, gui.OPT_CENTERX, 'x1000'],
    g_vel.gui_l(),
    [gui.CTRL_TEXT, 520, 305, 28, gui.OPT_CENTERX, 'km/h'],
    [gui.CTRL_SLIDER, 30, 20, 220, 20, 0xffff, led_brightness],
])
