/*
 * Filename: project_zero.c
 *
 * Description: This is the simple_central example modified to receive
 * data over BLE at a high throughput.
 *
 *
 * Copyright (C) 2015-2017 Texas Instruments Incorporated - http://www.ti.com/
 *
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
 Generated by:
 BDS version: 1.1.3139.0
 Plugin:      Texas Instruments BLE SDK GATT Server plugin 1.0.9
 Time:        Mon Jan 08 2018 13:50:01 GMT-07:00
 *****************************************************************************/

/*
 * INCLUDES
 */
#include <string.h>

//#define xdc_runtime_Log_DISABLE_ALL 1  // Add to disable logs from this file

#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/knl/Queue.h>
#include <ti/sysbios/knl/Clock.h>

#include <ti/drivers/PIN.h>
#include <ti/drivers/PWM.h>
#include <ti/drivers/GPIO.h>
#include "C:\ti\simplelink_cc2640r2_sdk_1_40_00_45\source\ti\drivers\gpio.h"

#include <xdc/runtime/Log.h>
#include <xdc/runtime/Diags.h>

/* This Header file contains all BLE API and icall structure definition */
#include "icall_ble_api.h"
#include <icall.h>

#include <inc/hw_types.h>

#include <osal_snv.h>
#include <peripheral.h>

#include "util.h"
#include "Board.h"
#include "project_zero.h"

//SDI UART
#include "sdi_task.h"
#include "sdi_tl_uart.h"

// Bluetooth Developer Studio services
#include "notification.h"
/*
#include "device_information.h"
#include "spectrum_data.h"
#include "request.h"
*/
#include "adc.h"

//Metrohm Processing Functions
#include "Parse.h"
#include "EEPROM.h"
#include "NVSManager.h"

#include "Laser.h"
#include "Raster.h"
#include "SRI_Global.h"
#include "ADC.h"
#include "EEPROM.h"
#include "i2c.h"
#include "PCA9536.h"
#include "LP5521.h"
#include "IMX290.h"
#include "Si7055.h"
#include "Interlocks.h"
#include "util.h"


/*********************************************************************
 * CONSTANTS
 */
// Advertising interval when device is discoverable (units of 625us, 160=100ms)
#define DEFAULT_ADVERTISING_INTERVAL          160

// Limited discoverable mode advertises for 30.72s, and then stops
// General discoverable mode advertises indefinitely
#define DEFAULT_DISCOVERABLE_MODE             GAP_ADTYPE_FLAGS_GENERAL

// Default pass-code used for pairing.
#define DEFAULT_PASSCODE                      000000

// Task configuration
#define PRZ_TASK_PRIORITY                     1

#ifndef PRZ_TASK_STACK_SIZE
#define PRZ_TASK_STACK_SIZE                   1024
#endif

// Internal Events for RTOS application
#define PRZ_ICALL_EVT                         ICALL_MSG_EVENT_ID  // Event_Id_31
#define PRZ_QUEUE_EVT                         UTIL_QUEUE_EVENT_ID // Event_Id_30
#define PRZ_STATE_CHANGE_EVT                  Event_Id_00
#define PRZ_CHAR_CHANGE_EVT                   Event_Id_01
#define PRZ_PERIODIC_EVT                      Event_Id_02
#define PRZ_APP_MSG_EVT                       Event_Id_03
#define PRZ_CONN_EVT_END_EVT                  Event_Id_30
#define SBP_UART_QUEUE_EVT                    Event_Id_04

#define PRZ_ALL_EVENTS                       (PRZ_ICALL_EVT        | \
                                              PRZ_QUEUE_EVT        | \
                                              PRZ_STATE_CHANGE_EVT | \
                                              PRZ_CHAR_CHANGE_EVT  | \
                                              PRZ_PERIODIC_EVT     | \
                                              PRZ_APP_MSG_EVT      | \
                                              SBP_UART_QUEUE_EVT   |\
                                              PRZ_CONN_EVT_END_EVT)

/*********************************************************************
 * TYPEDEFS
 */

// Struct for messages sent to the application task
typedef struct
{
  Queue_Elem       _elem;
  app_msg_types_t  type;
  uint8_t          pdu[];
} app_msg_t;

// RTOS queue for profile/app messages.
typedef struct _queueRec_
{
    Queue_Elem _elem;          // queue element
    uint8_t *pData;            // pointer to app data
} queueRec_t;

// App event passed from profiles.
typedef struct
{
    appEvtHdr_t hdr;  // event header.
} sbpEvt_t;

// App event passed from profiles.
typedef struct
{
    uint8_t event;  // Type of event
    uint8_t *pData;  // New data
    uint8_t length; // New status
} sbpUARTEvt_t;

// Struct for messages about characteristic data
typedef struct
{
  uint16_t svcUUID; // UUID of the service
  uint16_t dataLen; //
  uint8_t  paramID; // Index of the characteristic
  uint8_t  data[];  // Flexible array member, extended to malloc - sizeof(.)
} char_data_t;

// Struct for message about sending/requesting passcode from peer.
typedef struct
{
  uint16_t connHandle;
  uint8_t  uiInputs;
  uint8_t  uiOutputs;
  uint32   numComparison;
} passcode_req_t;

// Struct for message about button state
typedef struct
{
  PIN_Id   pinId;
  uint8_t  state;
} button_state_t;


/*********************************************************************
 * LOCAL VARIABLES
 */

#define BS_BATTERY_LEVEL_ID                 0
#define BS_BATTERY_LEVEL_LEN                1
#define BS_BATTERY_LEVEL_LEN_MIN            1

uint16 currentMTUSize;

// Entity ID globally used to check for source and/or destination of messages
static ICall_EntityID selfEntity;

// Sync handle globally used to post events to the application thread
static ICall_SyncHandle syncEvent;

// Queue object used for application messages.
static Queue_Struct applicationMsgQ;
static Queue_Handle hApplicationMsgQ;

// Queue object used for UART messages
static Queue_Struct appUARTMsg;
static Queue_Handle appUARTMsgQueue;

// Value to write
//static const uint8_t charVal = 0x41;

// Clock structs for periodic notification example
static Clock_Struct n_Battery_Level_clock;

static Clock_Struct User_100mS_Periodic_clock;

static Clock_Struct User_1S_Periodic_clock;

// Task configuration
Task_Struct przTask;
Char przTaskStack[PRZ_TASK_STACK_SIZE];


// GAP - SCAN RSP data (max size = 31 bytes)
static uint8_t scanRspData[] =
{
  // No scan response data provided.
  0x00 // Placeholder to keep the compiler happy.
};

// GAP - Advertisement data (max size = 31 bytes, though this is
// best kept short to conserve power while advertisting)
static uint8_t advertData[] =
{
  // Flags; this sets the device to use limited discoverable
  // mode (advertises for 30 seconds at a time) or general
  // discoverable mode (advertises indefinitely), depending
  // on the DEFAULT_DISCOVERY_MODE define.
  0x02,   // length of this data
  GAP_ADTYPE_FLAGS,
  DEFAULT_DISCOVERABLE_MODE | GAP_ADTYPE_FLAGS_BREDR_NOT_SUPPORTED,

  // complete name
  10,
  GAP_ADTYPE_LOCAL_NAME_COMPLETE,
  'M', 'i', 'n', 'i', 'R', 'a', 'm', 'a', 'n',

};

// GAP GATT Attributes
static uint8_t attDeviceName[GAP_DEVICE_NAME_LEN] = "MiniRaman";

// Globals used for ATT Response retransmission
static gattMsgEvent_t *pAttRsp = NULL;
static uint8_t rspTxRetry = 0;


/*********************************************************************
 * LOCAL FUNCTIONS
 */

static void ProjectZero_init( void );
static void ProjectZero_taskFxn(UArg a0, UArg a1);

