/*
 * Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
 *
 * The information contained herein is confidential property of Nordic Semiconductor. The use,
 * copying, transfer or disclosure of such information is prohibited except by express written
 * agreement with Nordic Semiconductor.
 *
 */

#include <stdint.h>
#include <string.h>
#include "nrf.h"
#include "nordic_common.h"
#include "ble.h"
#include "ble_gap.h"
#include "ble_hci.h"
#include "app_error.h"
#include "app_timer.h"
#include "app_button.h"
#include "ble_advdata.h"
#include "boards.h"
#include "nrf_gpio.h"
#include "nrf_sdm.h"
#include "softdevice_handler.h"
#include "pstorage.h"
#include "device_manager.h"
#include "bsp.h"
#include "bsp_btn_ble.h"
#include "ble_advertising.h"
#include "debug_util.h"
#include "simple_uart.h"
#include "configuration.h"
#include "spi_master.h"
#include "acc_lis2.h"
#include "nrf_delay.h"

#define IS_SRVC_CHANGED_CHARACT_PRESENT    0                                          /**< Include or not the service_changed characteristic. If not enabled, the server's database cannot be changed for the lifetime of the device. */

#define SEND_NOTIFICATION_BUTTON_ID        0                                          /**< Id for button used for sending notification button. */

#define DEVICE_NAME                        "Helmet_alert"                                /**< Name of device. Will be included in the advertising data. */

#define APP_TIMER_PRESCALER                0                                          /**< Value of the RTC1 PRESCALER register. */
#define APP_TIMER_MAX_TIMERS               (1 + BSP_APP_TIMERS_NUMBER)                /**< Maximum number of simultaneously created timers. */
#define APP_TIMER_OP_QUEUE_SIZE            4                                          /**< Size of timer operation queues. */

#define APP_ADV_INTERVAL                   MSEC_TO_UNITS(50, UNIT_0_625_MS)           /**< The advertising interval (in units of 0.625 ms. This value corresponds to 40 ms). */
#define APP_ADV_TIMEOUT_IN_SECONDS         180                                        /**< The advertising timeout (in units of seconds). */

#define MIN_CONN_INTERVAL                  MSEC_TO_UNITS(500, UNIT_1_25_MS)           /**< Minimum acceptable connection interval (0.5 seconds). */
#define MAX_CONN_INTERVAL                  MSEC_TO_UNITS(1000, UNIT_1_25_MS)          /**< Maximum acceptable connection interval (1 second). */
#define SLAVE_LATENCY                      0                                          /**< Slave latency. */
#define CONN_SUP_TIMEOUT                   MSEC_TO_UNITS(4000, UNIT_10_MS)            /**< Connection supervisory timeout (4 seconds). */

#define HELMETALERT_PERIPHERAL_BASE_UUID     {{0xB3, 0x58, 0x55, 0x40, 0x50, 0x60, 0x11, \
                                           0xe3, 0x8f, 0x96, 0x08, 0x00, 0x00, 0x00,   \
                                           0x9a, 0x66}}                               /**< 128bit UUID base used for example. */

#define MULTILINK_PERIPHERAL_SERVICE_UUID  0x9001                                     /**< Serrvice UUID over the 128-bit base used for the example. */
#define MULTILINK_PERIPHERAL_CHAR_UUID     0x900A                                     /**< Characteristic UUID over the 128-bit base used for the example. */

#define HELMETALERT_SERVICE_UUID           0x900B																			 
#define HELMETALERT_UUID_ACC_CHAR          0x900C

#define SEC_PARAM_TIMEOUT                  30                                         /**< Timeout for Pairing Request or Security Request (in seconds). */
#define SEC_PARAM_BOND                     1                                          /**< Perform bonding. */
#define SEC_PARAM_MITM                     0                                          /**< Man In The Middle protection not required. */
#define SEC_PARAM_IO_CAPABILITIES          BLE_GAP_IO_CAPS_NONE                       /**< No I/O capabilities. */
#define SEC_PARAM_OOB                      0                                          /**< Out Of Band data not available. */
#define SEC_PARAM_MIN_KEY_SIZE             7                                          /**< Minimum encryption key size. */
#define SEC_PARAM_MAX_KEY_SIZE             16

