/**
 * Universal remote controller, 2nd version
 *
 * Author: Jan Dvořák z Vozerovic
 * E-mail: dvorkaman@gmail.com
 * Web: dvorkaman.asp2.cz
 * Created: 2014
 */

/* Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

// libraries
#include "stdio.h"
#define _define_bool_

#define DEBUG_USART

#include "main.h"
#include "libInit.c"
#include "libGPIO.c"
#include "libTime.c"
#include "libDisplay.c"
#include "libSwitch.c"
#include "libUSART.c"
#include "libAnalog.c"
#include "libUtils.c"
#include "libXbeeApi.c"
#include "libRemote.c"
#include "libPWM.c"

// DEBUG
vu8 debug = 0; // false

// matrix input
libMatrixSwitch matrixInput;
uint16_t matrixInputPins[5] = {GPIO_Pin_5, GPIO_Pin_6, GPIO_Pin_7, GPIO_Pin_8, GPIO_Pin_9};
uint16_t matrixOutputPins[5] = {GPIO_Pin_10, GPIO_Pin_11, GPIO_Pin_12, GPIO_Pin_13, GPIO_Pin_14};

// analog
volatile uint16_t analogInputsDMA[REM_ANALOG_COUNT]; // buffer from ADC filled using DMA
libAnalogFilter analogFilter; // filter to the input values
libAnalog analogs[REM_ANALOG_COUNT]; // analog input trimmers using filtered values
u16 analogValues[REM_ANALOG_COUNT]; // used when reading analog collection

// digital
libLCDDisplay display;

// xbee
libXbeeApi xbee; // me
libRemoteXbee remoteXbees[MAX_XBEE_COUNT];
uint8_t enabledRemoteXbees = 0;
libRemoteXbee * actualRemoteXbee = 0;
uint8_t inputXbeeBuffer[XBEE_INPUT_BUFFER_LENGTH]; // for packet building RECIEVE
uint8_t outputXbeeBuffer[XBEE_OUTPUT_BUFFER_LENGTH]; // for packet building SEND

libPWMpin xbeeStatePins[MAX_XBEE_COUNT];
uint8_t xbeeStates[MAX_XBEE_COUNT]; // percentage

// - packet processing
xbeeApiGenericFrame* genericFrame; // allocated in input buffer
xbeeApiTransmitStatus transmitStatus;
xbeeApiRecievePacket recievePacket;
uint8_t recieveData[XBEE_RECIEVE_DATA_LENGTH];

// Command buffer
libRemoteTx remoteTx;

/**
* Main method
*/
int main(void)
{
    /**
     * DECLARATION
     */
    systemTime now, lastInputTime = getClock(), lastXbeeStateTime = getClock();
    uint32_t matrixInputState, lastMatrixInputState = 0;
    bool xbeeStateActivePeriod = FALSE, processInputs;
    uint8_t i;

    /**
     * INITIALIZATION
     */
    initialize(3);
    initializeClock();

    // get enabled xbees count
    enabledRemoteXbees = 1;

    // perihperies
    initializeRemoteController(enabledRemoteXbees);

    // LCD string
    printLCDDisplayAtPosition(&display, "Universal remote", 1, 1);
    printLCDDisplayAtPosition(&display, "controller|_*XB|", 1, 2);
    writeLCDDisplayAtPosition(&display, '0' + enabledRemoteXbees, 12, 2);

    // Read input, detect setup
    matrixInputState = readMatrixSwitch(&matrixInput);
    if (isMatrixSwitchPressedByMask(&matrixInput, MASK_BUTTON_1 | MASK_BUTTON_1) == TRUE)
    {
        setupMenu();
    }

    // Active 1st xbee
    activateRemoteXbee(&remoteXbees[0]);

    /**
     * MAIN LOOP
     */
    while (1)
    {
        now = getClock();
        processInputs = TRUE;

        // Xbee input Packets
        if (getXbeeApiReponseState(&xbee) == XBEE_API_SUCCESS)
        {
            genericFrame = getXbeeApiGenericFrameResponse();
            processGenericFrame(genericFrame);
        }

        // Connection check (if not debug)
        if (debug == 0)
        {
            for (i = 0; i < MAX_XBEE_COUNT; i++) checkRemoteXbeeConnection(&remoteXbees[i]);
        }

        // Connection state display
        if (getClockDiffInMs(lastXbeeStateTime, now) >= XBEE_STATE_INTERVAL)
        {
            lastXbeeStateTime = now;
            showRemoteXbeesState(xbeeStateActivePeriod);
            xbeeStateActivePeriod = xbeeStateActivePeriod == TRUE ? FALSE : TRUE; // negate
        }

        /* IF active NOT CONNECTED do not read input or send commands */
        if (actualRemoteXbee->state != REM_XBEE_CONNECTED && debug == 0) {
            processInputs = FALSE;    // skip following lines
        }

        // Sending commands
        if (isRemoteTxEmpty(&remoteTx) == FALSE && processInputs == TRUE)
        {
            if (debug == 0) // (if not debug)
            {
                actualRemoteXbee->state = REM_XBEE_SENDING; // disable further communication until responsed

                sendRemoteXbee(actualRemoteXbee, getRemoteTxNextCommand(&remoteTx));

                processInputs = FALSE; // skip input read untill all is sent

                // DEBUG
            } else {
                showSentCommand(getRemoteTxNextCommand(&remoteTx));
            }
        }

        // Inputs
        if (getClockDiffInMs(lastInputTime, now) >= PROCESS_INPUT_EACH &&
                isAnalogCollectionReady() == TRUE)
        {
            lastInputTime = now;

            // analog
            if (processInputs == TRUE)
            {
                readAnalogCollection(analogValues);
                checkAnalogInputs(actualRemoteXbee);
            }

            // Digital
            matrixInputState = readMatrixSwitch(&matrixInput);
            if (matrixInputState != lastMatrixInputState) // if not changed do nothing
            {
                checkXbeeSelections();

                if (processInputs == FALSE)
                {
                    continue; // skip further processing
                }

                lastMatrixInputState = matrixInputState;

                matrixInputState = lastMatrixInputState;
                checkButtonInputs(actualRemoteXbee);
                checkSwitchInputs(actualRemoteXbee);
            }
        }
    }
}

