##
# @file    main.py
# @brief   Torque Display
# @version 1.0

import can
import gui
import ft8xx as ft
import gui
import ustruct as us
import vts
from screenshot import screenshot_cb
from picture_button import Picture_Button
from image import Image_Bank
from utils import Filter
import os

### Constants ###
saved = {'scale': 1, 'offset': 0, 'units': 'Nm'}
scale =  ["{:.2f}".format(saved['scale'])]
offset = ["{:.2f}".format(saved['offset'])]
calibration_values = []
stored = []

keyboard_str = ['0']
keyboard_val = 0.0

page = 0
unit = ['NM']
swipe_r = 50
torque = ["{:06.2f}".format(0.0)]
filter = Filter()
int_max = 0
max_torque = ["{:06.2f}".format(0.0)]
filtered = 0
max_active = True

path = os.getcwd() + '/'
raw = 1

def load_fonts():
    from font import load_font
    start, end = load_font(path + 'BebasNeue_0-28.rft', 7)
    start, end = load_font(path + 'BebasNeue Bold-56.rft', 8, mempos_end=start)
    start, end = load_font(path + 'BebasNeue_0-50.rft', 9, mempos_end=start)
    start, end = load_font(path + 'BebasNeue_0-184.rft', 10, mempos_end=start)

def vsync_cb(l):
    gui.redraw()

def swipe_cb(gui_list,start):
    global page

    if start:
        pass
    else:
        si = gui.swipe_info()
        if si.dx <= -50:
            if (page == 0) and (max_active == True):
                page += 1
        elif si.dx >= 50:
            if page > 0:
                page -= 1
        draw()

def next_cb(button):
    global page
    global max_active
    if (page == 0) and (max_active == True):
        page += 1
    draw()

def previous_cb(button):
    global page
    if page > 0:
        page -= 1
    draw()

def view_press_cb(data):
    global scale, offset, torque, page
    if data[0] == 2:    # 0 = release, 1 = press, 2 = hold
        screenshot_cb(data)
    if data[0] == 0:    # 0 = release, 1 = press, 2 = hold
        print("Raw CAN: {}\t\tTorque:{}\t\tScale: {}\t\toffset:{}\t\tpage:{}".format(raw, torque[0], saved['scale'], saved['offset'], page))

def bytes_to_int(bytes):
    result = 0
    for b in bytes:
        result = result * 256 + int(b)
    return result

def handle_can(bus):
    global torque,filtered, int_max, max_torque, raw
    accepted_ids=[0x420,0x421,0x422,0x423,0x424,0x425,0x426,0x427,
                0x428,0x429,0x42A, 0x42B,0x42C,0x42D,0x42E,0x42F]
    while True:
        can_msg = can.get_msg(bus)
        if can_msg is None:
            return
        if can_msg.id in accepted_ids:
            raw = bytes_to_int(can_msg.data[0:2])
            if raw > 32767:                          # converts to signed integer (in the future maybe use ustruct.unpack()?)
                raw = raw-65536
            adjusted = (raw * saved['scale']) + saved['offset']
            filtered = filter(adjusted)
            # print("Raw: {} \t\tFiltered: {}".format((raw-offset_r[0])*scale[0], filtered))
            pad = '06' if filtered >= 0 else '07'
            torque[0] = "{:{p}.2f}".format(filtered, p=pad)
            if filtered > int_max:
                int_max = filtered
                max_torque[0] = "{:{p}.2f}".format(filtered, p= pad)

def can0_cb():
    handle_can(can.CAN0)

def can1_cb():
    handle_can(can.CAN1)

def reset_cb(button):
    global max_torque, int_max
    int_max = 0.0
    max_torque = ["{:06.2f}".format(0.0)]

def zero_cb(button):
    print(button)
    led_flash(0,0,50)