#define PIN_UART_TX											 	 2																							
#define PIN_UART_RX											   3

static bool m_is_sending_data = false;

static uint16_t                  m_conn_handle = BLE_CONN_HANDLE_INVALID;             /**< Connection handle. */
static ble_gatts_char_handles_t  m_char_handles;                                      /**< GATT characteristic definition handles. */
static uint8_t                   m_base_uuid_type;                                    /**< UUID type. */
static dm_application_instance_t m_app_handle;                                        /**< Application instance allocated by device manager to the application. */

typedef struct {
    uint16_t                    service_handle;
    ble_gatts_char_handles_t    acc_value_handles;
    uint16_t                    conn_handle;
} ble_acc_t;

/**@brief Function for asserts in the SoftDevice.
 *
 * @details This function will be called in case of an assert in the SoftDevice.
 *
 * @warning This handler is an example only and does not fit a final product. You need to analyze
 *          how your product is supposed to react in case of Assert.
 * @warning On assert from the SoftDevice, the system can only recover on reset.
 *
 * @param[in] line_num    Line number of the failing ASSERT call.
 * @param[in] p_file_name File name of the failing ASSERT call.
 */
void assert_nrf_callback(uint16_t line_num, const uint8_t * p_file_name)
{
    app_error_handler(0xDEADBEEF, line_num, p_file_name);
}


/**@brief Function for the GAP initialization.
 *
 * @details This function sets up all the necessary GAP (Generic Access Profile) parameters of the
 *          device including the device name, appearance, and the preferred connection parameters.
 */
static void gap_params_init(void)
{
    uint32_t                err_code;
    ble_gap_conn_params_t   gap_conn_params;
    ble_gap_conn_sec_mode_t sec_mode;

    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);

    err_code = sd_ble_gap_device_name_set(&sec_mode,
                                          (const uint8_t *)DEVICE_NAME,
                                          strlen(DEVICE_NAME));
    APP_ERROR_CHECK(err_code);

    memset(&gap_conn_params, 0, sizeof(gap_conn_params));

    gap_conn_params.min_conn_interval = MIN_CONN_INTERVAL;
    gap_conn_params.max_conn_interval = MAX_CONN_INTERVAL;
    gap_conn_params.slave_latency     = SLAVE_LATENCY;
    gap_conn_params.conn_sup_timeout  = CONN_SUP_TIMEOUT;

    err_code = sd_ble_gap_ppcp_set(&gap_conn_params);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for initializing services that will be used by the application.
 *
 * @details Initialize the multilink custom service.
 */