/**
* Activate remote xbee
*/
void activateRemoteXbee(libRemoteXbee * remoteXbee)
{
    if (remoteXbee->enabled == FALSE || remoteXbee->active == TRUE)return; // do nothing if disabled or already active
    remoteXbee->active = TRUE;

    // clear TX buffer
    if (isRemoteTxEmpty(&remoteTx) != TRUE)clearRemoteXbee(remoteXbee); // clear last sent values if the bufer is not fully empty
    clearRemoteTx(&remoteTx);

    // send commands
    if (actualRemoteXbee != 0)
    {
        actualRemoteXbee->active = FALSE;
        if (actualRemoteXbee->state == REM_XBEE_CONNECTED)
        {
            actualRemoteXbee->state = REM_XBEE_SENDING; // disable further communication until responsed
            sendRemoteXbee(actualRemoteXbee, REM_COMMNAND_LEAVE);
        }
    }

    actualRemoteXbee = remoteXbee;
    if (remoteXbee->state != REM_XBEE_SENDING)
    {
        remoteXbee->state = REM_XBEE_SENDING; // disable further communication until responsed
        sendRemoteXbee(remoteXbee, REM_COMMNAND_ACTIVATE);
    }
}

/**
* Check xbee selection
*/
void checkXbeeSelections(void)
{
    if (isMatrixSwitchPressedByMask(&matrixInput, MASK_XBEE_SELECT_1)) activateRemoteXbee(&remoteXbees[0]);
    else if (isMatrixSwitchPressedByMask(&matrixInput, MASK_XBEE_SELECT_2)) activateRemoteXbee(&remoteXbees[1]);
    else if (isMatrixSwitchPressedByMask(&matrixInput, MASK_XBEE_SELECT_3)) activateRemoteXbee(&remoteXbees[2]);
    else if (isMatrixSwitchPressedByMask(&matrixInput, MASK_XBEE_SELECT_4)) activateRemoteXbee(&remoteXbees[3]);
}