static void user_processApplicationMessage(app_msg_t *pMsg);
static uint8_t ProjectZero_processStackMsg(ICall_Hdr *pMsg);
static uint8_t ProjectZero_processGATTMsg(gattMsgEvent_t *pMsg);

static void ProjectZero_sendAttRsp(void);
static uint8_t ProjectZero_processGATTMsg(gattMsgEvent_t *pMsg);
static void ProjectZero_freeAttRsp(uint8_t status);

void ProjectZero_enqueueUARTMsg(uint8_t event, uint8_t *data, uint8_t len);

void SDITask_RXerror(uint8_t event, uint8_t *data, uint8_t len);
static void user_processGapStateChangeEvt(gaprole_States_t newState);
static void user_gapStateChangeCB(gaprole_States_t newState);
static void user_gapBondMgr_passcodeCB(uint8_t *deviceAddr, uint16_t connHandle,
                                       uint8_t uiInputs, uint8_t uiOutputs, uint32 numComparison);
static void user_gapBondMgr_pairStateCB(uint16_t connHandle, uint8_t state,
                                        uint8_t status);


// Generic callback handlers for value changes in services.
static void user_service_ValueChangeCB( uint16_t connHandle, uint16_t svcUuid, uint8_t paramID, uint8_t *pValue, uint16_t len );
static void user_service_CfgChangeCB( uint16_t connHandle, uint16_t svcUuid, uint8_t paramID, uint8_t *pValue, uint16_t len );

// Task context handlers for generated services.
static void user_Notification_ValueChangeHandler(char_data_t *pCharData);
static void user_Notification_CfgChangeHandler(char_data_t *pCharData);

/*static void user_DeviceInformation_ValueChangeHandler(char_data_t *pCharData);

static void user_SpectrumData_ValueChangeHandler(char_data_t *pCharData);

static void user_Request_ValueChangeHandler(char_data_t *pCharData);*/


// Callback handler(s) for the clock object(s) used to demonstrate notifications
static void user_Notification_clockSwiHandler(UArg paramID);

static void user_100mSPeriodic_clockSwiHandler(UArg paramID);
static void user_1SPeriodic_clockSwiHandler(UArg paramID);

// Task handler for sending notifications.
static void user_updateCharVal(char_data_t *pCharData);

// Utility functions
static void user_enqueueRawAppMsg(app_msg_types_t appMsgType, uint8_t *pData, uint16_t len );
static void user_enqueueCharDataMsg( app_msg_types_t appMsgType, uint16_t connHandle,
                                       uint16_t serviceUUID, uint8_t paramID,
                                       uint8_t *pValue, uint16_t len );

static char *Util_getLocalNameStr(const uint8_t *data);
static char *Util_convertArrayToHexString(uint8_t const *src, uint8_t src_len,
                                          uint8_t *dst, uint8_t dst_len);

/*********************************************************************
 * PROFILE CALLBACKS
 */

// GAP Role Callbacks
static gapRolesCBs_t user_gapRoleCBs =
{
  user_gapStateChangeCB     // Profile State Change Callbacks
};

// GAP Bond Manager Callbacks
static gapBondCBs_t user_bondMgrCBs =
{
  user_gapBondMgr_passcodeCB, // Passcode callback
  user_gapBondMgr_pairStateCB // Pairing / Bonding state Callback
};

/*
 * Callbacks in the user application for events originating from BLE services.
 */
// Service callback structure for registering our handlers with the service
// Notification callback handler.
// The type NotificationCBs_t is defined in Notification.h
static NotificationCBs_t user_NotificationCBs =
{
  .pfnChangeCb    = user_service_ValueChangeCB, // Characteristic value change callback handler
  .pfnCfgChangeCb = user_service_CfgChangeCB, // Noti/ind configuration callback handler
};

/*
// Device Information callback handler.
// The type Device_InformationCBs_t is defined in Device_Information.h
static DeviceInformationCBs_t user_Device_InformationCBs =
{
  .pfnChangeCb    = user_service_ValueChangeCB, // Characteristic value change callback handler
  .pfnCfgChangeCb = NULL, // No notification-/indication enabled chars in Device Information
};

// Spectrum Data callback handler.
// The type Spectrum_DataCBs_t is defined in Spectrum_Data.h
static SpectrumDataCBs_t user_Spectrum_DataCBs =
{
  .pfnChangeCb    = user_service_ValueChangeCB, // Characteristic value change callback handler
  .pfnCfgChangeCb = NULL, // No notification-/indication enabled chars in Spectrum Data
};

// Request callback handler.
// The type RequestCBs_t is defined in Request.h
static RequestCBs_t user_RequestCBs =
{
  .pfnChangeCb    = user_service_ValueChangeCB, // Characteristic value change callback handler
  .pfnCfgChangeCb = NULL, // No notification-/indication enabled chars in Request
};
*/



/*********************************************************************
 * PUBLIC FUNCTIONS
 */

/*
 * @brief   Task creation function for the user task.
 *
 * @param   None.
 *
 * @return  None.
 */
void ProjectZero_createTask(void)
{
  Task_Params taskParams;

  // Configure task
  Task_Params_init(&taskParams);
  taskParams.stack = przTaskStack;
  taskParams.stackSize = PRZ_TASK_STACK_SIZE;
  taskParams.priority = PRZ_TASK_PRIORITY;

  Task_construct(&przTask, ProjectZero_taskFxn, &taskParams, NULL);
}


void Mini_Init(void)
{
    status.safe = TRUE;
    /* Call driver init functions. */
    PWM_init();
    SPI_init();
    i2c_Init();
    GPIO_init();
    GPIO_setDio(MINIRAMAN_EN_3V3);
    usleep(30000);
    GPIO_setDio(MINIRAMAN_TE_PWR_EN);
    usleep(30000);
    /* PCA9536 I/O expander provides enables for 2v9, 1v2, 1v8 and Laser Cooling. */
    PCA9536_Init();
    Laser_ADC_Init();
    //NVS_Init();
    EEPROM_Init();  // Do this before initializing subsystems like Raster and Laser.
    Raster_Init();
    Laser_Init();
    lp5521_init();
    /* IMX290 Init() must occur after PCA9536 Init() */
    IMX290_Init();
    /* This must occur after the 3V3 is up or interrupts will be continuous. */
    CC2640R2_MiniRaman_InitInterlocks();
    gen_gauss_coeffs(eeprom_base.eeprom.GaussSigma, eeprom_base.eeprom.GaussThreshold);
}

/*
 * @brief   Called before the task loop and contains application-specific
 *          initialization of the BLE stack, hardware setup, power-state
 *          notification if used, and BLE profile/service initialization.
 *
 * @param   None.
 *
 * @return  None.
 */
