#include "mbed.h"
#include "FlashIAPBlockDevice.h"
#include "update-client/block_device_application.hpp"

#include "mbed_trace.h"
#if MBED_CONF_MBED_TRACE_ENABLE
#define TRACE_GROUP "BOOTLOADER"
#endif // MBED_CONF_MBED_TRACE_ENABLE

#if MBED_CONF_MBED_TRACE_ENABLE
static UnbufferedSerial g_uart(CONSOLE_TX, CONSOLE_RX);

// Function that directly outputs to an unbuffered serial port in blocking mode.
static void boot_debug(const char *s) {
    size_t len = strlen(s);
    g_uart.write(s, len);
    g_uart.write("\r\n", 2);
}
#endif

int main() {
#if MBED_CONF_MBED_TRACE_ENABLE
    mbed_trace_init();
    mbed_trace_print_function_set(boot_debug);
#endif // MBED_CONF_MBED_TRACE_ENABLE

    tr_debug("BikeComputer bootloader\r");

    FlashIAPBlockDevice flashIAPBlockDevice(MBED_ROM_START, MBED_ROM_SIZE);
    int ret = flashIAPBlockDevice.init();
    if(ret == 0) {
        update_client::BlockDeviceApplication app(flashIAPBlockDevice, HEADER_ADDR - MBED_ROM_START, POST_APPLICATION_ADDR - MBED_ROM_START); //MBED_CONF_UPDATE_CLIENT_STORAGE_ADDRESS);//
        tr_debug("Checking active application\n");

        tr_debug("MBED_ROM_START : 0x%08x", MBED_ROM_START);
        tr_debug("MBED_ROM_SIZE : 0x%08x", MBED_ROM_SIZE);
        tr_debug("HEADER_ADDR : 0x%08x", HEADER_ADDR);
        tr_debug("APPLICATION_ADDR : 0x%08x", APPLICATION_ADDR);
        tr_debug("POST_APPLICATION_ADDR : 0x%08x\n", POST_APPLICATION_ADDR);

        update_client::UCErrorCode ret = app.checkApplication();
        if(ret == update_client::UCErrorCode::UC_ERR_NONE) {
            tr_debug("Application is valid");
            void *sp = *((void **) POST_APPLICATION_ADDR + 0);  // NOLINT(readability/casting)
            void *pc = *((void **) POST_APPLICATION_ADDR + 1);  // NOLINT(readability/casting)
            tr_debug("Starting application at address 0x%08x (sp 0x%08x, pc 0x%08x)", POST_APPLICATION_ADDR, (uint32_t) sp, (uint32_t) pc);
            mbed_start_application(POST_APPLICATION_ADDR);
        }
        tr_error("Error on check of the application");
    }
	tr_error("Error on init flash IAP block device");
    return -1;
}