/**
* Check button inputs
*/
void checkButtonInputs(libRemoteXbee * remoteXbee)
{
    checkInputByMask(MASK_BUTTON_1, &(remoteXbee->lastButtonValues[0]), REM_COMMNAND_BUTTON1);
    checkInputByMask(MASK_BUTTON_2, &(remoteXbee->lastButtonValues[1]), REM_COMMNAND_BUTTON2);
    checkInputByMask(MASK_BUTTON_3, &(remoteXbee->lastButtonValues[2]), REM_COMMNAND_BUTTON3);
    checkInputByMask(MASK_BUTTON_4, &(remoteXbee->lastButtonValues[3]), REM_COMMNAND_BUTTON4);
    checkInputByMask(MASK_BUTTON_5, &(remoteXbee->lastButtonValues[4]), REM_COMMNAND_BUTTON5);
    checkInputByMask(MASK_BUTTON_6, &(remoteXbee->lastButtonValues[5]), REM_COMMNAND_BUTTON6);
    checkInputByMask(MASK_BUTTON_7, &(remoteXbee->lastButtonValues[6]), REM_COMMNAND_BUTTON7);
    checkInputByMask(MASK_BUTTON_8, &(remoteXbee->lastButtonValues[7]), REM_COMMNAND_BUTTON8);
    checkInputByMask(MASK_BUTTON_9, &(remoteXbee->lastButtonValues[8]), REM_COMMNAND_BUTTON9);
    checkInputByMask(MASK_BUTTON_10, &(remoteXbee->lastButtonValues[9]), REM_COMMNAND_BUTTON10);
}

/**
* Check switch inputs
*/
void checkSwitchInputs(libRemoteXbee * remoteXbee)
{
    checkInputByMask(MASK_SWITCH_1, &(remoteXbee->lastSwitchValues[0]), REM_COMMNAND_SWITCH1);
    checkInputByMask(MASK_SWITCH_2, &(remoteXbee->lastSwitchValues[1]), REM_COMMNAND_SWITCH2);
    checkInputByMask(MASK_SWITCH_3, &(remoteXbee->lastSwitchValues[2]), REM_COMMNAND_SWITCH3);
    checkInputByMask(MASK_SWITCH_4, &(remoteXbee->lastSwitchValues[3]), REM_COMMNAND_SWITCH4);
    checkInputByMask(MASK_SWITCH_5, &(remoteXbee->lastSwitchValues[4]), REM_COMMNAND_SWITCH5);
    checkInputByMask(MASK_SWITCH_6, &(remoteXbee->lastSwitchValues[5]), REM_COMMNAND_SWITCH6);
}

/**
* Check analog inputs
*/
void checkAnalogInputs(libRemoteXbee * remoteXbee)
{
    checkAnalogInputByIndex(0, &(remoteXbee->lastAnalogValues[0]), REM_COMMNAND_ANALOG1);
    checkAnalogInputByIndex(1, &(remoteXbee->lastAnalogValues[1]), REM_COMMNAND_ANALOG2);
    checkAnalogInputByIndex(2, &(remoteXbee->lastAnalogValues[2]), REM_COMMNAND_ANALOG3);
    checkAnalogInputByIndex(3, &(remoteXbee->lastAnalogValues[3]), REM_COMMNAND_ANALOG4);
    checkAnalogInputByIndex(4, &(remoteXbee->lastAnalogValues[4]), REM_COMMNAND_ANALOG5);
    checkAnalogInputByIndex(5, &(remoteXbee->lastAnalogValues[5]), REM_COMMNAND_ANALOG6);
    checkAnalogInputByIndex(6, &(remoteXbee->lastAnalogValues[6]), REM_COMMNAND_ANALOG7);
    checkAnalogInputByIndex(7, &(remoteXbee->lastAnalogValues[7]), REM_COMMNAND_ANALOG8);
}

/**
* Check digital input by mask
*/
void checkInputByMask(uint32_t mask, uint8_t* lastValueSource, u8 command)
{
    uint8_t val = isMatrixSwitchPressedByMask(&matrixInput, mask) == TRUE ? 1 : 0;
    if (*lastValueSource != val)
    {
        *lastValueSource = val;
        putRemoteTxCommand(&remoteTx, command, val);
    }
}