static void services_init(void)
{
    uint32_t            err_code;
    ble_uuid_t          uuid;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_t    attr;
    ble_gatts_attr_md_t attr_md;
    ble_gatts_attr_md_t cccd_md;
    ble_gatts_attr_md_t char_ud_md;
    uint16_t            svc_test;

    static uint8_t multilink_peripheral_data;
    static uint8_t multilink_peripheral_ud[] = "Modifiable multilink_peripheral Data";

    ble_uuid128_t base_uuid = HELMETALERT_PERIPHERAL_BASE_UUID;

    err_code = sd_ble_uuid_vs_add(&base_uuid, &m_base_uuid_type);
    APP_ERROR_CHECK(err_code);

    uuid.type = m_base_uuid_type;
    uuid.uuid = MULTILINK_PERIPHERAL_SERVICE_UUID;

    err_code = sd_ble_gatts_service_add(BLE_GATTS_SRVC_TYPE_PRIMARY, &uuid, &svc_test);
    APP_ERROR_CHECK(err_code);

    uuid.uuid = MULTILINK_PERIPHERAL_CHAR_UUID;

    memset(&attr, 0, sizeof(ble_gatts_attr_t));
    attr.p_uuid    = &uuid;
    attr.p_attr_md = &attr_md;
    attr.max_len   = 1;
    attr.p_value   = &multilink_peripheral_data;
    attr.init_len  = sizeof(multilink_peripheral_data);

    memset(&attr_md, 0, sizeof(ble_gatts_attr_md_t));
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_ENC_NO_MITM(&attr_md.write_perm);
    attr_md.vloc = BLE_GATTS_VLOC_STACK;
    attr_md.vlen = 0;

    memset(&cccd_md, 0, sizeof(ble_gatts_attr_md_t));
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_ENC_NO_MITM(&cccd_md.write_perm);
    cccd_md.vloc = BLE_GATTS_VLOC_STACK;

    memset(&char_md, 0, sizeof(ble_gatts_char_md_t));
    char_md.p_cccd_md               = &cccd_md;
    char_md.char_props.notify       = 1;
    char_md.char_props.indicate     = 1;
    char_md.char_props.read         = 1;
    char_md.char_props.write        = 1;
    char_md.char_ext_props.wr_aux   = 1;
    char_md.p_user_desc_md          = &char_ud_md;
    char_md.p_char_user_desc        = multilink_peripheral_ud;
    char_md.char_user_desc_size     = (uint8_t)strlen((char *)multilink_peripheral_ud);
    char_md.char_user_desc_max_size = (uint8_t)strlen((char *)multilink_peripheral_ud);

    memset(&char_ud_md, 0, sizeof(ble_gatts_attr_md_t));
    char_ud_md.vloc = BLE_GATTS_VLOC_STACK;
    char_ud_md.vlen = 1;
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&char_ud_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&char_ud_md.write_perm);

    err_code = sd_ble_gatts_characteristic_add(BLE_GATT_HANDLE_INVALID,
                                               &char_md,
                                               &attr,
                                               &m_char_handles);
																								//char buf[20];
																								//sprintf(buf,"First service char handles: %d\n\r",(uint8_t)&m_char_handles.value_handle);
																								//debug_printf(buf);

    APP_ERROR_CHECK(err_code);

	
}

static void acc_service_init(void)
{
    uint32_t            err_code;
    ble_uuid_t          uuid;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_t    attr;
    ble_gatts_attr_md_t attr_md;
    ble_gatts_attr_md_t cccd_md;
    ble_gatts_attr_md_t char_ud_md;
    uint16_t            svc_test;

    static uint8_t accelerometer_data;
    static uint8_t accelerometer_ud[] = "Accelerometer data";

    ble_uuid128_t base_uuid = HELMETALERT_PERIPHERAL_BASE_UUID;

    err_code = sd_ble_uuid_vs_add(&base_uuid, &m_base_uuid_type);
    APP_ERROR_CHECK(err_code);

    uuid.type = m_base_uuid_type;
    uuid.uuid = HELMETALERT_SERVICE_UUID;

    err_code = sd_ble_gatts_service_add(BLE_GATTS_SRVC_TYPE_PRIMARY, &uuid, &svc_test);
    APP_ERROR_CHECK(err_code);

    // Accelerometer char init
		
    uuid.uuid = HELMETALERT_UUID_ACC_CHAR;

    memset(&attr, 0, sizeof(ble_gatts_attr_t));
    attr.p_uuid    = &uuid;
    attr.p_attr_md = &attr_md;
    attr.max_len   = sizeof(uint8_t);
    attr.p_value   = &accelerometer_data;
    attr.init_len  = sizeof(accelerometer_data);

    memset(&attr_md, 0, sizeof(ble_gatts_attr_md_t));
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&attr_md.write_perm);
    attr_md.vloc = BLE_GATTS_VLOC_STACK;
    attr_md.vlen = 0;

    memset(&cccd_md, 0, sizeof(ble_gatts_attr_md_t));
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.write_perm);
    cccd_md.vloc = BLE_GATTS_VLOC_STACK;

    memset(&char_md, 0, sizeof(ble_gatts_char_md_t));
    char_md.p_cccd_md               = &cccd_md;
    char_md.char_props.notify       = 1;
    char_md.char_props.read         = 1;
		char_md.char_props.write        = 1;
		char_md.char_props.write_wo_resp = 1;
    char_md.p_user_desc_md          = NULL; //&char_ud_md;
    char_md.p_char_user_desc        = accelerometer_ud;
    char_md.char_user_desc_size     = (uint8_t)strlen((char *)accelerometer_ud);
    char_md.char_user_desc_max_size = (uint8_t)strlen((char *)accelerometer_ud);

    memset(&char_ud_md, 0, sizeof(ble_gatts_attr_md_t));
    char_ud_md.vloc = BLE_GATTS_VLOC_STACK;
    char_ud_md.vlen = 1;
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&char_ud_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&char_ud_md.write_perm);

    err_code = sd_ble_gatts_characteristic_add(BLE_GATT_HANDLE_INVALID,
                                               &char_md,
                                               &attr,
                                               &m_char_handles);
																							 //char buf[20];
																							 //sprintf(buf,"Acc service char handles: %d\n\r",(uint8_t)m_char_handles.value_handle);
																							 //debug_printf(buf);
	
}