static void ProjectZero_init(void)
{
    /* Mini_Init must occur after BIOS_start() because it uses usleep() */
    Mini_Init();
    // ******************************************************************
    // NO STACK API CALLS CAN OCCUR BEFORE THIS CALL TO ICall_registerApp
    // ******************************************************************
    // Register the current thread as an ICall dispatcher application
    // so that the application can send and receive messages via ICall to Stack.
    ICall_registerApp(&selfEntity, &syncEvent);

  //Log_Info0("Initializing the user task, hardware, BLE stack and services.");

    // Initialize queue for application messages.
    // Note: Used to transfer control to application thread from e.g. interrupts.
    Queue_construct(&applicationMsgQ, NULL);
    hApplicationMsgQ = Queue_handle(&applicationMsgQ);
    appUARTMsgQueue = Util_constructQueue(&appUARTMsg);

  // ******************************************************************
  // Hardware initialization
  // ******************************************************************

    // ******************************************************************
    // Initialization of clock objects used for notifiable characterisics
    // ******************************************************************
    Clock_Params clockParams;
    Clock_Params_init(&clockParams);
    clockParams.period = 5000 * (1000 / Clock_tickPeriod); // 5000ms period

  // Clock struct initialization for periodic notification example
  // Clock callbacks only have one parameter, so make one callback handler per service
  // and one Clock Struct per noti/ind characteristic.
  clockParams.arg = N_BATTERY_LEVEL_ID;
  Clock_construct(&n_Battery_Level_clock,
                  user_Notification_clockSwiHandler,
                  0,
                  &clockParams);

    clockParams.arg = NULL;
    clockParams.period = 100 * (1000 / Clock_tickPeriod); // 100ms period
    Clock_construct(&User_100mS_Periodic_clock,
                    user_100mSPeriodic_clockSwiHandler, 0, &clockParams);
    Clock_start((Clock_Handle) &User_100mS_Periodic_clock);

    clockParams.period = 1000 * (1000 / Clock_tickPeriod); // 1 Second period
    Clock_construct(&User_1S_Periodic_clock,
                    user_1SPeriodic_clockSwiHandler, 0, &clockParams);
    Clock_start((Clock_Handle) &User_1S_Periodic_clock);
    // ******************************************************************
    // BLE Stack initialization
    // ******************************************************************

  // Setup the GAP Peripheral Role Profile
  uint8_t initialAdvertEnable = TRUE;  // Advertise on power-up

  // By setting this to zero, the device will go into the waiting state after
  // being discoverable. Otherwise wait this long [ms] before advertising again.
  uint16_t advertOffTime = 0; // miliseconds

  // Set advertisement enabled.
  GAPRole_SetParameter(GAPROLE_ADVERT_ENABLED, sizeof(uint8_t),
                       &initialAdvertEnable);

  // Configure the wait-time before restarting advertisement automatically
  GAPRole_SetParameter(GAPROLE_ADVERT_OFF_TIME, sizeof(uint16_t),
                       &advertOffTime);

  // Initialize Scan Response data
  GAPRole_SetParameter(GAPROLE_SCAN_RSP_DATA, sizeof(scanRspData), scanRspData);

  // Initialize Advertisement data
  GAPRole_SetParameter(GAPROLE_ADVERT_DATA, sizeof(advertData), advertData);

  //Log_Info1("Name in advertData array: \x1b[33m%s\x1b[0m",
  //          (IArg)Util_getLocalNameStr(advertData));

  // Set advertising interval
  uint16_t advInt = DEFAULT_ADVERTISING_INTERVAL;

  GAP_SetParamValue(TGAP_LIM_DISC_ADV_INT_MIN, advInt);
  GAP_SetParamValue(TGAP_LIM_DISC_ADV_INT_MAX, advInt);
  GAP_SetParamValue(TGAP_GEN_DISC_ADV_INT_MIN, advInt);
  GAP_SetParamValue(TGAP_GEN_DISC_ADV_INT_MAX, advInt);

  // Set duration of advertisement before stopping in Limited adv mode.
  GAP_SetParamValue(TGAP_LIM_ADV_TIMEOUT, 30); // Seconds

  // ******************************************************************
  // BLE Bond Manager initialization
  // ******************************************************************
  uint32_t passkey = 0; // passkey "000000"
  uint8_t pairMode = GAPBOND_PAIRING_MODE_WAIT_FOR_REQ;
  uint8_t mitm = TRUE;
  uint8_t ioCap = GAPBOND_IO_CAP_DISPLAY_ONLY;
  uint8_t bonding = TRUE;

  GAPBondMgr_SetParameter(GAPBOND_DEFAULT_PASSCODE, sizeof(uint32_t),
                          &passkey);
  GAPBondMgr_SetParameter(GAPBOND_PAIRING_MODE, sizeof(uint8_t), &pairMode);
  GAPBondMgr_SetParameter(GAPBOND_MITM_PROTECTION, sizeof(uint8_t), &mitm);
  GAPBondMgr_SetParameter(GAPBOND_IO_CAPABILITIES, sizeof(uint8_t), &ioCap);
  GAPBondMgr_SetParameter(GAPBOND_BONDING_ENABLED, sizeof(uint8_t), &bonding);

  // ******************************************************************
  // BLE Service initialization
  // ******************************************************************

  // Add services to GATT server
  GGS_AddService(GATT_ALL_SERVICES);           // GAP
  GATTServApp_AddService(GATT_ALL_SERVICES);   // GATT attributes
  //DevInfo_AddService();                        // Device Information Service

  // Set the device name characteristic in the GAP Profile
  GGS_SetParameter(GGS_DEVICE_NAME_ATT, GAP_DEVICE_NAME_LEN, attDeviceName);

// Add services to GATT server and give ID of this task for Indication acks.
  Notification_AddService( selfEntity );
/*  DeviceInformation_AddService( selfEntity );
  SpectrumData_AddService( selfEntity );
  Request_AddService( selfEntity );*/

  // Register callbacks with the generated services that
  // can generate events (writes received) to the application
  Notification_RegisterAppCBs( &user_NotificationCBs );
/*  DeviceInformation_RegisterAppCBs( &user_Device_InformationCBs );
  SpectrumData_RegisterAppCBs( &user_Spectrum_DataCBs );
  Request_RegisterAppCBs( &user_RequestCBs );*/
  // Placeholder variable for characteristic intialization
  uint8_t someVal[20] = {0};

  // Initalization of characteristics in Notification that can provide data
  // to a peer device over the air.
  Notification_SetParameter(N_BATTERY_LEVEL_ID, N_BATTERY_LEVEL_LEN, &someVal);

  // Initalization of characteristics in Device_Information that can provide data
  // to a peer device over the air.
 /* DeviceInformation_SetParameter(DI_SERIAL_NUMBER_STRING_ID, DI_SERIAL_NUMBER_STRING_LEN, &someVal);
  DeviceInformation_SetParameter(DI_FIRMWARE_REVISION_STRING_ID, DI_FIRMWARE_REVISION_STRING_LEN, &someVal);
  DeviceInformation_SetParameter(DI_RTC_ID, DI_RTC_LEN, &someVal);
  DeviceInformation_SetParameter(DI_WAVELENGTH_ID, DI_WAVELENGTH_LEN, &someVal);
  DeviceInformation_SetParameter(DI_RANGE_ID, DI_RANGE_LEN, &someVal);
  DeviceInformation_SetParameter(DI_LASER_POWER_LEVELS_ID, DI_LASER_POWER_LEVELS_LEN, &someVal);
  DeviceInformation_SetParameter(DI_XCALCOEFFICIENTS_ID, DI_XCALCOEFFICIENTS_LEN, &someVal);*/

  // Initalization of characteristics in Spectrum_Data that can provide data
  // to a peer device over the air.
/*  SpectrumData_SetParameter(SD_TX_DATA_ID, SD_TX_DATA_LEN, &someVal);
  SpectrumData_SetParameter(SD_TX_FORMAT_ID, SD_TX_FORMAT_LEN, &someVal);
  SpectrumData_SetParameter(SD_TX_READ_ID, SD_TX_READ_LEN, &someVal);*/

  // Initalization of characteristics in Request that can provide data
  // to a peer device over the air.
/*  Request_SetParameter(R_REQUEST_SPECTRUM_ACQUISITION_ID, R_REQUEST_SPECTRUM_ACQUISITION_LEN, &someVal);
  Request_SetParameter(R_INTEGRATION_TIME_ID, R_INTEGRATION_TIME_LEN, &someVal);
  Request_SetParameter(R_LASER_POWER_ID, R_LASER_POWER_LEN, &someVal);*/



  // Start the stack in Peripheral mode.
  VOID GAPRole_StartDevice(&user_gapRoleCBs);

  // Start Bond Manager
  VOID GAPBondMgr_Register(&user_bondMgrCBs);

  // Register with GAP for HCI/Host messages
  GAP_RegisterForMsgs(selfEntity);

    // Register for GATT local events and ATT Responses pending for transmission
    GATT_RegisterForMsgs(selfEntity);

    //Util_InitSemaPhore();

    //Register to receive UART messages
    SDITask_registerIncomingRXEventAppCB(ProjectZero_enqueueUARTMsg);

    SDITLUART_registerIncomingRXErrorStatusAppCB(SDITask_RXerror);

    //SDITask_init();
}