/**
* Check analog input by index
*/
void checkAnalogInputByIndex(uint8_t index, uint16_t* lastValueSource, u8 command)
{
    uint16_t val = analogValues[index];
    libAnalog*	la = &analogs[index];
    u16 lastValue = la->lastChangedValue;

    // use internal functinality of analog filtering
    la->lastChangedValue = *lastValueSource;

    if (isAnalogValueChangedDoNotProcessCheck(la, TRUE) == TRUE)
    {
        *lastValueSource = val;
        putRemoteTxCommand(&remoteTx, command, val);
    }

    // restore
    la->lastChangedValue = lastValue;
}

/**
* Show remote xbee state
*/
void showRemoteXbeesState(bool activePeriod)
{
    uint8_t i;
    uint8_t value = 0;
    libRemoteXbee* rx;

    for (i = 0; i < MAX_XBEE_COUNT; i++)
    {
        // determine value
        rx = &remoteXbees[i];
        if (rx->enabled == FALSE)
        {
            value = 0; // OFF
        }

        else
        {
            if (rx->active == TRUE) value = XBEE_STATE_ACTIVE;
            else value = XBEE_STATE_CONNECTED;

            if ((rx->state == REM_XBEE_NOT_CONNECTED || rx->state == REM_XBEE_CONNECTING) && activePeriod == FALSE) value = 0; // blink if not connected
        }

        // write if changed
        if (xbeeStates[i] != value)
        {
            xbeeStates[i] = value;
            writePWMpinPercent(&xbeeStatePins[i], value);
        }
    }
}

/**
* Initialize perihperies
*/
void initializeRemoteController(uint8_t enabledRemoteXbees)
{
    uint8_t i;

    // XBEE at USART 1
    initializeUSART1(); // A9 = TX, A10 = RX
    xbee = initializeXbeeApi(USART1, ROUTER, inputXbeeBuffer, XBEE_INPUT_BUFFER_LENGTH, outputXbeeBuffer, XBEE_OUTPUT_BUFFER_LENGTH);

    // Remote xbees
    for (i = 0; i < MAX_XBEE_COUNT; i++)
    {
        switch (i)
        {
            case 0:
                initializeRemoteXbee(&remoteXbees[0], &xbee, 1, XBEE_API_END_1_MSB, XBEE_API_END_1_LSB);
                break;
            case 1:
                initializeRemoteXbee(&remoteXbees[1], &xbee, 2, XBEE_API_END_2_MSB, XBEE_API_END_2_LSB);
                break;
            case 2:
                initializeRemoteXbee(&remoteXbees[2], &xbee, 3, 0, 0);
                break;
            case 3:
                initializeRemoteXbee(&remoteXbees[3], &xbee, 4, 0, 0);
                break;
        }

        remoteXbees[i].enabled = (i + 1 <= enabledRemoteXbees) ? TRUE : FALSE;
    }

    // Xbee state diodes
    initializePWMRemapTimer3();
    for (i = 0; i < MAX_XBEE_COUNT; i++)
    {
        xbeeStatePins[i] = initializePWMpin(TIM3, i + 1);
    }

    // Xbee memory pointers
    recievePacket.data = recieveData;

    // remote commands buffer
    remoteTx = initializeRemoteTx();

    // Matrix input - switches, buttons, xbee selectors, display
    matrixInput = initializeMatrixSwitch(GPIOB, matrixInputPins, 5, GPIOB, matrixOutputPins, 5);

    // Analog input
    initializeInputAnalogPin(GPIOA, GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7);
    inicializeADC(0, 7, (uint32_t) &analogInputsDMA); // A0 = port 0; A7 = port 7

    analogFilter = initializeAnalogFilter(REM_ANALOG_COUNT, analogInputsDMA);
    for (i = 0; i < REM_ANALOG_COUNT; i++)
    {
        analogs[i] = initializeAnalogInputRanges(&analogFilter, i, (i < 4 ? ANALOG_BLIND_SPOT : 0), ANALOG_MIMINAL_CHANGE, ANALOG_MIN_VALUE, ANALOG_VALUE_RANGE - ANALOG_MIN_VALUE);
        addLibAnalogToCollection(&analogs[i]);
    }

    // LCD display
    display = initializeLCDRWDisplay(GPIOB, GPIO_Pin_0, GPIO_Pin_1, GPIO_Pin_2, GPIOC, GPIO_Pin_0, GPIO_Pin_1, GPIO_Pin_2, GPIO_Pin_3, 2);

    // TURN Xbee state diodes ON
    for (i = 0; i < MAX_XBEE_COUNT; i++)
    {
        writePWMpinPercent(&xbeeStatePins[i], 100);
        xbeeStates[i] = 100;
    }
}

