import ft8xx as ft
import gui
import math
from screenshot import screenshot_cb
from picture_button import Picture_Button
from image import Image_Bank
from utils import Filter, Colors
import vts
import vbox
import vbo
import gnss
from ui import UI

class GReadings:
    def __init__(self):
        self.pk_lat = 0
        self.pk_acc = 0
        self.pk_dec = 0
        self.combo_g = 0
        self.filter = (Filter(), Filter())
    
    def draw(self):
        self.gui_l = [
            gui.SUBLIST,
            [gui.DL_COLOR_RGB(0, 128, 255)],
            [gui.CTRL_TEXT, 780, 65, 9, gui.OPT_RIGHTX,'{:+.2f}'.format(self.pk_lat)],
            [gui.DL_COLOR_RGB(0, 128, 0)],
            [gui.CTRL_TEXT, 780, 185, 9, gui.OPT_RIGHTX,'{:+.2f}'.format(self.pk_acc)],
            [gui.DL_COLOR_RGB(255, 0, 0)],
            [gui.CTRL_TEXT, 780, 305, 9, gui.OPT_RIGHTX,'{:+.2f}'.format(self.pk_dec)],
            [gui.DL_COLOR_RGB(0, 0, 0)],
            [gui.CTRL_TEXT, 150, 330, 8, gui.OPT_CENTERX,'{:.2f}G'.format(self.combo_g)],
        ]
        return self.gui_l

    def update(self):
        self.gui_l[2][5] = '{:+.2f}'.format(self.pk_lat)
        self.gui_l[4][5] = '{:+.2f}'.format(self.pk_acc)
        self.gui_l[6][5] = '{:+.2f}'.format(self.pk_dec)
        self.gui_l[8][5] = '{:.2f}G'.format(self.combo_g)

    def reset(self):
        self.pk_acc = 0
        self.pk_dec = 0
        self.pk_lat = 0
        self.combo_g = 0

    def update_vals(self,lat,lon):
        lat = self.filter[0](lat)
        lon = self.filter[1](lon)
        if lon > self.pk_acc:
            self.pk_acc = lon
        if lon < self.pk_dec:
            self.pk_dec = lon
        if abs(lat) > abs(self.pk_lat):
            self.pk_lat = lat
        self.combo_g = math.sqrt(lat**2 + lon**2)



class GDisplay:
    def __init__(self, x, y, r):
        self.x = x
        self.y = y
        self.r = r
        self.val = (0, 0)
        self.filter = (Filter(), Filter())

    def draw(self):
        self.gui_l = [
            gui.SUBLIST,
            [
                gui.DL_SAVE_CONTEXT(),
                gui.DL_SCISSOR_SIZE(2 * self.r, 2 * self.r),
                gui.DL_SCISSOR_XY(self.x - self.r, self.y - self.r),
                gui.DL_COLOR_RGB(0, 41, 66),
                gui.DL_POINT_SIZE(self.r),
                gui.DL_BEGIN(gui.PRIM_POINTS),
                gui.DL_VERTEX2F(self.x, self.y),
                gui.DL_END(),
                gui.DL_COLOR(Colors.white),
                gui.DL_POINT_SIZE(self.r*.66),
                gui.DL_BEGIN(gui.PRIM_POINTS),
                gui.DL_VERTEX2F(self.x, self.y),
                gui.DL_END(),
                gui.DL_COLOR_RGB(0, 41, 66),
                gui.DL_POINT_SIZE(self.r*.66-3),
                gui.DL_BEGIN(gui.PRIM_POINTS),
                gui.DL_VERTEX2F(self.x, self.y),
                gui.DL_END(),
                gui.DL_COLOR(Colors.white),
                gui.DL_POINT_SIZE(self.r*.33),
                gui.DL_BEGIN(gui.PRIM_POINTS),
                gui.DL_VERTEX2F(self.x, self.y),
                gui.DL_END(),
                gui.DL_COLOR_RGB(0, 41, 66),
                gui.DL_POINT_SIZE(self.r*.33-3),
                gui.DL_BEGIN(gui.PRIM_POINTS),
                gui.DL_VERTEX2F(self.x, self.y),
                gui.DL_END(),
                gui.DL_COLOR_RGB(0, 41, 66),
                gui.DL_COLOR_A(255),
                gui.DL_RESTORE_CONTEXT(),
                gui.DL_COLOR(Colors.white),
                gui.DL_LINE_WIDTH(2),
                gui.DL_BEGIN(gui.PRIM_LINES),
                gui.DL_VERTEX2F(self.x - self.r + 2, self.y),
                gui.DL_VERTEX2F(self.x + self.r - 2, self.y),
                gui.DL_VERTEX2F(self.x, self.y - self.r + 2),
                gui.DL_VERTEX2F(self.x, self.y + self.r - 2),
                gui.DL_END(),
                gui.DL_COLOR(Colors.red),
                gui.DL_POINT_SIZE(self.r * .1),
            ],
            [gui.PRIM_POINTS, [gui.DL_VERTEX2F(*self.val)]],
        ]
        return self.gui_l

    def update_vals(self, lat,lon):
        if lat > 1.5:
            lat = 1.5
        elif lat < -1.5:
            lat = -1.5
        if lon > 1.5:
            lon = 1.5
        elif lon < -1.5:
            lon = -1.5
        self.val = ((lat*(self.r*.66)),(lon*(self.r*.66)))

    def update(self):
        self.gui_l[2][1][0] = gui.DL_VERTEX2F(
            self.x + self.filter[0](self.val[0]),
            self.y + self.filter[1](self.val[1]),
        )