/*
 * @brief   Application task entry point.
 *
 *          Invoked by TI-RTOS when BIOS_start is called. Calls an init function
 *          and enters an infinite loop waiting for messages.
 *
 *          Messages can be either directly from the BLE stack or from user code
 *          like Hardware Interrupt (Hwi) or a callback function.
 *
 *          The reason for sending messages to this task from e.g. Hwi's is that
 *          some RTOS and Stack APIs are not available in callbacks and so the
 *          actions that may need to be taken is dispatched to this Task.
 *
 * @param   a0, a1 - not used.
 *
 * @return  None.
 */
static void ProjectZero_taskFxn(UArg a0, UArg a1)
{
  // Initialize application
  ProjectZero_init();
  interlockPinIntCb(NULL);
  SDITask_Printf("\nEEPROM Size: %d \n>",EEPROM_LEN);
  SDITask_Printf("\nReady\n>");
  // Application main loop
  for (;;)
  {
    uint32_t events;

    // Waits for an event to be posted associated with the calling thread.
    // Note that an event associated with a thread is posted when a
    // message is queued to the message receive queue of the thread
    events = Event_pend(syncEvent, Event_Id_NONE, PRZ_ALL_EVENTS,
                        ICALL_TIMEOUT_FOREVER);

    if (events)
    {
      ICall_EntityID dest;
      ICall_ServiceEnum src;
      ICall_HciExtEvt *pMsg = NULL;

      // Check if we got a signal because of a stack message
      if (ICall_fetchServiceMsg(&src, &dest,
                                (void **)&pMsg) == ICALL_ERRNO_SUCCESS)
      {
        uint8 safeToDealloc = TRUE;

        if ((src == ICALL_SERVICE_CLASS_BLE) && (dest == selfEntity))
        {
          ICall_Stack_Event *pEvt = (ICall_Stack_Event *)pMsg;

          // Check for event flags received (event signature 0xffff)
          if (pEvt->signature == 0xffff)
          {
            // Event received when a connection event is completed
            if (pEvt->event_flag & PRZ_CONN_EVT_END_EVT)
            {
              // Try to retransmit pending ATT Response (if any)
              ProjectZero_sendAttRsp();
            }
          }
          else // It's a message from the stack and not an event.
          {
            // Process inter-task message
            safeToDealloc = ProjectZero_processStackMsg((ICall_Hdr *)pMsg);
          }
        }

        if (pMsg && safeToDealloc)
        {
          ICall_freeMsg(pMsg);
        }
      }
        // If RTOS queue is not empty, process app message.
        if (events & SBP_UART_QUEUE_EVT)
        {
            // If RTOS queue is not empty, process app message.
            if (!Queue_empty(appUARTMsgQueue))
            {
                //Get the message at the front of the queue but still keep it in the queue
                queueRec_t *pRec = Queue_head(appUARTMsgQueue);
                sbpUARTEvt_t *pMsg = (sbpUARTEvt_t *) pRec->pData;

                if (pMsg)
                {
                    switch (pMsg->event)
                    {
                    case SBP_UART_DATA_EVT:
                    {
                        //SDITask_sendToUART(pMsg->pData, sizeof(pMsg->pData)/sizeof(uint8));
                        for(int i = 0; i < pMsg->length; i++)
                        {
                            process_incoming_character(pMsg->pData[i]);
                        }

                        //Remove from queue
                        Util_dequeueMsg(appUARTMsgQueue);

                        //Deallocate data payload being transmitted.
                        ICall_freeMsg(pMsg->pData);
                        // Free the space from the message.
                        ICall_free(pMsg);

                        if (!Queue_empty(appUARTMsgQueue))
                        {
                            // Wake up the application to flush out any remaining UART data in the queue.
                            Event_post(syncEvent, SBP_UART_QUEUE_EVT); // SBP_UART_QUEUE_EVT);
                        }
                        break;
                    }
                    default:
                        break;
                    }
                }
            }
        }

        // Process messages sent from another task or another context.
        while (!Queue_empty(hApplicationMsgQ))
        {
            app_msg_t *pMsg = Queue_dequeue(hApplicationMsgQ);

            // Process application-layer message probably sent from ourselves.
            user_processApplicationMessage(pMsg);

            // Free the received message.
            ICall_free(pMsg);
        }
    }  /* end if (events) */
  }  /* end for ( ;;) */
}


/*
 * @brief   Handle application messages
 *
 *          These are messages not from the BLE stack, but from the
 *          application itself.
 *
 *          For example, in a Software Interrupt (Swi) it is not possible to
 *          call any BLE APIs, so instead the Swi function must send a message
 *          to the application Task for processing in Task context.
 *
 * @param   pMsg  Pointer to the message of type app_msg_t.
 *
 * @return  None.
 */
static void user_processApplicationMessage(app_msg_t *pMsg)
{
        char_data_t *pCharData = (char_data_t *)pMsg->pdu;
        static uint8_t AmbientTempState = 0;

  switch (pMsg->type)
  {
          case APP_MSG_SERVICE_WRITE: /* Message about received value write */
            /* Call different handler per service */
            switch(pCharData->svcUUID) {
              case NOTIFICATION_SERV_UUID:
                user_Notification_ValueChangeHandler(pCharData);
                break;
              /*case DEVICE_INFORMATION_SERV_UUID:
                user_DeviceInformation_ValueChangeHandler(pCharData);
                break;
              case SPECTRUM_DATA_SERV_UUID:
                user_SpectrumData_ValueChangeHandler(pCharData);
                break;
              case REQUEST_SERV_UUID:
                user_Request_ValueChangeHandler(pCharData);
                break;*/
            }
            break;

          case APP_MSG_SERVICE_CFG: /* Message about received CCCD write */
            /* Call different handler per service */
            switch(pCharData->svcUUID) {
              case NOTIFICATION_SERV_UUID:
                user_Notification_CfgChangeHandler(pCharData);
                break;
            }
            break;

    case APP_MSG_UPDATE_CHARVAL: /* Message to self from to update a value */
        user_updateCharVal(pCharData);
        break;

    case APP_MSG_GAP_STATE_CHANGE: /* Message that GAP state changed  */
      user_processGapStateChangeEvt( *(gaprole_States_t *)pMsg->pdu );
      break;

    case APP_MSG_SEND_PASSCODE: /* Message about pairing PIN request */
      {
        passcode_req_t *pReq = (passcode_req_t *)pMsg->pdu;
        //Log_Info2("BondMgr Requested passcode. We are %s passcode %06d",
        //          (IArg)(pReq->uiInputs?"Sending":"Displaying"),
        //          DEFAULT_PASSCODE);
        // Send passcode response.
        GAPBondMgr_PasscodeRsp(pReq->connHandle, SUCCESS, DEFAULT_PASSCODE);
    }
        break;

    case APP_MSG_100MS_UPDATE: /* Message to self from to update a value */
        status.LD_Temp = getLaserTemp();
        break;

    case APP_MSG_1S_UPDATE: /* Message to self from to update a value */
        if (AmbientTempState == 0)
        {
            Si7055_Start_Temperature_No_Hold();
            AmbientTempState = 1;
        }
        else
        {
            status.ambientTemp = Si7055_Read_Temperature();
            AmbientTempState = 0;
        }
        break;

    case APP_MSG_INTERLOCK_INTR: /* Message to self from to update interlock status */
        if (status.Interlock1 == SAFE)  lp55xx_write(LP5521_REG_R_PWM, 0x20);
        else  lp55xx_write(LP5521_REG_R_PWM, 0x00);
        if (status.Interlock2 == SAFE)  lp55xx_write(LP5521_REG_G_PWM, 0x2F);
        else  lp55xx_write(LP5521_REG_G_PWM, 0x00);
        if (status.Interlock3 == SAFE)  lp55xx_write(LP5521_REG_B_PWM, 0x28);
        else  lp55xx_write(LP5521_REG_B_PWM, 0x00);
        break;
    }
}