/**
* Check remote xbee connections
*/
void checkRemoteXbeeConnection(libRemoteXbee* remote)
{
    if (remote->enabled == FALSE) return;

    // not connected OR connected - retry connection
    if ((remote->state == REM_XBEE_NOT_CONNECTED) ||
            (remote->state == REM_XBEE_CONNECTED && getClockDiffInMs(remote->lastSuccessCommunication, getClock()) >= XBEE_RETRY_EACH))
    {
        remote->state = (remote->state == REM_XBEE_CONNECTED) ? REM_XBEE_SENDING : REM_XBEE_CONNECTING;	 // connecting if not connected before
        if (remote->active == TRUE)	sendRemoteXbee(remote, REM_COMMNAND_TEST_ACTIVE);
        else sendRemoteXbee(remote, REM_COMMNAND_TEST_NOT_ACTIVE);
        return;
    }

    // connecting - check time out
    if (remote->state == REM_XBEE_CONNECTING || remote->state == REM_XBEE_SENDING)
    {
        if (getClockDiffInMs(remote->lastSentCommunication, getClock()) >= REM_XBEE_CHECK_EACH)
        {
            remote->state = REM_XBEE_NOT_CONNECTED;
            clearRemoteXbee(remote);
        }
    }
}

/**
* Show error
*/
void showError(uint8_t code)
{
    uint8_t level = XBEE_STATE_CONNECTED;

    // show code
    writePWMpinPercent(&xbeeStatePins[1], ((code >> 1) & 0x1) * 100);
    writePWMpinPercent(&xbeeStatePins[2], (code & 0x1) * 100);

    while (1)
    {
        writePWMpinPercent(&xbeeStatePins[0], level);
        writePWMpinPercent(&xbeeStatePins[3], level);

        level += 10;
        if (level > 100) level = XBEE_STATE_CONNECTED;

        delayMs(100);
    }
}

/**
* Get xbee by frame id
*/
libRemoteXbee* getRemoteXbeeByFrameId(uint8_t frameId)
{
    uint8_t i;
    libRemoteXbee* found = 0;
    for (i = 0; i < MAX_XBEE_COUNT; i++)
    {
        if (remoteXbees[i].lastFrameId == frameId)
        {
            // detect same id in more xbees
            if (found == 0)found = &remoteXbees[i];
            else return 0;
        }
    }

    return found;
}


/**
* Get xbee by address
*/
libRemoteXbee* getRemoteXbeeByAddress(uint32_t addr64msb, uint32_t addr64lsb)
{
    uint8_t i;
    libRemoteXbee* found = 0;
    for (i = 0; i < MAX_XBEE_COUNT; i++)
    {
        if (remoteXbees[i].addr64msb == addr64msb && remoteXbees[i].addr64lsb == addr64lsb)
        {
            // detect same id in more xbees
            if (found == 0)found = &remoteXbees[i];
            else return 0;
        }
    }

    return found;
}

