#include "app_config.h"

#include "nrf_sdh.h"
#include "nrf_sdh_ble.h"
#include "nrf_drv_clock.h"

#include "nrf_sdh_freertos.h"
#include "FreeRTOS.h"
#include "task.h"

#include "ble_advdata.h"
#include "nrf_ble_gatt.h"

#include "app_timer.h"
#include "bsp.h"

#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#define TASK_DELAY                      200                                 /**< Task delay. Delays the LED task for 200 ms */

#define SCAN_INTERVAL                   0x00A0                              /**< Determines scan interval in units of 0.625 millisecond. */
#define SCAN_WINDOW                     0x0050                              /**< Determines scan window in units of 0.625 millisecond. */
#define SCAN_DURATION                   0x0000                              /**< Timout when scanning. 0x0000 disables timeout. */

#define MIN_CONNECTION_INTERVAL         MSEC_TO_UNITS(7.5, UNIT_1_25_MS)    /**< Determines minimum connection interval in milliseconds. */
#define MAX_CONNECTION_INTERVAL         MSEC_TO_UNITS(15, UNIT_1_25_MS)     /**< Determines maximum connection interval in milliseconds. */
#define SLAVE_LATENCY                   0                                   /**< Determines slave latency in terms of connection events. */
#define SUPERVISION_TIMEOUT             MSEC_TO_UNITS(4000, UNIT_10_MS)     /**< Determines supervision time-out in units of 10 milliseconds. */

#define APP_BLE_CONN_CFG_TAG            1                                   /**< A tag identifying the SoftDevice BLE configuration. */
#define APP_BLE_OBSERVER_PRIO           3                                   /**< Application's BLE observer priority. You shouldn't need to modify this value. */

NRF_BLE_GATT_DEF(m_gatt);                                                   /**< GATT module instance. */

static TaskHandle_t m_led_thread;                                           /**< Definition of LED bliking thread. */

/**@brief Parameters used when scanning. */
static ble_gap_scan_params_t const m_scan_params =
{
    .active   = 1,
    .interval = SCAN_INTERVAL,
    .window   = SCAN_WINDOW,

    .timeout           = SCAN_DURATION,
    .scan_phys         = BLE_GAP_PHY_1MBPS,
    .filter_policy     = BLE_GAP_SCAN_FP_ACCEPT_ALL,
};

/**@brief Connection parameters requested for connection. */
static ble_gap_conn_params_t const m_connection_param =
{
    (uint16_t)MIN_CONNECTION_INTERVAL,
    (uint16_t)MAX_CONNECTION_INTERVAL,
    (uint16_t)SLAVE_LATENCY,
    (uint16_t)SUPERVISION_TIMEOUT
};

static char const m_target_periph_name[] = DEVICE_NAME_PERIPHERAL;          /**< Name of the device we try to connect to. This name is searched in the scan report data*/

#define INVALID_CONN_HANDLE 0xFFFF
uint16_t m_conn_handle = INVALID_CONN_HANDLE;

ble_gap_addr_t m_peer_addr;

static uint8_t m_scan_buffer_data[BLE_GAP_SCAN_BUFFER_MIN]; /**< buffer where advertising reports will be stored by the SoftDevice. */

/**@brief Pointer to the buffer where advertising reports will be stored by the SoftDevice. */
static ble_data_t m_scan_buffer =
{
    m_scan_buffer_data,
    BLE_GAP_SCAN_BUFFER_MIN
};

/**@brief Function to start scanning ble devices.
 */
static void app_ble_scan_start(void *p_context)
{
    UNUSED_PARAMETER(p_context);
    ret_code_t err_code;

    (void) sd_ble_gap_scan_stop();

    err_code = sd_ble_gap_scan_start(&m_scan_params, &m_scan_buffer);
    APP_ERROR_CHECK(err_code);

    // Signal scanning.
    NRF_LOG_INFO("Started scanning.");
}

/**@brief Function for handling the advertising report BLE event.
 *
 * @param[in] p_adv_report  Advertising report from the SoftDevice.
 */
static void on_adv_report(ble_gap_evt_adv_report_t const * p_adv_report)
{
    ret_code_t err_code;

    if (ble_advdata_name_find(p_adv_report->data.p_data,
                              p_adv_report->data.len,
                              m_target_periph_name))
    {
        // copy address of found device
        memcpy(&m_peer_addr, &p_adv_report->peer_addr, sizeof(ble_gap_addr_t));
      
        NRF_LOG_DEBUG("Compatible device found, connecting.");
        err_code = sd_ble_gap_scan_start(NULL, &m_scan_buffer);

        // Name is a match, initiate connection.
        err_code = sd_ble_gap_connect(&m_peer_addr,
                                      &m_scan_params,
                                      &m_connection_param,
                                      APP_BLE_CONN_CFG_TAG);
        APP_ERROR_CHECK(err_code);
    }
    else
    {
      //NRF_LOG_DEBUG("Compatible device not found, continue scanning.");
        err_code = sd_ble_gap_scan_start(NULL, &m_scan_buffer);
        APP_ERROR_CHECK(err_code);
    }
}


/**@brief Function for handling BLE events.
 *
 * @param[in]   p_ble_evt   Bluetooth stack event.
 * @param[in]   p_context   Unused.
 */
