/**
 * Initializes the High Level System such as IO Pins and Drivers
 */
void high_level_init(void)
{
    // Initialize all board pins (early) that are connected internally.
    board_io_pins_initialize();
    
    #if ENABLE_TELEMETRY
        /* Add default telemetry components */
        tlm_component_add("disk");
        tlm_component_add("debug");
    #endif

    /**
     * Set-up Timer0 so that delay_ms(us) functions will work.
     * This function is used by FreeRTOS run time statistics.
     * If FreeRTOS is used, timer will be set-up anyway.
     * If FreeRTOS is not used, call it here such that delay_ms(us) functions will work.
     */
    vConfigureTimerForRunTimeStats();

    /**
     * Intentional delay here because this gives the user some time to
     * close COM Port at Hyperload and Open it at Hercules
     */
    delay_ms(STARTUP_DELAY_MS);
    hl_print_line();

    /* Print boot info */
#if USE_REDUCED_PRINTF
    const unsigned int cpuClock = sys_get_cpu_clock();
    const unsigned int sig = cpuClock / (1000 * 1000);
    const unsigned int fraction = (cpuClock - (sig*1000*1000)) / 1000;
    printf("System Boot @ %u.%u Mhz\n", sig, fraction);
#else
    printf("System Boot @ %.3f Mhz\n", sys_get_cpu_clock() / (1000 * 1000.0f));
#endif

    if(boot_watchdog_recover == sys_get_boot_type()) {
        char taskName[sizeof(FAULT_LAST_RUNNING_TASK_NAME) * 2] = { 0 };
        memcpy(&taskName[0], (void*) &(FAULT_LAST_RUNNING_TASK_NAME), sizeof(FAULT_LAST_RUNNING_TASK_NAME));

        hl_print_line();
        printf("System rebooted after crash.  Relevant info:\n"
               "PC: 0x%08X.  LR: 0x%08X.  PSR: 0x%08X\n"
               "Possible last running OS Task: '%s'\n",
                (unsigned int)FAULT_PC, (unsigned int)FAULT_LR, (unsigned int)FAULT_PSR,
                taskName);
        hl_print_line();
    }

    /**
     * Initialize the Peripherals used in the system
     * I2C2 : Used by LED Display, Acceleration Sensor, Temperature Sensor
     * ADC0 : Used by Light Sensor
     * SPI0 : Used by Nordic
     * SPI1 : Used by SD Card & External SPI Flash Memory
     */
    adc0_init();
    ssp1_init();
    ssp0_init(SPI0_CLOCK_SPEED_MHZ);
    if (!I2C2::getInstance().init(I2C2_CLOCK_SPEED_KHZ)) {
        puts("ERROR: Possible short on SDA or SCL wire (I2C2)!");
    }

    /* Initialize nordic wireless mesh network before setting up sys_setup_rit()
     * callback since it may access NULL function pointers.
     */
    if (!wireless_init()) {
        puts("ERROR: Failed to initialize wireless");
    }

    /* Set-up our RIT callback to perform some background book-keeping
     * If FreeRTOS is running, this service is disabled and FreeRTOS
     * tick hook will call hl_periodic_service()
     * @warning RIT interrupt should be setup before SD card is mounted
     *           since it relies on our timer.
     */
    sys_rit_setup(hl_periodic_service, g_time_per_rit_isr_ms);

    /**
     * If Flash is not mounted, it is probably a new board and the flash is not
     * formatted so format it, alert the user, and try to re-mount it
     */
    if(!hl_mount_storage(Storage::getFlashDrive(), " Flash "))
    {
        printf("FLASH not formatted, formatting now ... ");
        printf("%s\n", FR_OK == Storage::getFlashDrive().format() ? "Done" : "Error");
        hl_mount_storage(Storage::getFlashDrive(), " Flash ");
    }

    hl_mount_storage(Storage::getSDDrive(), "SD Card");

	/* After SD card is initted, set desired speed for spi1 */
    ssp1_set_max_clock(SPI1_CLOCK_SPEED_MHZ);
    hl_print_line();

    #if LOG_BOOT_INFO_TO_FILE
    log_boot_info(__DATE__);
    #endif

    /* Initialize all sensors of this board. */
    if(!hl_init_board_io()) {
        hl_print_line();
        LD.setLeftDigit('-');
        LD.setRightDigit('-');
        LE.setAll(0xFF);
    }
    else {
        LD.setNumber(TS.getFarenheit());
    }

    /* After Flash memory is mounted, try to set node address from a file */
    hl_wireless_set_addr_from_file();
    srand(LS.getRawValue() + time(NULL));

    // Display CPU speed in Mhz on LED display
    // LD.setNumber(sys_get_cpu_clock()/(1000*1000));

    /* Print memory information before we call main() */
    do {
        char buff[512] = { 0 };
        sys_get_mem_info_str(buff);
        printf("%s", buff);
        hl_print_line();
    } while(0);

    hl_handle_board_id();
    hl_show_prog_info();
    hl_print_line();

    puts("Calling your main()");
    hl_print_line();
}
/**
 * Initializes the High Level System such as IO Pins and Drivers
 */