def ok_cb(null):
    global scale, offset, page, keyboard_str, stored

    stored = [saved['scale'], saved['offset']]

    if keyboard_str[0][len(keyboard_str[0])-1] == '.':
        keyboard_str[0] = keyboard_str[0][:-1]

    if page == -1:
        page = 0
    elif page == -2:
        apply_changes('scale')
        page = -1
    elif page == -3:
        apply_changes('offset')
        page = -1
    elif page == -4:
        calibration_values.append([filtered, keyboard_val])
        page = -5
    elif page == -5:
        calibration_values.append([filtered, keyboard_val])
        page = -6
    elif page == -6:
        calibration_values.append([filtered, keyboard_val])
        calibrate_cb()
        page = -1

    keyboard_str[0] = '0'
    draw()

def cancel_cb(null):
    global page, calibration_values

    if page == -1:
        page = 0
    elif page == -2:
        page = -1
    elif page == -3:
        page = -1
    elif page == -4:
        page = -1
    if page in [-5,-6]:
        saved['scale'], saved['offset'] = stored[0], stored[1]
        page = -1


    keyboard_str[0] = '0'
    calibration_values.clear()
    draw()

def settings_cb(button):
    global page
    page = -1
    draw()

def units_cb(button):
    global unit
    if unit[0] == 'NM':
        unit[0] = 'FT LBS'
    elif unit[0] == 'FT LBS':
        unit[0] = 'uV'
    elif unit[0] == 'uV':
        unit[0] = 'NM'

def calibrate_cb():
    from math import sqrt
    global calibration_values, scale, offset, saved

    if len(calibration_values) == 3:
        print(calibration_values)
        try:
            sum_x = calibration_values[0][0] + calibration_values[1][0] + calibration_values[2][0]
            sum_y = calibration_values[0][1] + calibration_values[1][1] + calibration_values[2][1]
            sum_x2 = ((calibration_values[0][0])**2) + ((calibration_values[1][0])**2) + ((calibration_values[2][0])**2)
            sum_y2 = ((calibration_values[0][1])**2) + ((calibration_values[1][1])**2) + ((calibration_values[2][1])**2)
            sum_xy = ((calibration_values[0][0]*calibration_values[0][1])+(calibration_values[1][0]*calibration_values[1][1])+(calibration_values[2][0]*calibration_values[2][1]))
            numerator = (len(calibration_values)*sum_xy)-(sum_x*sum_y)
            denomenator = len(calibration_values)*sum_x2-(sum_x**2)
            gradient = numerator/denomenator
            print("sumx:{}\tsumy:{}\tsumx2:{}\tsumxy:{}\nNumerator:{}\tDenominator:{}\tGradient:{}".format(sum_x,sum_y,sum_x2,sum_xy,numerator,denomenator,gradient))

        except ZeroDivisionError:
            gradient = 1
            print("Zero Division Error - Gradient")
        
        try:
            intercept = (sum_y-(gradient*sum_x))/len(calibration_values)

        except ZeroDivisionError:
            intercept = 0
            print("Zero Division Error - Intercept")
        try:
            error = numerator/sqrt(denomenator*(len(calibration_values)*sum_y2-(sum_y**2)))
            # expected = calibration_values[0][0]+(calibration_values[1][1]-calibration_values[0][1])*(calibration_values[2][0]-calibration_values[0][0])\
            #         / (calibration_values[2][1]-calibration_values[0][1])

            # error = (abs(calibration_values[1][0]-expected)/expected)*100

        except ZeroDivisionError:
            print("Zero Division Error - Unable to Calculate Error Percentage")
            error = 0

        print("\nCalibrated Scale: {}\tCalibrated offset: {}\tAccuracy: {}\n".format(gradient, intercept, error))

        saved['scale'] = gradient
        scale =  ["{:.2f}".format(gradient)]
        saved['offset'] = intercept
        offset = ["{:.2f}".format(intercept)]


        calibration_values.clear()
        pass
    else:
        calibration_values.clear()
        print("Calibration Error: Not Enough Values")

def max_toggle_cb(button):
    global max_active
    if button[5]== 0:
        button[5] = 65535
        max_active = True
    elif button[5]== 65535:
        button[5] = 0
        max_active = False