/**
* Process generic frame
*/
void processGenericFrame(xbeeApiGenericFrame* frame)
{
    libRemoteXbee* xbee = 0;
    u16 recievedCommand;

    // TX status
    if (frame->api == XBEE_API_TX_STATUS)
    {
        getXbeeApiResponseAsTxStatus(frame, &transmitStatus);
        xbee = getRemoteXbeeByFrameId(transmitStatus.frameId);
        if (xbee == 0) return;

        if (transmitStatus.deliveryStatus == XBEE_API_TX_STATUS_SUCESS)
        {
            // if not require ack from remote xbee set connected only after status OK	for SENDING state, required for connection
            if (xbee->state == REM_XBEE_SENDING && REQUIRE_CONFIRM_FROM_ROUTER == 0)xbee->state = REM_XBEE_CONNECTED;
        } else {
            xbee->state = REM_XBEE_NOT_CONNECTED;
            clearRemoteXbee(xbee);
            return;
        }
    }

    // INPUT PACKET
    else if (frame->api == XBEE_API_RECIEVE_PACKET)
    {
        getXbeeApiResponseAsRecievePacket(frame, &recievePacket, XBEE_RECIEVE_DATA_LENGTH);
        xbee = getRemoteXbeeByAddress(recievePacket.addr64msb, recievePacket.addr64lsb);
        if (xbee == 0) return;

        // get command
        if (recievePacket.dataLength < REM_COMMAND_LENGTH) return;
        recievedCommand = getRemoteRecievedCommand(&recievePacket.data[0]);

        // ACK
        if (recievedCommand == REM_COMMNAND_CONFIRM)
        {
            xbee->state = REM_XBEE_CONNECTED;
        }
    }

    // updpate system info
    if (xbee == 0) return;
    xbee->lastSuccessCommunication = getClock();
}

/**
* Setup
*/
void setupMenu(void)
{
    uint8_t state = 0, lastState = 255;
    uint8_t originEnabledRemoteXbees = enabledRemoteXbees;
    uint32_t matrixInputState, lastMatrixInputState = readMatrixSwitch(&matrixInput);

    printLCDDisplayAtPosition(&display, "Setup |B1:v|B2:x", 1, 1);

    // wait until released
    while (lastMatrixInputState != 0)
    {
        lastMatrixInputState = readMatrixSwitch(&matrixInput);
        delayMs(10);
    }

    lastMatrixInputState = 255; // force first redraw

    // read
    while (1)
    {
        matrixInputState = readMatrixSwitch(&matrixInput);
        if (matrixInputState == lastMatrixInputState) continue;
        lastMatrixInputState = matrixInputState;

        // Input - Select
        if (isMatrixSwitchPressedByMask(&matrixInput, MASK_BUTTON_1) == TRUE)
        {
            state += 1;
            if (state > 2) state = 0;
        }

        //  - OK
        else if (isMatrixSwitchPressedByMask(&matrixInput, MASK_BUTTON_2) == TRUE)
        {
            switch (state)
            {
                case 0:
                    enabledRemoteXbees += 1;
                    if (enabledRemoteXbees > MAX_XBEE_COUNT) enabledRemoteXbees = 1;
                    lastState = 255; // force redraw
                    break;

                case 1:
                    printLCDDisplayAtPosition(&display, " | DEBUG MODE | ", 1, 1);
                    debug = 1;
                    delayMs(120);
                    return;

                case 2:
                    // Save
                    if (originEnabledRemoteXbees != enabledRemoteXbees)
                    {
                        // TODO:

                        // Show message, wait
                        printLCDDisplayAtPosition(&display, "Settings saved. ", 1, 1);
                        printLCDDisplayAtPosition(&display, "Restart remote! ", 1, 2);
                        while (1) {}
                    }

                    return;
            }
        }

        // redraw
        if (state != lastState)
        {
            switch (state)
            {
                case 0:
                    printLCDDisplayAtPosition(&display, "Enabled Xbees: x", 1, 2);
                    writeLCDDisplayAtPosition(&display, '0' + enabledRemoteXbees, 16, 2);
                    break;

                case 1:
                    printLCDDisplayAtPosition(&display, "Enter debug mode", 1, 2);
                    break;

                case 2:
                    printLCDDisplayAtPosition(&display, "Exit setup      ", 1, 2);
                    break;
            }

            lastState = state;
        }
    }
}

/**
* Show sent command
*/
void showSentCommand(u16 command)
{
    u8 str[5];

    libRemoteCommandValue v = parseRemoteRecievedValue(command);
    printLCDDisplayAtPosition(&display, "Cmd:xx, Val:yyyy", 1, 2);

    numberToHex(v.command, 2, &str[0]);
    str[2] = '\0';
    printLCDDisplayAtPosition(&display, &str[0], 5, 2);

    numberToHex(v.value, 4, &str[0]);
    str[4] = '\0';
    printLCDDisplayAtPosition(&display, &str[0], 13, 2);
}