/**@brief Function for putting the chip into sleep mode.
 *
 * @note This function will not return.
 */
static void sleep_mode_enter(void)
{
    uint32_t err_code = bsp_indication_set(BSP_INDICATE_IDLE);
    APP_ERROR_CHECK(err_code);

    // Prepare wakeup buttons.
    err_code = bsp_btn_ble_sleep_mode_prepare();
    APP_ERROR_CHECK(err_code);

    // Go to system-off mode (this function will not return; wakeup will cause a reset).
    err_code = sd_power_system_off();
    APP_ERROR_CHECK(err_code);
}

uint32_t ble_acc_change_notify(uint8_t acc_z_value)
{
		uint32_t err_code = NRF_SUCCESS;
    uint16_t len = sizeof(acc_z_value);
		

    // Send value if connected
    if (m_conn_handle != BLE_CONN_HANDLE_INVALID) {
        ble_gatts_hvx_params_t params;

        memset(&params, 0, sizeof(params));
        params.type = BLE_GATT_HVX_NOTIFICATION;
        params.handle = m_char_handles.value_handle;
        params.p_data = &acc_z_value;
        params.p_len = &len;
		
				//debug_printf("Notify gatt value\n\r");
        err_code = sd_ble_gatts_hvx(m_conn_handle, &params);
			
    }
    return err_code;
}

uint32_t ble_update_acc_value(uint8_t acc_z_value)
{
    uint32_t err_code = NRF_SUCCESS;
		uint16_t len = sizeof(acc_z_value);
	
		ble_gatts_value_t ble_gatt_value;
		ble_gatt_value.len = len;
	  ble_gatt_value.offset = 0;
		ble_gatt_value.p_value = &acc_z_value;
	
    // Send value if connected
		if (m_conn_handle != BLE_CONN_HANDLE_INVALID) 
		{
      //debug_printf("Set gatt value\n\r");
			err_code = sd_ble_gatts_value_set(m_conn_handle,m_char_handles.value_handle, &ble_gatt_value);
    }
	
		
    return err_code;
}


/**@brief Function for handling advertising events.
 *
 * @details This function will be called for advertising events which are passed to the application.
 *
 * @param[in] ble_adv_evt  Advertising event.
 */
static void on_adv_evt(ble_adv_evt_t ble_adv_evt)
{
    uint32_t err_code;

    switch (ble_adv_evt)
    {
        case BLE_ADV_EVT_FAST:
            err_code = bsp_indication_set(BSP_INDICATE_ADVERTISING);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_ADV_EVT_IDLE:
            sleep_mode_enter();
            break;

        default:
            break;
    }
}


/**@brief Function for handling the Application's BLE Stack events.
 *
 * @param[in] p_ble_evt   Bluetooth stack event.
 */
