##
# @file    main.py
# @brief   Visual display of VIPS over serial
# @version 1.0
#

import serial
import gui
import ft8xx as ft
import math
import ustruct as us
from utils import Filter
from screenshot import screenshot_cb

class Font_Loader:
    def read_file(self, fn):
        with open(fn, 'rb') as f:
            if f.read(4) != b'RF0 ':
                raise Exception('unrecognized font file format')
            first_char, nchars, total_data_len = us.unpack('<BBL', f.read(6))
            zdata = f.read()
        return first_char, total_data_len, zdata

    def set_base_address(self, mempos, mempos_end, total_data_len):
        if mempos is not None and mempos_end is not None:
            raise Exception('only one of mempos, mempos_end may be specified')
        if mempos is not None:
            pos = mempos
        elif mempos_end is not None:
            pos = mempos_end - total_data_len
        else:
            pos = 1048576 - total_data_len
        base = ft.RAM_G + pos
        base &= ~3
        return base

    def load(self, fn, font_handle, mempos=None, mempos_end=None):
        first_char, total_data_len, zdata = self.read_file(fn)
        base = self.set_base_address(mempos, mempos_end, total_data_len)

        gui.pause(True)
        ft.cpcmd_inflate(base, len(zdata), ft.addressof(zdata))
        font_metrics_start = base
        font_bmp_start = base + 148
        fmt = ft.rd32(font_metrics_start + 128)
        stride = ft.rd32(font_metrics_start + 132)
        width = ft.rd32(font_metrics_start + 136)
        height = ft.rd32(font_metrics_start + 140)
        ft.wr32(font_metrics_start + 144, font_bmp_start)
        ft.cp_start()
        ft.cp_cmd(gui.DL_BITMAP_HANDLE(font_handle))
        ft.cp_cmd(gui.DL_BITMAP_SOURCE(font_bmp_start))
        ft.cp_cmd(gui.DL_BITMAP_LAYOUT(fmt, stride, height))
        ft.cp_cmd(gui.DL_BITMAP_SIZE(0, 0, 0, width, height))
        ft.cpcmd_setfont2(font_handle, font_metrics_start, first_char)
        ft.cp_finish()
        gui.pause(False)

        return (base, base + total_data_len)

font_loader_obj = Font_Loader()
def load_font(*args, **kwargs):
    return font_loader_obj.load(*args, **kwargs)

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

# Functions and constants ######################################################
reportedBeacons = 0
markedPoint =[]
swipe_r = 50

def vsync_cb(l):
    global reportedBeacons,beacons
    global xValFiltered,yValFiltered
    global xLocation,yLocation
    global gui_base, gui_live
    global markedPoint
    beacons[0] = "{:d}".format(reportedBeacons)       
    xLocation[0] = "{:6.2f}".format(xValFiltered)
    yLocation[0] = "{:6.2f}".format(yValFiltered)
    reportedBeacons = 0
    gui_live = []
    gui_live.extend(gui_base)
    gui_live.append([gui.DL_COLOR_RGB(50, 200, 50)])
    gui_live.append([gui.DL_POINT_SIZE(5)])
    gui_live.append([gui.DL_BEGIN(gui.PRIM_POINTS)])    
    RoverLocation = metersToPixels([xValFiltered,yValFiltered])
    gui_live.append([gui.DL_VERTEX2F(RoverLocation[0],RoverLocation[1])])
    gui_live.append([gui.DL_END()])
    if (len(markedPoint) > 0):
        gui_live.append([gui.DL_COLOR_RGB(50, 50, 200)])
        gui_live.append([gui.DL_POINT_SIZE(5)])
        gui_live.append([gui.DL_BEGIN(gui.PRIM_POINTS)])    
        MarkLocation = metersToPixels(markedPoint)
        gui_live.append([gui.DL_VERTEX2F(MarkLocation[0],MarkLocation[1])])
        gui_live.append([gui.DL_END()])
#        markDistance = (markedPoint[0]-xValFiltered)*(markedPoint[0]-xValFiltered) + (markedPoint[1]-yValFiltered)*(markedPoint[1]-yValFiltered)
#        markDistance = math.sqrt(markDistance)
        gui_live.append([gui.DL_COLOR_RGB(0, 0, 0)])
        gui_live.append([gui.CTRL_TEXT, 180, 330, 28, gui.OPT_RIGHTX,'Distance to mark'])
        gui_live.append([gui.CTRL_TEXT, 155, 360, 28, gui.OPT_RIGHTX,"{0:.2f},{1:.2f}".format(markedPoint[0]-xValFiltered,markedPoint[1]-yValFiltered)])
        gui_live.append([gui.CTRL_TEXT, 180, 360, 28, gui.OPT_RIGHTX,'M'])
        gui_live.append([gui.DL_BEGIN(gui.PRIM_LINE_STRIP)])
        gui_live.append([gui.DL_VERTEX2F(RoverLocation[0],RoverLocation[1])])
        gui_live.append([gui.DL_VERTEX2F(MarkLocation[0],MarkLocation[1])])
        gui_live.append([gui.DL_END()])

    gui.show(gui_live)

def view_press_cb(data):
    if data[0] == 2:
        screenshot_cb(data)   

def swipe_cb(gui_list,start):
    if start:
        print('Starting to swipe')
    else:
        print('Stopping swipe')

# Serial parsing#####################################################################

existingBuffer = bytearray(0)

# Find the start of the message
def findDataStart():
    for start in range(len(existingBuffer)):
        if (existingBuffer[start] == 0x2A):
            return start
    return len(existingBuffer)