/******************************************************************************
 *****************************************************************************
 *
 *  Handlers of system/application events deferred to the user Task context.
 *  Invoked from the application Task function above.
 *
 *  Further down you can find the callback handler section containing the
 *  functions that defer their actions via messages to the application task.
 *
 ****************************************************************************
 *****************************************************************************/


/*
 * @brief   Process a pending GAP Role state change event.
 *
 * @param   newState - new state
 *
 * @return  None.
 */
static void user_processGapStateChangeEvt(gaprole_States_t newState)
{
  switch ( newState )
  {
    case GAPROLE_STARTED:
      {
        uint8_t ownAddress[B_ADDR_LEN];
        /*uint8_t systemId[DEVINFO_SYSTEM_ID_LEN];

        GAPRole_GetParameter(GAPROLE_BD_ADDR, ownAddress);

        // use 6 bytes of device address for 8 bytes of system ID value
        systemId[0] = ownAddress[0];
        systemId[1] = ownAddress[1];
        systemId[2] = ownAddress[2];

        // set middle bytes to zero
        systemId[4] = 0x00;
        systemId[3] = 0x00;

        // shift three bytes up
        systemId[7] = ownAddress[5];
        systemId[6] = ownAddress[4];
        systemId[5] = ownAddress[3];

        DevInfo_SetParameter(DEVINFO_SYSTEM_ID, DEVINFO_SYSTEM_ID_LEN, systemId);*/

        // Display device address
        char *cstr_ownAddress = Util_convertBdAddr2Str(ownAddress);
        //Log_Info1("GAP is started. Our address: \x1b[32m%s\x1b[0m", (IArg)cstr_ownAddress);
      }
      break;

    case GAPROLE_ADVERTISING:
      //Log_Info0("Advertising");
      break;

    case GAPROLE_CONNECTED:
      {
        uint8_t peerAddress[B_ADDR_LEN];

        GAPRole_GetParameter(GAPROLE_CONN_BD_ADDR, peerAddress);

        char *cstr_peerAddress = Util_convertBdAddr2Str(peerAddress);
        //Log_Info1("Connected. Peer address: \x1b[32m%s\x1b[0m", (IArg)cstr_peerAddress);
       }
      break;

    case GAPROLE_CONNECTED_ADV:
      //Log_Info0("Connected and advertising");
      break;

    case GAPROLE_WAITING:

        // Turn off periodic clocks for ind/noti demo
        Clock_stop((Clock_Handle) &n_Battery_Level_clock);
        //Log_Info0("Disconnected / Idle");
        break;

    case GAPROLE_WAITING_AFTER_TIMEOUT:

        // Turn off periodic clocks for ind/noti demo
        Clock_stop((Clock_Handle) &n_Battery_Level_clock);
        //Log_Info0("Connection timed out");
        break;

    case GAPROLE_ERROR:
      //Log_Info0("Error");
      break;

    default:
      break;
  }
}

/*
 * @brief   Handle a write request sent from a peer device.
 *
 *          Invoked by the Task based on a message received from a callback.
 *
 *          When we get here, the request has already been accepted by the
 *          service and is valid from a BLE protocol perspective as well as
 *          having the correct length as defined in the service implementation.
 *
 * @param   pCharData  pointer to malloc'd char write data
 *
 * @return  None.
 */
void user_Notification_ValueChangeHandler(char_data_t *pCharData)
{
  static uint8_t pretty_data_holder[16]; // 5 bytes as hex string "AA:BB:CC:DD:EE"
  Util_convertArrayToHexString(pCharData->data, pCharData->dataLen,
                               pretty_data_holder, sizeof(pretty_data_holder));

  switch (pCharData->paramID)
  {
    case N_ALERT_LEVEL_ID:
      /*Log_info3("Value Change msg: %s %s: %s",
                (IArg)"Notification",
                (IArg)"Alert Level",
                (IArg)pretty_data_holder);*/

      // Do something useful with pCharData->data here
      // -------------------------
      break;

  default:
    return;
  }
}

/*
 * @brief   Handle a CCCD (configuration change) write received from a peer
 *          device. This tells us whether the peer device wants us to send
 *          Notifications or Indications.
 *
 * @param   pCharData  pointer to malloc'd char write data
 *
 * @return  None.
 */
void user_Notification_CfgChangeHandler(char_data_t *pCharData)
{
    // Cast received data to uint16, as that's the format for CCCD writes.
    uint16_t configValue = *(uint16_t *) pCharData->data;
    char *configValString;

    // Determine what to tell the user
    switch (configValue)
    {
    case GATT_CFG_NO_OPERATION:
        configValString = "Noti/Ind disabled";
        break;
    case GATT_CLIENT_CFG_NOTIFY:
        configValString = "Notifications enabled";
        break;
    case GATT_CLIENT_CFG_INDICATE:
        configValString = "Indications enabled";
        break;
    }

  switch (pCharData->paramID)
  {
    case N_BATTERY_LEVEL_ID:
      /*Log_info3("CCCD Change msg: %s %s: %s",
                (IArg)"Notification",
                (IArg)"Battery Level",
                (IArg)configValString);*/
      // -------------------------
      // Do something useful with configValue here. It tells you whether someone
      // wants to know the state of this characteristic.
      // ... In the generated example we turn periodic clocks on/off
      if (configValue) // 0x0001 and 0x0002 both indicate turned on.
        Clock_start((Clock_Handle)&n_Battery_Level_clock);
      else
        Clock_stop((Clock_Handle)&n_Battery_Level_clock);
      break;

  default:
    return;
  }
}

/*
 * @brief   Handle a write //Request sent from a peer device.
 *
 *          Invoked by the Task based on a message received from a callback.
 *
 *          When we get here, the //Request has already been accepted by the
 *          service and is valid from a BLE protocol perspective as well as
 *          having the correct length as defined in the service implementation.
 *
 * @param   pCharData  pointer to malloc'd char write data
 *
 * @return  None.
 */
/*void user_DeviceInformation_ValueChangeHandler(char_data_t *pCharData)
{
  static uint8_t pretty_data_holder[16]; // 5 bytes as hex string "AA:BB:CC:DD:EE"
  Util_convertArrayToHexString(pCharData->data, pCharData->dataLen,
                               pretty_data_holder, sizeof(pretty_data_holder));

  switch (pCharData->paramID)
  {
    case DI_SERIAL_NUMBER_STRING_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Device Information",
      //          (IArg)"Serial Number String",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case DI_FIRMWARE_REVISION_STRING_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Device Information",
      //          (IArg)"Firmware Revision String",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case DI_RTC_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Device Information",
      //          (IArg)"RTC",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case DI_WAVELENGTH_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Device Information",
      //          (IArg)"WaveLength",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case DI_RANGE_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Device Information",
      //          (IArg)"Range",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case DI_LASER_POWER_LEVELS_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Device Information",
      //          (IArg)"Laser Power Levels",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case DI_XCALCOEFFICIENTS_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Device Information",
      //          (IArg)"XCalCoefficients",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

  default:
    return;
  }
}*/