def apply_changes(parameter):
    global scale, offset, saved
    if parameter == 'scale':
        saved['scale'] = keyboard_val
        scale =  ["{}".format(keyboard_str[0])]

    elif parameter == 'offset':
        saved['offset'] = keyboard_val
        offset =  ["{}".format(keyboard_str[0])]

def keyboard_cb(button):
    from util import str_num_dec_places, normalise_value
    global keyboard_str, keyboard_val
    digits = 6 if '.' in keyboard_str[0] else 5
    if (0 <= button[8] <= 9):
        # Digit button
        if keyboard_str[0] == '0':
            keyboard_str[0] = str(button[8])
        elif keyboard_str[0] == '-0':
            keyboard_str[0] = "-" + str(button[8])
        elif len(str(keyboard_str[0])) < digits:
            keyboard_str[0] = keyboard_str[0] + str(button[8])
    elif button[8] == 10:
        # Decimal button
        if ('.' not in keyboard_str[0]) and (len(keyboard_str[0]) < digits):
            keyboard_str[0] = keyboard_str[0] + '.'
    elif button[8] == 11:
        # Clear button
        keyboard_str[0] = '0'
    elif button[8] == 12:
        # +/- button
        if (len(keyboard_str[0]) < digits) and (keyboard_str[0][0]!='-'):
            keyboard_str[0] = '-' + keyboard_str[0]
        elif (len(keyboard_str[0]) == digits) and (keyboard_str[0][0]!='-'):
            if (keyboard_str[0][digits-1]=='.') or (keyboard_str[0][digits-2]=='.'):
                pass
            elif keyboard_str[0][0]!='0':
                keyboard_str[0] = keyboard_str[0][0:digits-1]
                keyboard_str[0] = '-' + keyboard_str[0]
            else:
                keyboard_str[0] = '-' + keyboard_str[0]
        elif keyboard_str[0][0]=='-':
            keyboard_str[0] = keyboard_str[0][1:]
    while (len(keyboard_str[0]) > 1) and (keyboard_str[0][0] == '0'):
        if keyboard_str[0][1] == '.':
            break
        keyboard_str[0] = keyboard_str[0][1:]
    keyboard_val = normalise_value(float(keyboard_str[0]), 2)
    print ('keyboard_str: {}\t\t keyboard_val: {}'.format(keyboard_str[0],keyboard_val))

def led_flash(red= 0, green= 50, blue= 0, delay= 250, blink = 0):
    """A function to flash the LEDs

    Keyword Arguments:
        red {int} -- 0-255 (default: {0})
        green {int} -- 0-255 (default: {50})
        blue {int} -- 0-255 (default: {0})
        delay {int} -- Delay in ms between flashes (default: {250})
        blink {int} -- Number of times the leds should additionally flash (default: {0})

    Returns:
        N/A -- Does not return a value
    """
    if blink == 0:
        vts.leds(*[red,green,blue]*4)
        vts.delay_ms(delay)
        vts.leds(*[0]*12)
    else:
        for i in range(blink):
            vts.leds(*[red,green,blue]*4)
            vts.delay_ms(delay)
            vts.leds(*[0]*12)
            vts.delay_ms(delay)

def wrap_callback(callback):
    def cb(*args, **kwargs):
        callback(*args, **kwargs)
    return cb

def create_buttons(*args):
    global buttons
    buttons = []
    if 'Reset' in args:
        buttons.append(Picture_Button(350, 410, bank.get('Reset'),'Reset',reset_cb))
    if 'Settings' in args:
        buttons.append(Picture_Button(0, 410, bank.get('Settings'),'Settings',settings_cb))
    if 'Previous' in args:
        buttons.append(Picture_Button(650, 410, bank.get('Previous'), 'Previous', previous_cb))
    if 'Next' in args:
        buttons.append(Picture_Button(725, 410, bank.get('Next'), 'Next', next_cb))
    if 'Cancel' in args:
        buttons.append(Picture_Button(-20, 410, bank.get('Cancel'), 'Cancel', cancel_cb))
    if 'Ok' in args:
        buttons.append(Picture_Button(725, 410, bank.get('Ok'),'Ok',ok_cb))
    if 'Save' in args:
        buttons.append(Picture_Button(350, 410, bank.get('Save'),'Save',save_cb))

    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))

    return button_icons_l, button_cbs_l