static void on_ble_evt(ble_evt_t * p_ble_evt)
{
    uint32_t err_code;
		char buf[20];
    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
            m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
            err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GAP_EVT_DISCONNECTED:
            m_conn_handle = BLE_CONN_HANDLE_INVALID;
            err_code = bsp_indication_set(BSP_INDICATE_IDLE);
            APP_ERROR_CHECK(err_code);

            err_code = bsp_indication_set(BSP_INDICATE_ALERT_OFF);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GATTS_EVT_WRITE:
            if ((p_ble_evt->evt.gatts_evt.params.write.handle == m_char_handles.cccd_handle) &&
                (p_ble_evt->evt.gatts_evt.params.write.len == 2))
            {
                err_code = bsp_indication_set(BSP_INDICATE_RCV_OK);
                APP_ERROR_CHECK(err_code);
            }
            break;
				
				case BLE_ERROR_GATTS_INVALID_ATTR_TYPE:
					debug_printf("BLE_ERROR_GATTS_INVALID_ATTR_TYPE\n\r");
					break;
				
				case BLE_ERROR_INVALID_CONN_HANDLE:
					debug_printf("BLE_ERROR_INVALID_CONN_HANDLE");
					break;
				
        default:
            // No implementation needed.
				
				//sprintf(buf,"Error id: %d\r\n",p_ble_evt->header.evt_id);
				//debug_printf(buf);
            break;
    }
}



/**@brief Function for dispatching a BLE stack event to all modules with a BLE stack event handler.
 *
 * @details This function is called from the scheduler in the main loop after a BLE stack
 *          event has been received.
 *
 * @param[in] p_ble_evt   Bluetooth stack event.
 */
static void ble_evt_dispatch(ble_evt_t * p_ble_evt)
{
    dm_ble_evt_handler(p_ble_evt);
    bsp_btn_ble_on_ble_evt(p_ble_evt);
    on_ble_evt(p_ble_evt);
    ble_advertising_on_ble_evt(p_ble_evt);
}


/**@brief Function for dispatching a system event to interested modules.
 *
 * @details This function is called from the System event interrupt handler after a system
 *          event has been received.
 *
 * @param[in]   sys_evt   System stack event.
 */
static void sys_evt_dispatch(uint32_t sys_evt)
{
    pstorage_sys_event_handler(sys_evt);
    ble_advertising_on_sys_evt(sys_evt);
}


/**@brief Function for initializing the BLE stack.
 *
 * @details Initializes the SoftDevice and the BLE event interrupt.
 */
static void ble_stack_init(void)
{
    uint32_t err_code;

    // Initialize the SoftDevice handler module.
    SOFTDEVICE_HANDLER_INIT(NRF_CLOCK_LFCLKSRC_XTAL_20_PPM, NULL);

#if defined(S110) || defined(S130)
    // Enable BLE stack. 
    ble_enable_params_t ble_enable_params;
    memset(&ble_enable_params, 0, sizeof(ble_enable_params));
#ifdef S130
    ble_enable_params.gatts_enable_params.attr_tab_size   = BLE_GATTS_ATTR_TAB_SIZE_DEFAULT;
#endif
    ble_enable_params.gatts_enable_params.service_changed = IS_SRVC_CHANGED_CHARACT_PRESENT;
    err_code = sd_ble_enable(&ble_enable_params);
    APP_ERROR_CHECK(err_code);
#endif

    // Register with the SoftDevice handler module for BLE events.
    err_code = softdevice_ble_evt_handler_set(ble_evt_dispatch);
    APP_ERROR_CHECK(err_code);
    // Register with the SoftDevice handler module for BLE events.
    err_code = softdevice_sys_evt_handler_set(sys_evt_dispatch);
    APP_ERROR_CHECK(err_code);

}


/**@brief Function for handling events from the BSP module.
 *
 * @param[in]   event   Event generated by button press.
 */