static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
{
    ret_code_t err_code;

    // For readability.
    ble_gap_evt_t const * p_gap_evt = &p_ble_evt->evt.gap_evt;

    switch (p_ble_evt->header.evt_id)
    {
        // Update connection handle.
        case BLE_GAP_EVT_CONNECTED:
        {
            m_conn_handle = p_gap_evt->conn_handle;
            NRF_LOG_INFO("BLE peripheral connected.");
            bsp_board_led_on(BSP_BOARD_LED_1);
        } break;

        // Upon disconnection, reset the connection handle of the peer which disconnected
        case BLE_GAP_EVT_DISCONNECTED:
        {
            m_conn_handle = INVALID_CONN_HANDLE;
            NRF_LOG_INFO("BLE peripheral disconnected.");
            bsp_board_led_off(BSP_BOARD_LED_1);
            
            // start scanning again
            app_ble_scan_start(NULL);
        } break;

        // Call advertising report handler and connect if applicable
        case BLE_GAP_EVT_ADV_REPORT:
        {
            on_adv_report(&p_gap_evt->params.adv_report);
        } break;

        // We have not specified a timeout for scanning, so only connection attemps can timeout.
        case BLE_GAP_EVT_TIMEOUT:
        {
            NRF_LOG_DEBUG("GAP Connection Timeout.");
            if (p_gap_evt->params.timeout.src == BLE_GAP_TIMEOUT_SRC_CONN)
            {
          NRF_LOG_DEBUG("Connection request timed out.");
            }
        } break;

        // Accept parameters requested by peer.
        case BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST:
        {
            NRF_LOG_DEBUG("Connection Parameter Update Request.");
            err_code = sd_ble_gap_conn_param_update(p_gap_evt->conn_handle,
                                        &p_gap_evt->params.conn_param_update_request.conn_params);
            APP_ERROR_CHECK(err_code);
        } break;

        case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
        {
            NRF_LOG_DEBUG("PHY Update Request.");
            ble_gap_phys_t const phys =
          {
                .rx_phys = BLE_GAP_PHY_AUTO,
                .tx_phys = BLE_GAP_PHY_AUTO,
          };
            err_code = sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
            APP_ERROR_CHECK(err_code);
        } break;

        // Disconnect on GATT Client timeout event.
        case BLE_GATTC_EVT_TIMEOUT:
        {
            NRF_LOG_DEBUG("GATT Client Timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        APP_ERROR_CHECK(err_code);
        } break;

        // Disconnect on GATT Server timeout event.
        case BLE_GATTS_EVT_TIMEOUT:
        {
            NRF_LOG_DEBUG("GATT Server Timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
        } break;

        // No implementation needed.
        default:
        {
            NRF_LOG_DEBUG("ble_evt_handler evt id %i", p_ble_evt->header.evt_id);
        } break;
    }
}



/**@brief Function for initializing the BLE stack.
 *
 * @details Initializes the SoftDevice and the BLE event interrupts.
 */
static void app_ble_stack_init(void)
{
    ret_code_t err_code;

    err_code = nrf_sdh_enable_request();
    APP_ERROR_CHECK(err_code);

    // Configure the BLE stack using the default settings.
    // Fetch the start address of the application RAM.
    uint32_t ram_start = 0;
    err_code = nrf_sdh_ble_default_cfg_set(APP_BLE_CONN_CFG_TAG, &ram_start);
    APP_ERROR_CHECK(err_code);

    // Enable BLE stack.
    err_code = nrf_sdh_ble_enable(&ram_start);
    APP_ERROR_CHECK(err_code);

    // Register a handler for BLE events.
    NRF_SDH_BLE_OBSERVER(m_ble_observer, APP_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);

    NRF_LOG_INFO("BLE stack initialized.");
}

/**@brief Function for initializing the log.
 */
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_INFO("Logging initialized.");
}


/**@brief Function for initializing the clock.
 */
static void clock_init(void)
{
    ret_code_t err_code = nrf_drv_clock_init();
    APP_ERROR_CHECK(err_code);
}


/**@brief A function which is hooked to idle task.
 * @note Idle hook must be enabled in FreeRTOS configuration (configUSE_IDLE_HOOK).
 */
void vApplicationIdleHook( void ) { /*void*/ }


/**@brief Function for initializing the GATT module.
 */
static void app_ble_gatt_init(void)
{
    ret_code_t err_code = nrf_ble_gatt_init(&m_gatt, NULL);
    APP_ERROR_CHECK(err_code);
    NRF_LOG_INFO("GATT initialized.");
}

/**@brief LED task entry function.
 *
 * @param[in] pvParameter   Pointer that will be used as the parameter for the task.
 */
static void led_toggle_task_function (void * pvParameter)
{
    NRF_LOG_INFO("LED toggle task initialized.");

    UNUSED_PARAMETER(pvParameter);
    while (true)
    {
        bsp_board_led_invert(BSP_BOARD_LED_2);
        vTaskDelay(TASK_DELAY);
    }
}

/**@brief Function for initializing the BSP module.
 */
static void led_init(void)
{
    bsp_board_init(BSP_INIT_LEDS);
    bsp_board_led_on(BSP_BOARD_LED_0);
    NRF_LOG_INFO("LEDs initialized.");

    // Start blinking LED3.
    if (pdPASS != xTaskCreate(led_toggle_task_function,
                              "LED", 128, NULL, 3,
                              &m_led_thread))
    {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }
}

int main(void)
{
    // Initialize modules.
    log_init();
    clock_init();

    // Configure and initialize BLE stack and modules.
    app_ble_stack_init();
    app_ble_gatt_init();

    led_init();

    // Create a FreeRTOS task for the BLE stack.
    // The task will run app_ble_scan_start(NULL) before entering its loop.
    nrf_sdh_freertos_init(app_ble_scan_start, NULL);

    // Start FreeRTOS scheduler.
    vTaskStartScheduler();

    // application should never leave FreeRTOS scheduler and reach this point
    for (;;)
    {
        APP_ERROR_HANDLER(NRF_ERROR_FORBIDDEN);
    }
}
