#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <vector>

#include "TaskMain.hpp"
#include "SDCard.hpp"
#include "peripheral_pin_map.h"

#ifdef __cplusplus
extern "C" {
#endif
#include <nrf_drv_timer.h>
#include <nrf_delay.h>

#include <nrf_drv_gpiote.h>
#include <nordic_common.h>
#include <bsp.h>
#include <app_error.h>
#include <app_scheduler.h>
#include "app_timer.h"

#include <nrf_sdh_soc.h>
#include <ble_nus.h>
#include <nrf_ble_qwr.h>
#include <nrf_log.h>
#include <nrf_log_ctrl.h>
#include <nrf_log_default_backends.h>
#include <nrf_gpio.h>

// Scheduler settings
#define SCHED_MAX_EVENT_DATA_SIZE       sizeof(uint32_t)
#define SCHED_QUEUE_SIZE                10

#define MANUFACTURER_NAME               "SensorStim"                   // Manufacturer. Will be passed to Device Information Service.
#define DEAD_BEEF                       0xDEADBEEF                     // Value used as error code on stack dump, can be used to identify stack location on stack unwind.


static uint32_t m_nus_link_ctx_storage_ctx_data_pool[((1))*(((sizeof(ble_nus_client_context_t)) + 3) >> 2)];
static blcm_link_ctx_storage_t m_nus_link_ctx_storage =                                                          \
{
	.p_ctx_data_pool = m_nus_link_ctx_storage_ctx_data_pool,
	.max_links_cnt   = ((1)),
	.link_ctx_size   = sizeof(m_nus_link_ctx_storage_ctx_data_pool)
};
static ble_nus_t m_nus =
{
	.uuid_type = 0,
	.service_handle = 0,
	.tx_handles = {0},
	.rx_handles = {0},
	.p_link_ctx_storage = &m_nus_link_ctx_storage,
};

NRF_SECTION_ITEM_REGISTER(sdh_ble_observers2, static nrf_sdh_ble_evt_observer_t m_nus_obs) =
{
    .handler   = ble_nus_on_ble_evt,
    .p_context = &m_nus,
};                                                // BLE NUS service instance.


/**@brief Function for the Timer initialization.
 *
 * @details Initializes the timer module. This creates and starts application timers.
 */
static void timers_init(void)
{
    // Initialize timer module.
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);

    // Create timers.
    NRF_LOG_DEBUG("Timers initialized.");
}


//! @brief Function for initializing buttons and leds.
//!
//! @param[out] p_erase_bonds  Will be true if the clear bonding button was pressed to wake the application up.
//!
static void buttons_leds_init(bool * p_erase_bonds)
{
    ret_code_t err_code;
    //bsp_event_t startup_event;
    err_code = bsp_init(BSP_INIT_LEDS, NULL);
    APP_ERROR_CHECK(err_code);

    // set LED_ERROR
    //nrf_gpio_cfg_output(LED_ERROR);
#ifdef BOARD_PCA10056
	nrf_gpio_pin_clear(LED_ERROR);
	nrf_delay_ms(500);
	nrf_gpio_pin_set(LED_ERROR);
#else
	//nrf_gpio_pin_set(LED_ERROR);
	nrf_delay_ms(500);
	//nrf_gpio_pin_clear(LED_ERROR);
#endif
    NRF_LOG_DEBUG("Board Support Package initialized.");
}


//! @brief Function for initializing the nrf log module.
//!
static void log_init(void)
{
    ret_code_t err_code = NRF_LOG_INIT(NULL);
    APP_ERROR_CHECK(err_code);

    NRF_LOG_DEFAULT_BACKENDS_INIT();
    NRF_LOG_DEBUG("Log module initialized.");
}

void power_handler(nrf_drv_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
{
	printf("Is this the End?. \r\n");
	printf("Game Over. \r\n");
	nrf_gpio_pin_clear(LED_STATUS);
	nrf_delay_ms(100);
	nrf_gpio_pin_clear(PS_HOLD);
}


void gpio_init(void)
{
	ret_code_t err_code;
	    #if defined BOARD_CUSTOM
	        err_code = nrf_drv_gpiote_init();
	        APP_ERROR_CHECK(err_code);

	        if (!nrf_drv_gpiote_is_init){
	               err_code = nrf_drv_gpiote_init();
	               APP_ERROR_CHECK(err_code);
	           }

	        nrf_gpio_cfg_output(PS_HOLD);
	        nrf_gpio_pin_set(PS_HOLD);
	        nrf_gpio_cfg_output(LED_STATUS);
	        nrf_gpio_pin_clear(LED_STATUS);

	        nrf_drv_gpiote_in_config_t power_config = {NRF_GPIOTE_POLARITY_HITOLO, NRF_GPIO_PIN_NOPULL, false, true};

	        err_code = nrf_drv_gpiote_in_init(POWERSWITCH, &power_config, power_handler);
	        APP_ERROR_CHECK(err_code);

	        nrf_drv_gpiote_in_event_enable(POWERSWITCH, true);
	        /* TODO: configure button for wakeup and power off */
	        //nrf_gpio_cfg_sense_input(POWERSWITCH, NRF_GPIO_PIN_NOPULL, NRF_GPIO_PIN_SENSE_LOW);

	    #elif defined BOARD_PCA10040
	        if (!nrf_drv_gpiote_is_init){
	            err_code = nrf_drv_gpiote_init();
	            APP_ERROR_CHECK(err_code);
	        }
	    #endif

}


#ifdef __cplusplus
}
#endif

//!
//! @brief Function for application main entry.
//!
int main(void)
{
    bool erase_bonds;
    ret_code_t err_code;

    log_init();
    //NRF_LOG_INFO("firmware compilation date and time: %s %s", __DATE__, __TIME__);
    NRF_LOG_ERROR("BOARD_CUSTOM");
    gpio_init();

    timers_init();
    buttons_leds_init(&erase_bonds);

    // Initiate the SD Card
    SDCard* sdcard = SDCard::getInstance();
    sdcard->init();

    TaskMain* taskMain = TaskMain::getInstance();
    taskMain->init();

    // initialized scheduler
    APP_SCHED_INIT(SCHED_MAX_EVENT_DATA_SIZE, SCHED_QUEUE_SIZE);
    while (true)
    {
        app_sched_execute();
    }
}