void bsp_event_handler(bsp_event_t event)
{
    uint32_t err_code;
    switch (event)
    {
        case BSP_EVENT_SLEEP:
            sleep_mode_enter();
            break;

        case BSP_EVENT_DISCONNECT:
            err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            if (err_code != NRF_ERROR_INVALID_STATE)
            {
                APP_ERROR_CHECK(err_code);
            }
            break;

        case BSP_EVENT_KEY_0:
            if (m_conn_handle != BLE_CONN_HANDLE_INVALID)
            {
                static uint8_t value = 0;

                ble_gatts_hvx_params_t hvx_params;
                uint16_t               len = sizeof(uint8_t);

                value = (value == 0) ? 1 : 0;

                memset(&hvx_params, 0, sizeof(hvx_params));

                hvx_params.handle = m_char_handles.value_handle;
                hvx_params.type   = BLE_GATT_HVX_NOTIFICATION;
                hvx_params.offset = 0;
                hvx_params.p_len  = &len;
                hvx_params.p_data = &value;

                err_code = sd_ble_gatts_hvx(m_conn_handle, &hvx_params);
                APP_ERROR_CHECK(err_code);

                if (value == 0)
                {
                    err_code = bsp_indication_set(BSP_INDICATE_ALERT_OFF);
                    APP_ERROR_CHECK(err_code);
                }
                else
                {
                    err_code = bsp_indication_set(BSP_INDICATE_ALERT_3);
                    APP_ERROR_CHECK(err_code);
                }
            }
            break;

        default:
            break;
    }
}


/**@brief Function for initializing the Advertising functionality.
 */