def drawPage():
    global max_active, page

    if page == 0:
        return drawPageMain()
    elif page == 1:
        return drawMax()
    elif page == -1: #settings
        return drawSettings()
    elif page == -2:
        return draw_number_input('ENTER SCALE')
    elif page == -3:
        return draw_number_input('ENTER OFFSET')
    elif page == -4:
        return draw_number_input('ENTER CALIBRATION VALUE 1')
    elif page == -5:
        return draw_number_input('ENTER CALIBRATION VALUE 2')
    elif page == -6:
        return draw_number_input('ENTER CALIBRATION VALUE 3')
    else:
        drawPageMain()
        print("Page Error")

def drawPageMain():
    global unit
    gui_input_list = [
        gui.SUBLIST,
        [gui.DL_COLOR_RGB(0, 0, 0)],
        [gui.CTRL_TEXT, 30, 100, 8, 0,'TORQUE'],
        [gui.CTRL_TEXT, 580, 180, 10, gui.OPT_RIGHTX,torque],
        [gui.CTRL_TEXT, 600, 300, 32, 0,unit],
        [gui.DL_COLOR_RGB(255, 255, 255)],
        [gui.CTRL_TEXT, 20, 10, 8, 0, 'TORQUE SENSOR']
    ]
    return gui_input_list

def drawMax():
    global unit
    gui_input_list = [
        gui.SUBLIST,
    [gui.DL_COLOR_RGB(0, 0, 0)],
    [gui.CTRL_TEXT, 30, 100, 8, 0,'MAX TORQUE'],
    [gui.CTRL_TEXT, 580, 180, 10, gui.OPT_RIGHTX, max_torque],
    [gui.CTRL_TEXT, 600, 300, 31, 0, unit],
    [gui.DL_COLOR_RGB(255, 255, 255)],
    [gui.CTRL_TEXT, 20, 10, 8, 0, 'TORQUE SENSOR']
    ]

    return gui_input_list

def drawSettings():
    global scale, offset_r, unit, max_active

    en = 65535 if max_active else 0

    gui_input_list = [
        gui.SUBLIST,
        [gui.CTRL_TEXT, 20, 10, 8, 0, 'GENERAL SETTINGS'],
        [gui.DL_COLOR_RGB(0, 0, 0)],
        [gui.CTRL_TEXT, 20, 80, 8, 0,'MANUAL INPUT'],
        [gui.CTRL_TEXT, 420, 80, 8, 0,'AUTO CALIBRATE'],
        [gui.CTRL_TEXT, 420, 260, 8, 0,'MAX TORQUE SCREEN'],
        [gui.DL_LINE_WIDTH(2)],
        [gui.DL_BEGIN(gui.PRIM_LINES)],
        [gui.DL_VERTEX2F(390, 65)], [gui.DL_VERTEX2F(390, 415)], # Centre line
        [gui.DL_VERTEX2F(390, 250)], [gui.DL_VERTEX2F(800,250)],
        [gui.DL_END()],
        [gui.CTRL_TEXT, 50, 150, 9, 0,'SCALE'],
        [gui.CTRL_TEXT, 50, 240, 9, 0,'OFFSET'],
        [gui.CTRL_TEXT, 50, 330, 9, 0,'UNITS'],
        [gui.DL_COLOR_RGB(255, 255, 255)],
        # [gui.CTRL_FLATBUTTON, 205, 135, 170,40,29,'CONTINUOUS',enter_cb], # log
        # [gui.CTRL_FLATBUTTON, 205, 305, 170,40,29,'ENABLED',enter_cb], # ntrip
        # [gui.CTRL_FLATBUTTON, 205, 365, 170,40,29,'38400',enter_cb], # dgps
        # [gui.CTRL_FLATBUTTON, 600, 135, 170,40,29,'AUTO',enter_cb], # Day
        # [gui.CTRL_FLATBUTTON, 600, 190, 170,40,29,'1',enter_cb], # Night
        # [gui.CTRL_FLATBUTTON, 600, 305, 170,40,29,'3',enter_cb], # day
        # [gui.CTRL_FLATBUTTON, 600, 365, 170,40,29,'1',enter_cb], # night
        [gui.CTRL_FLATBUTTON, 200, 145, 165, 65, 31, scale, number_input,'Scale'],
        [gui.CTRL_FLATBUTTON, 200, 235, 165, 65, 31, offset, number_input,'Offset'],
        [gui.CTRL_FLATBUTTON, 420, 145, 350, 75, 32, 'CALIBRATE', number_input, 'C1'],
        [gui.CTRL_FLATBUTTON, 200, 325, 165, 65, 31, unit, units_cb],
        [gui.CTRL_TOGGLE, 490, 350, 210, 31, en, [b'DISABLED\xffENABLED'],max_toggle_cb],
        ]


    return gui_input_list