def load_fonts():
    from font import load_font
    start, end = load_font('/sd/BebasNeue Bold-56.rft', 8)
    start, end = load_font('/sd/BebasNeue Bold-140.rft', 9, mempos_end=start)
    start, end = load_font('/sd/BebasNeue Bold-200.rft', 10, mempos_end=start)

def picbtn_gui_index_from_name(name):
    try:
        pb = next(pb for pb in buttons if pb.name == name)
        index = pb.gui_l_index
    except:
        index = None
    return index

def set_picture_btn_colour(button, colour):
    if isinstance(button, Picture_Button):
        index = button.gui_l_index
    elif isinstance(button, str):
        index = picbtn_gui_index_from_name(button)
    else:
        index = None
    if not index is None:
        gui_l[gui_l_picture_btns_index][index] = colour

def set_gnss_btn_state(state):
    if state:
        set_picture_btn_colour('GNSS', gui.DL_COLOR_RGB(0, 255, 0))
    else:
	    set_picture_btn_colour('GNSS', gui.DL_COLOR_RGB(255, 0, 0))

def set_media_btn_state(media, logging):
    if not media:
        set_picture_btn_colour('Media', gui.DL_COLOR_RGB(255, 0, 0))
    elif logging:
        set_picture_btn_colour('Media', gui.DL_COLOR_RGB(0, 128, 255))
    else:
        set_picture_btn_colour('Media', gui.DL_COLOR_RGB(0, 255, 0))	
	
	
# Functions and constants ######################################################
def vsync_cb(l):
    global gnss_status
    set_media_btn_state(vts.sd_present(), logging_active)
    set_gnss_btn_state(gnss_status)
    g_disp.update()
    g_readings.update()
    gui.redraw()

def wrap_callback(callback):
    def cb(*args, **kwargs):
        ui.play_sound(ui.snd_click)
        callback(*args, **kwargs)
    return cb

def reset_cb(button):
    g_readings.reset()

def create_buttons():
    global buttons
    buttons = [
        Picture_Button(468, 410, bank.get('Screenshot'),'Screenshot',screenshot_cb),
        Picture_Button(183, 410, bank.get('Reset'),'Reset',reset_cb),
        Picture_Button(340,  10, bank.get('Media'), 'Media'),
        Picture_Button(410,  10, bank.get('GNSS'), 'GNSS'),
    ]
    button_cbs_l = []
    button_icons_l = [gui.DL_BEGIN(gui.PRIM_BITMAPS)]
    for i, button in enumerate(buttons):
        button_cb_l = [
             gui.PARAM_TAG_REGISTER,
             wrap_callback(button.get_callback()),
        ]
        button_cb_l.append(button.name)
        button_cbs_l.append(button_cb_l)
        button.set_gui_l_index(len(button_icons_l))
        button_icons_l.extend(button.generate_gui_l(i + 1))
    button_icons_l.append(gui.DL_TAG(0))
    return button_cbs_l, button_icons_l

 