void high_level_init(void)
{
    // Initialize all board pins (early) that are connected internally.
    board_io_pins_initialize();
    
    /**
     * Initialize the Peripherals used in the system.
     * I2C2 : Used by LED Display, Acceleration Sensor, Temperature Sensor
     * ADC0 : Used by Light Sensor
     * SPI0 : Used by Nordic
     * SPI1 : Used by SD Card & External SPI Flash Memory
     */
    adc0_init();
    ssp1_init();
    ssp0_init(SYS_CFG_SPI0_CLK_MHZ);
    if (!I2C2::getInstance().init(SYS_CFG_I2C2_CLK_KHZ)) {
        puts("ERROR: Possible short on SDA or SCL wire (I2C2)!");
    }

    /**
     * This timer does several things:
     *      - Makes delay_ms() and delay_us() methods functional.
     *      - Provides run-time counter for FreeRTOS task statistics (cpu usage)
     *      - Provides timer needed by SD card init() and mesh network: nordic driver uses delay_us()
     *
     * The slightly tricky part is that the mesh networking task will start to be serviced every
     * one millisecond, so initialize this and immediately initialize the wireless (mesh/nordic)
     * so by the time 1ms elapses, the pointers are initalized (and not NULL).
     */
    lpc_sys_setup_system_timer();

    /* After the Nordic SPI is initialized, initialize the wireless system asap otherwise
     * the background task may access NULL pointers of the mesh networking task.
     *
     * @warning Need SSP0 init before initializing nordic wireless.
     * @warning Nordic uses timer delay, so we need the timer setup.
     */
    if (!wireless_init()) {
        puts("ERROR: Failed to initialize wireless");
    }

    /* Add default telemetry components if telemetry is enabled */
    #if SYS_CFG_ENABLE_TLM
        tlm_component_add(SYS_CFG_DISK_TLM_NAME);
        tlm_component_add(SYS_CFG_DEBUG_DLM_NAME);
    #endif

    /**
     * User configured startup delay to close Hyperload COM port and re-open it at Hercules serial window.
     * Since the timer is setup that is now resetting the watchdog, we can delay without a problem.
     */
    delay_ms(SYS_CFG_STARTUP_DELAY_MS);
    hl_print_line();

    /* Print out CPU speed and what caused the system boot */
    hl_print_boot_info();

    /**
     * If Flash is not mounted, it is probably a new board and the flash is not
     * formatted so format it, so alert the user, and try to re-mount it.
     */
    if (!hl_mount_storage(Storage::getFlashDrive(), " Flash "))
    {
        printf("Erasing and formatting SPI flash, this can take a while ... ");

        flash_chip_erase();
        printf("%s\n", FR_OK == Storage::getFlashDrive().format() ? "Done" : "Error");

        if (!hl_mount_storage(Storage::getFlashDrive(), " Flash "))
        {
            printf("SPI FLASH is possibly damaged!\n");
            printf("Page size: %u\n", (unsigned) flash_get_page_size());
            printf("Mem  size: %u (raw bytes)\n", (unsigned) (flash_get_page_count() * flash_get_page_size()));
        }
    }

    hl_mount_storage(Storage::getSDDrive(), "SD Card");

	/* SD card initialization modifies the SPI speed, so after it has been initialized, reset desired speed for spi1 */
    ssp1_set_max_clock(SYS_CFG_SPI1_CLK_MHZ);
    hl_print_line();

    /* File I/O is up, so log the boot message if chosen by the user */
    #ifdef SYS_CFG_LOG_BOOT_INFO_FILENAME
    log_boot_info(__DATE__);
    #endif

    /* File I/O is up, so initialize the logger if user chose the option */
    #if SYS_CFG_INITIALIZE_LOGGER
    logger_init(SYS_CFG_LOGGER_TASK_PRIORITY);
    #endif

    /* Initialize all sensors of this board and display "--" on the LED display if an error has occurred. */
    if(!hl_init_board_io()) {
        hl_print_line();
        LD.setLeftDigit('-');
        LD.setRightDigit('-');
        LE.setAll(0xFF);
    }
    else {
        LD.setNumber(TS.getFarenheit());
    }

    /* After Flash memory is mounted, try to set node address from a file */
    hl_wireless_set_addr_from_file();

    /* Feed the random seed to get random numbers from the rand() function */
    srand(LS.getRawValue() + time(NULL));

    /* Print memory information before we call main() */
    do {
        char buff[512] = { 0 };
        sys_get_mem_info_str(buff);
        printf("%s", buff);
        hl_print_line();
    } while(0);

    /* Print miscellaneous info */
    hl_handle_board_id();
    hl_show_prog_info();
    hl_print_line();

    /* and finally ... call the user's main() method */
    puts("Calling your main()");
    hl_print_line();
}