/*
 * @brief   Handle a write request sent from a peer device.
 *
 *          Invoked by the Task based on a message received from a callback.
 *
 *          When we get here, the request has already been accepted by the
 *          service and is valid from a BLE protocol perspective as well as
 *          having the correct length as defined in the service implementation.
 *
 * @param   pCharData  pointer to malloc'd char write data
 *
 * @return  None.
 */
/*void user_SpectrumData_ValueChangeHandler(char_data_t *pCharData)
{
  static uint8_t pretty_data_holder[16]; // 5 bytes as hex string "AA:BB:CC:DD:EE"
  Util_convertArrayToHexString(pCharData->data, pCharData->dataLen,
                               pretty_data_holder, sizeof(pretty_data_holder));

  switch (pCharData->paramID)
  {
    case SD_TX_FORMAT_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Spectrum Data",
      //          (IArg)"TX Format",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case SD_TX_READ_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Spectrum Data",
      //          (IArg)"TX Read",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

  default:
    return;
  }
}*/

/*
 * @brief   Handle a write //Request sent from a peer device.
 *
 *          Invoked by the Task based on a message received from a callback.
 *
 *          When we get here, the //Request has already been accepted by the
 *          service and is valid from a BLE protocol perspective as well as
 *          having the correct length as defined in the service implementation.
 *
 * @param   pCharData  pointer to malloc'd char write data
 *
 * @return  None.
 */
/*void user_Request_ValueChangeHandler(char_data_t *pCharData)
{
  static uint8_t pretty_data_holder[16]; // 5 bytes as hex string "AA:BB:CC:DD:EE"
  Util_convertArrayToHexString(pCharData->data, pCharData->dataLen,
                               pretty_data_holder, sizeof(pretty_data_holder));

  switch (pCharData->paramID)
  {
    case R_REQUEST_SPECTRUM_ACQUISITION_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Request",
      //          (IArg)"Request Spectrum Acquisition",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case R_INTEGRATION_TIME_ID:
      //Log_Info3("Value Change msg: %s %s: %s",
      //          (IArg)"Request",
      //          (IArg)"Integration Time",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

    case R_LASER_POWER_ID:
      //Log_info3("Value Change msg: %s %s: %s",
      //          (IArg)"Request",
      //          (IArg)"Laser Power",
      //          (IArg)pretty_data_holder);

      // Do something useful with pCharData->data here
      // -------------------------
      break;

  default:
    return;
  }
}*/


/*
 * @brief   Process an incoming BLE stack message.
 *
 *          This could be a GATT message from a peer device like acknowledgement
 *          of an Indication we sent, or it could be a response from the stack
 *          to an HCI message that the user application sent.
 *
 * @param   pMsg - message to process
 *
 * @return  TRUE if safe to deallocate incoming message, FALSE otherwise.
 */
static uint8_t ProjectZero_processStackMsg(ICall_Hdr *pMsg)
{
  uint8_t safeToDealloc = TRUE;

  switch (pMsg->event)
  {
    case GATT_MSG_EVENT:
      // Process GATT message
      safeToDealloc = ProjectZero_processGATTMsg((gattMsgEvent_t *)pMsg);
      break;

    case HCI_GAP_EVENT_EVENT:
      {
        // Process HCI message
        switch(pMsg->status)
        {
          case HCI_COMMAND_COMPLETE_EVENT_CODE:
            // Process HCI Command Complete Event
            //Log_Info0("HCI Command Complete Event received");
            break;

          default:
            break;
        }
      }
      break;

    default:
      // do nothing
      break;
  }

  return (safeToDealloc);
}


/*
 * @brief   Process GATT messages and events.
 *
 * @return  TRUE if safe to deallocate incoming message, FALSE otherwise.
 */
static uint8_t ProjectZero_processGATTMsg(gattMsgEvent_t *pMsg)
{
  // See if GATT server was unable to transmit an ATT response
  if (pMsg->hdr.status == blePending)
  {
   /* Log_warning1("Outgoing RF FIFO full. Re-schedule transmission of msg with opcode 0x%02x",
      pMsg->method);*/

    // No HCI buffer was available. Let's try to retransmit the response
    // on the next connection event.
    if (HCI_EXT_ConnEventNoticeCmd(pMsg->connHandle, selfEntity,
                                   PRZ_CONN_EVT_END_EVT) == SUCCESS)
    {
      // First free any pending response
      ProjectZero_freeAttRsp(FAILURE);

      // Hold on to the response message for retransmission
      pAttRsp = pMsg;

      // Don't free the response message yet
      return (FALSE);
    }
  }
  else if (pMsg->method == ATT_FLOW_CTRL_VIOLATED_EVENT)
  {
    // ATT request-response or indication-confirmation flow control is
    // violated. All subsequent ATT requests or indications will be dropped.
    // The app is informed in case it wants to drop the connection.

    // Log the opcode of the message that caused the violation.
    /*Log_error1("Flow control violated. Opcode of offending ATT msg: 0x%02x",
      pMsg->msg.flowCtrlEvt.opcode);*/
  }
  else if (pMsg->method == ATT_MTU_UPDATED_EVENT)
  {
    // MTU size updated
    //Log_Info1("MTU Size change: %d bytes", pMsg->msg.mtuEvt.MTU);
  }
  else
  {
    // Got an expected GATT message from a peer.
    //Log_Info1("Recevied GATT Message. Opcode: 0x%02x", pMsg->method);
  }

  // Free message payload. Needed only for ATT Protocol messages
  GATT_bm_free(&pMsg->msg, pMsg->method);

  // It's safe to free the incoming message
  return (TRUE);
}




/*
 *  Application error handling functions
 *****************************************************************************/

/*
 * @brief   Send a pending ATT response message.
 *
 *          The message is one that the stack was trying to send based on a
 *          peer request, but the response couldn't be sent because the
 *          user application had filled the TX queue with other data.
 *
 * @param   none
 *
 * @return  none
 */
static void ProjectZero_sendAttRsp(void)
{
  // See if there's a pending ATT Response to be transmitted
  if (pAttRsp != NULL)
  {
    uint8_t status;

    // Increment retransmission count
    rspTxRetry++;

    // Try to retransmit ATT response till either we're successful or
    // the ATT Client times out (after 30s) and drops the connection.
    status = GATT_SendRsp(pAttRsp->connHandle, pAttRsp->method, &(pAttRsp->msg));
    if ((status != blePending) && (status != MSG_BUFFER_NOT_AVAIL))
    {
      // Disable connection event end notice
      HCI_EXT_ConnEventNoticeCmd(pAttRsp->connHandle, selfEntity, 0);

      // We're done with the response message
      ProjectZero_freeAttRsp(status);
    }
    else
    {
      // Continue retrying
     /* Log_warning2("Retrying message with opcode 0x%02x. Attempt %d",
        pAttRsp->method, rspTxRetry);*/
    }
  }
}

/*
 * @brief   Free ATT response message.
 *
 * @param   status - response transmit status
 *
 * @return  none
 */
static void ProjectZero_freeAttRsp(uint8_t status)
{
  // See if there's a pending ATT response message
  if (pAttRsp != NULL)
  {
    // See if the response was sent out successfully
    if (status == SUCCESS)
    {
      //Log_Info2("Sent message with opcode 0x%02x. Attempt %d",
      //  pAttRsp->method, rspTxRetry);
    }
    else
    {
      /*Log_error2("Gave up message with opcode 0x%02x. Status: %d",
        pAttRsp->method, status);*/

      // Free response payload
      GATT_bm_free(&pAttRsp->msg, pAttRsp->method);
    }

    // Free response message
    ICall_freeMsg(pAttRsp);

    // Reset our globals
    pAttRsp = NULL;
    rspTxRetry = 0;
  }
}