static void advertising_init(void)
{
    uint32_t      err_code;
    ble_advdata_t advdata;

    // Build and set advertising data
    memset(&advdata, 0, sizeof(advdata));

    advdata.name_type               = BLE_ADVDATA_FULL_NAME;
    advdata.include_appearance      = true;
    advdata.flags                   = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;

    ble_adv_modes_config_t options = {0};
    options.ble_adv_fast_enabled  = BLE_ADV_FAST_ENABLED;
    options.ble_adv_fast_interval = APP_ADV_INTERVAL;
    options.ble_adv_fast_timeout  = APP_ADV_TIMEOUT_IN_SECONDS;

    err_code = ble_advertising_init(&advdata, NULL, &options, on_adv_evt, NULL);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling the Bond Manager events.
 *
 * @param[in]   p_evt   Data associated to the bond manager event.
 */
static uint32_t device_manager_evt_handler(dm_handle_t const * p_handle,
                                           dm_event_t const  * p_event,
                                           ret_code_t        event_result)
{   uint32_t err_code;

    switch(p_event->event_id)
    {
        case DM_EVT_CONNECTION:
            m_conn_handle = p_event->event_param.p_gap_param->conn_handle;
            err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
            APP_ERROR_CHECK(err_code);
            break;

        case DM_EVT_DISCONNECTION:
            m_conn_handle = BLE_CONN_HANDLE_INVALID;
            break;
    }

    return NRF_SUCCESS;
}


/**@brief Function for the Device Manager initialization.
 *
 * @param[in] erase_bonds  Indicates whether bonding information should be cleared from
 *                         persistent storage during initialization of the Device Manager.
 */
static void device_manager_init(bool erase_bonds)
{
    uint32_t               err_code;
    dm_init_param_t        init_param = {.clear_persistent_data = erase_bonds};
    dm_application_param_t register_param;

    // Initialize persistent storage module.
    err_code = pstorage_init();
    APP_ERROR_CHECK(err_code);

    err_code = dm_init(&init_param);
    APP_ERROR_CHECK(err_code);

    register_param.sec_param.bond         = SEC_PARAM_BOND;
    register_param.sec_param.mitm         = SEC_PARAM_MITM;
    register_param.sec_param.io_caps      = SEC_PARAM_IO_CAPABILITIES;
    register_param.sec_param.oob          = SEC_PARAM_OOB;
    register_param.sec_param.min_key_size = SEC_PARAM_MIN_KEY_SIZE;
    register_param.sec_param.max_key_size = SEC_PARAM_MAX_KEY_SIZE;
    register_param.evt_handler            = device_manager_evt_handler;
    register_param.service_type           = DM_PROTOCOL_CNTXT_GATT_SRVR_ID;

    err_code = dm_register(&m_app_handle, &register_param);
    APP_ERROR_CHECK(err_code);
}


/**@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)
{
    bsp_event_t startup_event;

    uint32_t err_code = bsp_init(BSP_INIT_LED | BSP_INIT_BUTTONS,
                                 APP_TIMER_TICKS(100, APP_TIMER_PRESCALER),
                                 bsp_event_handler);
    APP_ERROR_CHECK(err_code);

    err_code = bsp_btn_ble_init(NULL, &startup_event);
    APP_ERROR_CHECK(err_code);

    *p_erase_bonds = (startup_event == BSP_EVENT_CLEAR_BONDING_DATA);
}


/**@brief Function for the Power manager.
 */
static void power_manage(void)
{
    uint32_t err_code = sd_app_evt_wait();
    APP_ERROR_CHECK(err_code);
}

static void data_send(uint8_t z_value)
{
    uint32_t err_code;
            
    if (!m_is_sending_data)
    {
        return;
    }
    
		err_code = ble_acc_change_notify(z_value);					// send with notification
		APP_ERROR_CHECK(err_code);
		m_is_sending_data = !m_is_sending_data;	
    /**while (1)
    {
        //err_code = ble_update_acc_value(z_value);		
				err_code = ble_acc_change_notify(z_value);					// send with notification
        if (err_code != NRF_SUCCESS &&
            err_code != BLE_ERROR_INVALID_CONN_HANDLE &&
            err_code != NRF_ERROR_INVALID_STATE &&
            err_code != BLE_ERROR_NO_TX_BUFFERS)
        {
            char buf[20];
						sprintf(buf,"Error code: %d\n\r",err_code);
						debug_printf(buf);
						APP_ERROR_CHECK(err_code);
						debug_printf("Past ERROR_CHECK\n\r");
        }
        else
					m_is_sending_data = !m_is_sending_data;					
					break;
    }**/
}



/**@brief Function for application main entry.
 */
int main(void)
{
    uint32_t err_code;
    bool erase_bonds;
		char buffer[20];
	  // Acc values for UART
		//int16_t x_value, y_value, z_value = 0;
		int8_t z = 0;
		int8_t last_z = 0;
	
		// Start UART 
		simple_uart_config(0, PIN_UART_TX, 0, PIN_UART_RX,false);
		
		debug_printf("************ Main start **************\n\r");
	
    // Initialize.
    APP_TIMER_INIT(APP_TIMER_PRESCALER, APP_TIMER_MAX_TIMERS, APP_TIMER_OP_QUEUE_SIZE, NULL);
    buttons_leds_init(&erase_bonds);
    ble_stack_init();
    device_manager_init(erase_bonds);
    gap_params_init();
    advertising_init();
    services_init();
		acc_service_init();
		
		
	
		acc_init();
	  //debug_printf("Acc init done.\n\r");

    err_code = ble_advertising_start(BLE_ADV_MODE_FAST);
    APP_ERROR_CHECK(err_code);
		
		//debug_printf("Reading accelerometer: \n\r");
		
    for (;;)
    {
        power_manage();
				 z = acc_get_z8();
				//z = 5;
				if(abs(z-last_z)>=5)
				{
					//sprintf(buffer, "Z changed from %d to: %d\n\r",last_z,z);
					//debug_printf(buffer);
					last_z=z;
					
					m_is_sending_data = !m_is_sending_data;
          if (m_is_sending_data)
					{
						data_send(z);
						//debug_printf("sending\n");
					}
										
				}					
				
				//debug_printf("\n\r");
				//debug_printf("Delay\n\r");
			  nrf_delay_ms(1000);
			
			
			
    }
}