def sample_cb():
    global gnss_status, last_sample, logging_active
    sample = vbox.get_sample()
    if sample.sats_used >= 3 and 2 <= sample.fix_type <= 4:
        if not logging_active and vts.sd_present():
            vbo.start()
            logging_active = True
        elif logging_active and not vts.sd_present():
            vbo.stop()
            logging_active = False
        if last_sample is not None and sample.speed_gnd_mps > (0.8/1000):
            roc_heading = sample.heading_deg - last_sample.heading_deg
            if roc_heading > 300:
                roc_heading = -360 + roc_heading
            if roc_heading < -300:
                roc_heading = 360 + roc_heading
            if roc_heading == 0:
                roc_heading = 0.00001
            
            roc_heading = roc_heading / (sample.update_period_ms/1000)
            
            latacc_unsmooth = -1 * sample.speed_gnd_mps * roc_heading * (math.pi / 180)

            latacc = latacc_unsmooth/9.80665
            if latacc > 9.99:
                latacc = 9.99
            elif latacc < -9.99:
                latacc = -9.99
            
            longacc = sample.lngacc_smooth_mps2/9.80665
            if longacc > 9.99:
                longacc = 9.99
            elif longacc < -9.99:
                longacc = -9.99

            g_readings.update_vals(latacc,longacc)
            g_disp.update_vals(latacc,longacc)

        gnss_status = True
        last_sample = sample
    else:
        gnss_status = False
		
# Main #########################################################################
def main():
    global gui_l, gui_l_picture_btns_index
    global ui
    ui = UI(255)
	
    global bank, logging_active, gnss_status, last_sample
    bank = Image_Bank((
        ('/sd/icon-reset.png',                         'Reset'),
        ('/sd/icon-screenshot.png',                    'Screenshot'),
        ('/sd/icons8-gnss-50.png',                     'GNSS'),
        ('/sd/icons8-sdcard-50.png',                   'Media'),
    ))
    load_fonts()
	
    logging_active = False
    gnss_status = False
    last_sample = None
    
    global button_cbs_l, button_icons_l   
    button_cbs_l, button_icons_l = create_buttons()

    global g_disp, g_readings
    g_disp = GDisplay(150, 200, 120)
    g_readings = GReadings()
    while (gnss.init_status() > 0):
        pass
    vbox.init(vbox.VBOX_SRC_GNSS_BASIC)
    vbox.set_new_data_callback(sample_cb)

    gui_l = [
        [gui.EVT_VSYNC, vsync_cb],	
		[gui.DL_SAVE_CONTEXT()],
        [gui.DL_CLEAR_COLOR_RGB(0, 41, 66)],
        [gui.DL_CLEAR(1, 1, 1)],
        [gui.DL_SCISSOR_SIZE(800, 350)],
        [gui.DL_SCISSOR_XY(0, 65)],
        [gui.DL_CLEAR_COLOR_RGB(255, 255, 255)],
        [gui.DL_CLEAR(1, 1, 1)],
        [gui.DL_RESTORE_CONTEXT()],
        [gui.DL_COLOR_RGB(255, 255, 255)],
        [gui.CTRL_TEXT, 30, 7, 8, 0, 'G-FORCE METER'],	
		[gui.DL_COLOR_RGB(0, 0, 0)],	
		[gui.CTRL_TEXT, 310, 100, 8, 0,'PEAK LAT G'],
		[gui.CTRL_TEXT, 310, 220, 8, 0,'PEAK ACC'],
		[gui.CTRL_TEXT, 310, 340, 8, 0,'PEAK DEC'],
        g_disp.draw(),
        g_readings.draw(),
        [gui.DL_COLOR_RGB(255, 255, 255)],
    ]
    gui_l_picture_btns_index = len(gui_l)    
    gui_l.append(button_icons_l)
    gui_l.extend(button_cbs_l)
    gui.show(gui_l)

if __name__ == '__main__':
    main()