/******************************************************************************
 *****************************************************************************
 *
 *  Handlers of direct system callbacks.
 *
 *  Typically enqueue the information or request as a message for the
 *  application Task for handling.
 *
 ****************************************************************************
 *****************************************************************************/


/*
 *  Callbacks from the Stack Task context (GAP or Service changes)
 *****************************************************************************/

/**
 * Callback from GAP Role indicating a role state change.
 */
static void user_gapStateChangeCB(gaprole_States_t newState)
{
  //Log_Info1("(CB) GAP State change: %d, Sending msg to app.", (IArg)newState);
  user_enqueueRawAppMsg( APP_MSG_GAP_STATE_CHANGE, (uint8_t *)&newState, sizeof(newState) );
}

/*
 * @brief   Passcode callback.
 *
 * @param   connHandle - connection handle
 * @param   uiInputs   - input passcode?
 * @param   uiOutputs  - display passcode?
 * @param   numComparison - numeric comparison value
 *
 * @return  none
 */
static void user_gapBondMgr_passcodeCB(uint8_t *deviceAddr, uint16_t connHandle,
                                       uint8_t uiInputs, uint8_t uiOutputs, uint32 numComparison)
{
  passcode_req_t req =
  {
    .connHandle = connHandle,
    .uiInputs = uiInputs,
    .uiOutputs = uiOutputs,
    .numComparison = numComparison
  };

  // Defer handling of the passcode request to the application, in case
  // user input is required, and because a BLE API must be used from Task.
  user_enqueueRawAppMsg(APP_MSG_SEND_PASSCODE, (uint8_t *)&req, sizeof(req));
}

/*
 * @brief   Pairing state callback.
 *
 * @param   connHandle - connection handle
 * @param   state      - pairing state
 * @param   status     - pairing status
 *
 * @return  none
 */
static void user_gapBondMgr_pairStateCB(uint16_t connHandle, uint8_t state,
                                        uint8_t status)
{
  if (state == GAPBOND_PAIRING_STATE_STARTED)
  {
    //Log_Info0("Pairing started");
  }
  else if (state == GAPBOND_PAIRING_STATE_COMPLETE)
  {
    if (status == SUCCESS)
    {
      //Log_Info0("Pairing completed successfully.");
    }
    else
    {
      //Log_error1("Pairing failed. Error: %02x", status);
    }
  }
  else if (state == GAPBOND_PAIRING_STATE_BONDED)
  {
    if (status == SUCCESS)
    {
     //Log_Info0("Re-established pairing from stored bond info.");
    }
  }
}

/**
 * Callback handler for characteristic value changes in services.
 */
static void user_service_ValueChangeCB( uint16_t connHandle, uint16_t svcUuid,
                                        uint8_t paramID, uint8_t *pValue,
                                        uint16_t len )
{
  // See the service header file to compare paramID with characteristic.
  //Log_Info2("(CB) Characteristic value change: svc(0x%04x) paramID(%d). "
  //          "Sending msg to app.", (IArg)svcUuid, (IArg)paramID);
  user_enqueueCharDataMsg(APP_MSG_SERVICE_WRITE, connHandle, svcUuid, paramID,
                          pValue, len);
}

/**
 * Callback handler for characteristic configuration changes in services.
 */
static void user_service_CfgChangeCB(uint16_t connHandle, uint16_t svcUuid,
                                     uint8_t paramID, uint8_t *pValue,
                                     uint16_t len)
{
    /*Log_info2("(CB) Char config change: svc(0x%04x) paramID(%d). "
            "Sending msg to app.", (IArg)svcUuid, (IArg)paramID);*/
    user_enqueueCharDataMsg(APP_MSG_SERVICE_CFG, connHandle, svcUuid, paramID,
                            pValue, len);
}

/*
 *  Callbacks from Swi-context
 *****************************************************************************/

/*
 * Handles the Software Interrupt resulting from timeout of the clock object(s)
 * used to demonstrate notifications/indications.
 */
static void user_generic_clockSwiHandler(uint16_t svcUuid, uint16_t paramID)
{
    uint8_t notiData[20];
    uint16_t notiLen = sizeof notiData;

    //Log_Info2("(SWI) Generic clock handler: svc(0x%04x) paramID(%d)", (IArg)svcUuid, (IArg)paramID);
    static uint8_t someCounter = 0;

    // Get loopback data if char is writable, otherwise fill with junk
    switch (svcUuid)
    {
    case NOTIFICATION_SERV_UUID:
        switch (paramID)
        {
        case N_BATTERY_LEVEL_ID:
            // Characteristic is not writable, so send something, max 20 bytes (max default ATT MTU size minus header)
            notiLen = MIN(notiLen, N_BATTERY_LEVEL_LEN);
            memset(notiData, someCounter++, notiLen);
            break;
        }
        // Send message to application that it should update the value of the characteristic from Task context.
        user_enqueueCharDataMsg(APP_MSG_UPDATE_CHARVAL, 0xFFFF, svcUuid, paramID,
                                notiData, notiLen);
        break;
    case USER_100MS_PERIODIC_UUID:
        // Send message to application that it should update the value of the characteristic from Task context.
        user_enqueueCharDataMsg(APP_MSG_100MS_UPDATE, 0xFFFF, svcUuid, NULL,
                                notiData, notiLen);
        break;

    case USER_1S_PERIODIC_UUID:
        // Send message to application that it should update the value of the characteristic from Task context.
        user_enqueueCharDataMsg(APP_MSG_1S_UPDATE, 0xFFFF, svcUuid, NULL,
                                notiData, notiLen);
        break;
    }
}

/*
 * Swi handler for clock object(s) used by Notification.
 * paramID is stored in the clock object for each characteristic.
 */
static void user_Notification_clockSwiHandler(UArg paramID)
{
  // Act as a closure for the generic clockSwiHandler, as clock objects only have one parameter associated.
  user_generic_clockSwiHandler(NOTIFICATION_SERV_UUID, paramID);
}


static void user_100mSPeriodic_clockSwiHandler(UArg paramID)
{
    // Act as a closure for the generic clockSwiHandler, as clock objects only have one parameter associated.
    user_generic_clockSwiHandler(USER_100MS_PERIODIC_UUID, paramID);
}

static void user_1SPeriodic_clockSwiHandler(UArg paramID)
{
    // Act as a closure for the generic clockSwiHandler, as clock objects only have one parameter associated.
    user_generic_clockSwiHandler(USER_1S_PERIODIC_UUID, paramID);
}

/*
 *  Callbacks from Hwi-context
 *****************************************************************************/


/******************************************************************************
 *****************************************************************************
 *
 *  Utility functions
 *
 ****************************************************************************
 *****************************************************************************/

/*
 * @brief  Generic message constructor for characteristic data.
 *
 *         Sends a message to the application for handling in Task context where
 *         the message payload is a char_data_t struct.
 *
 *         From service callbacks the appMsgType is APP_MSG_SERVICE_WRITE or
 *         APP_MSG_SERVICE_CFG, and functions running in another context than
 *         the Task itself, can set the type to APP_MSG_UPDATE_CHARVAL to
 *         make the user Task loop invoke user_updateCharVal function for them.
 *
 * @param  appMsgType    Enumerated type of message being sent.
 * @param  connHandle    GAP Connection handle of the relevant connection
 * @param  serviceUUID   16-bit part of the relevant service UUID
 * @param  paramID       Index of the characteristic in the service
 * @oaram  *pValue       Pointer to characteristic value
 * @param  len           Length of characteristic data
 */