# decode beacons and x and y locations
def decodeBuffer():
    global xValFiltered,yValFiltered
    global reportedBeacons,beacons
    global existingBuffer
    xVal = us.unpack('d', existingBuffer[7:15])[0]
    yVal = us.unpack('d', existingBuffer[15:23])[0]
    if (existingBuffer[3] > reportedBeacons):
        reportedBeacons = existingBuffer[3]
    if (existingBuffer[3] > 0):
        if ((abs(xVal)<200) and (abs(yVal)<200)):
            xValFiltered = xFilter(xVal)
            yValFiltered = yFilter(yVal)
    # remove this packet from the buffer
    existingBuffer = existingBuffer[60:]

# called every serial data rx    
def serial_cb():
    global existingBuffer
    # read waiting data and add to buffer
    bytesWaiting = serial.available()
    dataIn = serial.read(bytesWaiting)
    existingBuffer.extend(dataIn)
    # find packet start marker and remove any bytes before it
    startIndex = findDataStart()
    if (startIndex>0):
        existingBuffer = existingBuffer[startIndex:]
    # if we have the full header check it's correct 
    if (len(existingBuffer) >= 3):
        if ((existingBuffer[1] == 60) and (existingBuffer[2] == 3)):
            if (len(existingBuffer) >= 60):
                decodeBuffer()
        else:
            # false positive on start marker, discard it and then next rx look again
            existingBuffer = existingBuffer[1:]

# map drawing############################################################
def metersToPixels(xyPos):
    global mapWidthM, mapHeightM, mapWidthPx, mapHeightPx
    global mapBottomM, mapLeftM,mapTopPx,mapLeftPx

    xlocation = mapLeftPx + mapWidthPx * (xyPos[0]-mapLeftM) / mapWidthM
    ylocation = mapTopPx + mapHeightPx - mapHeightPx * (xyPos[1]-mapBottomM) / mapHeightM

    return [xlocation,ylocation]

def MarkButton_cb(b):
    global markedPoint
    markedPoint=[xValFiltered,yValFiltered]
    
        
# Main #########################################################################
def main():
    global xLocation,yLocation,beacons
    global mapWidthM, mapHeightM, mapWidthPx, mapHeightPx
    global mapBottomM, mapLeftM,mapTopPx,mapLeftPx
    global xValFiltered,yValFiltered
    global xFilter,yFilter
    global gui_base, gui_live
    
    beacons = ["{:0d}".format(0)]
    xLocation = ["{:6.2f}".format(0.0)]
    yLocation = ["{:6.2f}".format(0.0)]
    xValFiltered = 0
    yValFiltered = 0
    xFilter = Filter()
    yFilter = Filter()
    # Initialization ###############################################################
    serial.open()
    serial.set_callback(serial_cb)
	
    load_fonts()

    mapWidthM = 8
    mapBottomM = -1
    mapLeftM = -1
    mapWidthPx = 500
    mapHeightPx = 350
    mapTopPx = 100
    mapLeftPx = 220
    mapHeightM = mapHeightPx*mapWidthM/mapWidthPx

    beaconLocations = [[0,0],[1.859,0],[3.643,0.184],[5.46,0.129],
                       [5.38,3.799],[3.5648, 3.7898],[1.714,3.660],[-0.2290,3.7712]]
   
    gui_base = [
        [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, 400)],
        [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, 'VBOX INDOOR POSITIONING SYSTEM'],	
        [gui.DL_COLOR_RGB(0, 0, 0)],	
        [gui.CTRL_TEXT, 180, 105, 28, gui.OPT_RIGHTX,'BEACONS USED'],
        [gui.CTRL_TEXT, 180, 180, 28, gui.OPT_RIGHTX,'X LOCATION'],
        [gui.CTRL_TEXT, 180, 255, 28, gui.OPT_RIGHTX,'Y LOCATION'],
        [gui.CTRL_TEXT, 180, 135, 28, gui.OPT_RIGHTX,beacons],
        [gui.CTRL_TEXT, 155, 210, 28, gui.OPT_RIGHTX,xLocation],
        [gui.CTRL_TEXT, 155, 285, 28, gui.OPT_RIGHTX,yLocation],
        [gui.CTRL_TEXT, 180, 210, 28, gui.OPT_RIGHTX,'M'],
        [gui.CTRL_TEXT, 180, 285, 28, gui.OPT_RIGHTX,'M'],
        [gui.DL_BEGIN(gui.PRIM_LINE_STRIP)],
        [gui.DL_VERTEX2F(mapLeftPx,mapTopPx)],
        [gui.DL_VERTEX2F(mapLeftPx,mapTopPx+mapHeightPx)],
        [gui.DL_VERTEX2F(mapLeftPx+mapWidthPx,mapTopPx+mapHeightPx)],
        [gui.DL_VERTEX2F(mapLeftPx+mapWidthPx,mapTopPx)],
        [gui.DL_VERTEX2F(mapLeftPx,mapTopPx)],
        [gui.DL_END()],
        [gui.DL_COLOR_RGB(255, 255, 255)],
        [gui.CTRL_BUTTON, 30, 395, 150, 40, 28, 'Mark', MarkButton_cb], # Arguments: type, x, y, width, height, label, font size,text,callback
        [gui.EVT_PRESS, view_press_cb],
        [gui.EVT_SWIPE, swipe_r, swipe_cb],
    ]

    gui_base.append([gui.DL_COLOR_RGB(255, 0, 0)])
    gui_base.append([gui.DL_POINT_SIZE(5)])
    gui_base.append([gui.DL_BEGIN(gui.PRIM_POINTS)])    
    for beacon in range(len(beaconLocations)):
        location = metersToPixels(beaconLocations[beacon])
        gui_base.append([gui.DL_VERTEX2F(location[0],location[1])])
    gui_base.append([gui.DL_END()])

    gui_live = []
    gui_live.extend(gui_base)

    gui.show(gui_live)

if __name__ == '__main__':
    main()