def draw_number_input(title="INPUT NUMBER"):
    gui_input_list = [
        gui.SUBLIST,
            [gui.CTRL_TEXT, 20, 10, 8, 0, title],
            [gui.CTRL_FLATBUTTON,  40,  85, 100, 70,  8, '1', keyboard_cb, 1],
            [gui.CTRL_FLATBUTTON, 150,  85, 100, 70,  8, '2', keyboard_cb, 2],
            [gui.CTRL_FLATBUTTON, 260,  85, 100, 70,  8, '3', keyboard_cb, 3],
            [gui.CTRL_FLATBUTTON,  40, 165, 100, 70,  8, '4', keyboard_cb, 4],
            [gui.CTRL_FLATBUTTON, 150, 165, 100, 70,  8, '5', keyboard_cb, 5],
            [gui.CTRL_FLATBUTTON, 260, 165, 100, 70,  8, '6', keyboard_cb, 6],
            [gui.CTRL_FLATBUTTON,  40, 245, 100, 70,  8, '7', keyboard_cb, 7],
            [gui.CTRL_FLATBUTTON, 150, 245, 100, 70,  8, '8', keyboard_cb, 8],
            [gui.CTRL_FLATBUTTON, 260, 245, 100, 70,  8, '9', keyboard_cb, 9],
            [gui.CTRL_FLATBUTTON, 150, 325, 100, 70,  8, '0', keyboard_cb, 0],
            [gui.CTRL_FLATBUTTON, 260, 325, 100, 70,  8, '.', keyboard_cb, 10],
            [gui.CTRL_FLATBUTTON,  40, 325, 100, 70,  8, 'C', keyboard_cb, 11],
            [gui.CTRL_FLATBUTTON, 370, 325, 100, 70,  8,'+-', keyboard_cb, 12],
            # [gui.CTRL_FLATBUTTON, 370, 325, 100, 70,  8, '#', zero_cb, 12],
            [gui.DL_COLOR_RGB(0, 0, 0)],
            [gui.CTRL_TEXT, 760, 150, 10, gui.OPT_RIGHTX, keyboard_str],
        ]

    return gui_input_list

def number_input(button):
    global page, stored

    if button[8] == 'Scale':
        page = -2
    elif button[8] == 'Offset':
        page = -3
    elif button[8] == 'C1':
        stored = [saved['scale'], saved['offset']]
        saved['scale'], saved['offset'] = 1, 0
        page = -4
    elif button[8] == 'C2':
        page = -5
    elif button[8] == 'C3':
        page = -6
    else:
        print("Number Input Error")
    draw()

def save_cb(button):
    global page
    if page == -1:
        try:
            save_ok, msg = save_txt()
        except TypeError:
            save_ok, msg = False, 'SD Card Error - Not Compatible'
        if save_ok:
            led_flash(blink=2)
            print(msg)
        else:
            led_flash(50,0,0, blink=2)
            print(msg)