static void user_enqueueCharDataMsg( app_msg_types_t appMsgType,
                                     uint16_t connHandle,
                                     uint16_t serviceUUID, uint8_t paramID,
                                     uint8_t *pValue, uint16_t len )
{
  // Called in Stack's Task context, so can't do processing here.
  // Send message to application message queue about received data.
  uint16_t readLen = len; // How much data was written to the attribute

  // Allocate memory for the message.
  // Note: The pCharData message doesn't have to contain the data itself, as
  //       that's stored in a variable in the service implementation.
  //
  //       However, to prevent data loss if a new value is received before the
  //       service's container is read out via the GetParameter API is called,
  //       we copy the characteristic's data now.
  app_msg_t *pMsg = ICall_malloc( sizeof(app_msg_t) + sizeof(char_data_t) +
                                  readLen );

  if (pMsg != NULL)
  {
    pMsg->type = appMsgType;

    char_data_t *pCharData = (char_data_t *)pMsg->pdu;
    pCharData->svcUUID = serviceUUID; // Use 16-bit part of UUID.
    pCharData->paramID = paramID;
    // Copy data from service now.
    memcpy(pCharData->data, pValue, readLen);
    // Update pCharData with how much data we received.
    pCharData->dataLen = readLen;
    // Enqueue the message using pointer to queue node element.
    Queue_enqueue(hApplicationMsgQ, &pMsg->_elem);
  // Let application know there's a message.
  Event_post(syncEvent, PRZ_APP_MSG_EVT);
  }
}


void int_enqueueCharDataMsg( app_msg_types_t appMsgType, uint16_t connHandle,
                                       uint16_t serviceUUID, uint8_t paramID,
                                       uint8_t *pValue, uint16_t len )
{
    user_enqueueCharDataMsg(appMsgType, connHandle, serviceUUID, paramID, pValue, len);
}

/*
 * @brief  Generic message constructor for application messages.
 *
 *         Sends a message to the application for handling in Task context.
 *
 * @param  appMsgType    Enumerated type of message being sent.
 * @oaram  *pValue       Pointer to characteristic value
 * @param  len           Length of characteristic data
 */
static void user_enqueueRawAppMsg(app_msg_types_t appMsgType, uint8_t *pData,
                                  uint16_t len)
{
  // Allocate memory for the message.
  app_msg_t *pMsg = ICall_malloc( sizeof(app_msg_t) + len );

  if (pMsg != NULL)
  {
    pMsg->type = appMsgType;

    // Copy data into message
    memcpy(pMsg->pdu, pData, len);

    // Enqueue the message using pointer to queue node element.

    Queue_enqueue(hApplicationMsgQ, &pMsg->_elem);
//    // Let application know there's a message.
    Event_post(syncEvent, PRZ_APP_MSG_EVT);
  }
}


/*
 * @brief  Convenience function for updating characteristic data via char_data_t
 *         structured message.
 *
 * @note   Must run in Task context in case BLE Stack APIs are invoked.
 *
 * @param  *pCharData  Pointer to struct with value to update.
 */
static void user_updateCharVal(char_data_t *pCharData)
{
  switch(pCharData->svcUUID)
  {
    case NOTIFICATION_SERV_UUID:
      Notification_SetParameter( pCharData->paramID,
              pCharData->dataLen, pCharData->data );
    break;

   /* case DEVICE_INFORMATION_SERV_UUID:
      DeviceInformation_SetParameter( pCharData->paramID,
              pCharData->dataLen, pCharData->data );
    break;

    case SPECTRUM_DATA_SERV_UUID:
      SpectrumData_SetParameter( pCharData->paramID,
              pCharData->dataLen, pCharData->data );
    break;

    case REQUEST_SERV_UUID:
      Request_SetParameter( pCharData->paramID,
              pCharData->dataLen, pCharData->data );
    break;*/

  }
}

/*
 * @brief   Convert {0x01, 0x02} to "01:02"
 *
 * @param   src - source byte-array
 * @param   src_len - length of array
 * @param   dst - destination string-array
 * @param   dst_len - length of array
 *
 * @return  array as string
 */
static char *Util_convertArrayToHexString(uint8_t const *src, uint8_t src_len,
                                          uint8_t *dst, uint8_t dst_len)
{
  char        hex[] = "0123456789ABCDEF";
  uint8_t     *pStr = dst;
  uint8_t     avail = dst_len-1;

  memset(dst, 0, avail);

  while (src_len && avail > 3)
  {
    if (avail < dst_len-1) { *pStr++ = ':'; avail -= 1; };
    *pStr++ = hex[*src >> 4];
    *pStr++ = hex[*src++ & 0x0F];
    avail -= 2;
    src_len--;
  }

  if (src_len && avail)
    *pStr++ = ':'; // Indicate not all data fit on line.

  return (char *)dst;
}

/*
 * @brief   Extract the LOCALNAME from Scan/AdvData
 *
 * @param   data - Pointer to the advertisement or scan response data
 *
 * @return  Pointer to null-terminated string with the adv local name.
 */
static char *Util_getLocalNameStr(const uint8_t *data) {
  uint8_t nuggetLen = 0;
  uint8_t nuggetType = 0;
  uint8_t advIdx = 0;

  static char localNameStr[32] = { 0 };
  memset(localNameStr, 0, sizeof(localNameStr));

  for (advIdx = 0; advIdx < 32;) {
    nuggetLen = data[advIdx++];
    nuggetType = data[advIdx];
    if ( (nuggetType == GAP_ADTYPE_LOCAL_NAME_COMPLETE ||
          nuggetType == GAP_ADTYPE_LOCAL_NAME_SHORT) && nuggetLen < 31) {
      memcpy(localNameStr, &data[advIdx + 1], nuggetLen - 1);
      break;
    } else {
      advIdx += nuggetLen;
    }
  }

  return localNameStr;
}


/*********************************************************************
 * @fn      SPPBLEServer_enqueueMsg
 *
 * @brief   Creates a message and puts the message in RTOS queue.
 *
 * @param   event  - message event.
 * @param   status - message status.
 *
 * @return  None.
 */
void ProjectZero_enqueueUARTMsg(uint8_t event, uint8_t *data, uint8_t len)
{
    sbpUARTEvt_t *pMsg;
    queueRec_t *pRec;

    //Enqueue message only in a connected state
    //if((gapProfileState == GAPROLE_CONNECTED) || (gapProfileState == GAPROLE_CONNECTED_ADV))
    //{
    // Create dynamic pointer to message.
    if (pMsg = ICall_malloc(sizeof(sbpUARTEvt_t)))
    {
        pMsg->event = event;
        pMsg->pData = (uint8 *) ICall_allocMsg(len);
        if (pMsg->pData)
        {
            //payload
            memcpy(pMsg->pData, data, len);
        }
        pMsg->length = len;

        // Enqueue the message.
        if ((pRec = ICall_malloc(sizeof(queueRec_t))))
        {
            pRec->pData = (uint8*) pMsg;
            // This is an atomic operation
            Queue_enqueue(appUARTMsgQueue, &pRec->_elem);

            Event_post(syncEvent, SBP_UART_QUEUE_EVT);
        }
        else
        {
            DEBUG("appUARTMsgQueue ERROR");
            ICall_free(pMsg);
        }
    }
    //}
}



void SDITask_RXerror(uint8_t event, uint8_t *data, uint8_t len)
{

}

/*********************************************************************
 * @fn      SPPBLEServer_enqueueMsg
 *
 * @brief   Creates a message and puts the message in RTOS queue.
 *
 * @param   event - message event.
 * @param   state - message state.
 *
 * @return  None.
 */
/*static void ProjectZero_enqueueMsg(uint8_t event, uint8_t state)
 {
 sbpEvt_t *pMsg;

 // Create dynamic pointer to message.
 if ((pMsg = ICall_malloc(sizeof(sbpEvt_t))))
 {
 pMsg->hdr.event = event;
 pMsg->hdr.state = state;

 // Enqueue the message.
 Util_enqueueMsg(hApplicationMsgQ, syncEvent, (uint8*)pMsg);
 }
 }*/