def get_file_name(directory = '/sd', prefix='VTTS_'):
    try:
        directory_list = os.listdir(directory)
    except OSError:
        return
    directory_list = [
        x for x in directory_list if len(x) == 12 and x[:5] == prefix  #if filename is 12 characters long and first 5 chars == prefix
    ]
    if not directory_list:
        n = 0
    else:
        n = int(max(directory_list)[5:8]) + 1
    return directory + '/{}{:03}.txt'.format(prefix, n)

def save_txt():
    if vts.sd_present():
        filename = get_file_name()
    else:
        return False, "Save to Txt Fail: No SD Card"
    with open(filename, 'w') as file:
        file.write("Torque Settings\n\n")
        for i in range(len(storedResults)):
            file.write("Scale: {}\nOffset: {}\nUnits: {}".format())
        file.close()

    return True, "Saved results to {}".format(filename)

def init_buttons():
    global button_layouts
    button_layouts = {}
    button_layouts['main'] = create_buttons('Settings', 'Next')
    button_layouts['main2'] = create_buttons('Settings')
    button_layouts['max'] = create_buttons('Reset', 'Settings', 'Previous')
    button_layouts['settings'] = create_buttons('Ok', 'Cancel')
    button_layouts['all'] = create_buttons('Reset', 'Settings', 'Previous', 'Next')

def button_options():
    global button_layouts
    global page
    gui_buttons = []
    settings_pages = [-1,-2,-3,-4,-5,-6]

    if page == 0: # Main page
        if max_active:
            gui_buttons.append(button_layouts['main'][0])
            gui_buttons.extend(button_layouts['main'][1])
        else:
            gui_buttons.append(button_layouts['main2'][0])
            gui_buttons.extend(button_layouts['main2'][1])

    elif page == 1:
        gui_buttons.append(button_layouts['max'][0])
        gui_buttons.extend(button_layouts['max'][1])

    elif page in settings_pages:
        gui_buttons.append(button_layouts['settings'][0])
        gui_buttons.extend(button_layouts['settings'][1])

    else: # incase of error, offer all options
        gui_buttons.append(button_layouts['all'][0])
        gui_buttons.extend(button_layouts['all'][1])

    return gui_buttons

def draw():
    gui_l = [
        [gui.EVT_VSYNC, vsync_cb],
        [gui.EVT_SWIPE, swipe_r, swipe_cb],
        [gui.EVT_PRESS, view_press_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_l.extend(button_options())

    gui_l.append(drawPage())

    gui.show(gui_l)

# Main #########################################################################
def main():
    global bank
    bank = Image_Bank((
        (path + 'icon-reset2.png',                         'Reset'),
        (path + 'icon-settings.png',                       'Settings'),
        (path + 'icon-forwards.png',                       'Next'),
        (path + 'icon-backwards.png',                      'Previous'),
        (path + 'icon-ok.png',                             'Ok'),
        (path + 'icon-cancel.png',                         'Cancel'),
        (path + 'icon-save.png',                           'Save'),
    ))

    # Initialization ###############################################################
    can.reset(can.CAN1, 1000000)
    # Set up CAN callback
    can.set_callback(can.CAN1, can1_cb)
    #Turn on Termination
    can.termination(can.CAN1, True)
    #Listen on all ids
    can.add_rx_id(can.CAN1,0,0)
    # Disable CAN test mode
    can.test_mode(can.CAN1, 0)
    # Initialization ###############################################################
    can.reset(can.CAN0, 1000000)
    # Set up CAN callback
    can.set_callback(can.CAN0, can0_cb)
    #Turn on Termination
    can.termination(can.CAN0, True)
    #Listen on all ids
    can.add_rx_id(can.CAN0,0,0)
    # Disable CAN test mode
    can.test_mode(can.CAN0, 0)

    load_fonts()

    init_buttons()

    draw()

if __name__ == '__main__':
    main()
    # print("\033[2J")  # clear the terminal when lots of stuff printed on startup