/**************************************************************************************************
*  Filename:       ActivityMonitor.c
*  Revised:        $Date: 2015-08-10 17:22:05 -0800 (Mon, 10 Aug 2015) $
*  Revision:       $Revision: 42106 $
*
*  Description:    This file contains the Activity Monitor sample application
*                  for use with the CC2650 Bluetooth Low Energy Protocol Stack.
*
*  Copyright (C) 2015 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.
*******************************************************************************/

/*********************************************************************
 * INCLUDES
 */
#include <xdc/std.h>

#include <xdc/runtime/Error.h>
#include <xdc/runtime/System.h>

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

#include <ICall.h>
#include "bcomdef.h"
#include "linkdb.h"
#include "gatt.h"
#include "hci.h"
#include "gapgattserver.h"
#include "gattservapp.h"
#include "peripheral.h"
#include "gapbondmgr.h"
#include "gatt_profile_uuid.h"
#include "heartrateservice.h"
#include "devinfoservice.h"
#include "battservice.h"

#include "osal_snv.h"
#include "ICallBleAPIMSG.h"

#include "util.h"
#include "board.h"
#include "am_i2c.h"

#include "hrm.h"
#include "AFE4404.h"
#include "ActivityMonitor.h"
#ifdef CALIBRATION_ENABLED
#include "Calibration_AFE4404.h"
#endif

/*********************************************************************
 * MACROS
 */

#define delay_ms(i) Task_sleep( ((i) * 1000) / Clock_tickPeriod )

/*********************************************************************
 * CONSTANTS
 */

// What is the 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 (LE limited discoverable mode)
//#define DEFAULT_DISCOVERABLE_MODE                       GAP_ADTYPE_FLAGS_LIMITED
#define DEFAULT_DISCOVERABLE_MODE                       GAP_ADTYPE_FLAGS_GENERAL

// Whether to enable automatic parameter update request when a connection is
// formed.
#define DEFAULT_ENABLE_UPDATE_REQUEST                   TRUE//FALSE

// Minimum connection interval (units of 1.25ms) if automatic parameter update
// request is enabled.
#define DEFAULT_DESIRED_MIN_CONN_INTERVAL               80//200

// Maximum connection interval (units of 1.25ms) if automatic parameter update
// request is enabled.
#define DEFAULT_DESIRED_MAX_CONN_INTERVAL               800//1600

// Slave latency to use if automatic parameter update request is enabled.
#define DEFAULT_DESIRED_SLAVE_LATENCY                   0//1

// Supervision timeout value (units of 10ms) if automatic parameter update
// request is enabled.
#define DEFAULT_DESIRED_CONN_TIMEOUT                    1000//100

//* Connection Pause Peripheral time value (in seconds)
#define DEFAULT_CONN_PAUSE_PERIPHERAL                   6//1

//* Company Identifier: Texas Instruments Inc. (13)
#define TI_COMPANY_ID                                   0x000D
#define INVALID_CONNHANDLE                              0xFFFF

// How often to perform heart rate periodic event.
#define DEFAULT_HEARTRATE_PERIOD                        2000//100

// Battery level is critical when it is less than this %
#define DEFAULT_BATT_CRITICAL_LEVEL                     6

// Battery measurement period in ms
#define DEFAULT_BATT_PERIOD                             15000

// AFE minimum measurement period in ms
#define DEFAULT_AFE_PERIOD                              6

// Internal Events for RTOS application
#define AM_STATE_CHANGE_EVT                             0x0001
#define AM_KEY_CHANGE_EVT                               0x0002

// Heart rate events
#define HEARTRATE_MEAS_EVT                              0x0004
#define HEARTRATE_MEAS_PERIODIC_EVT                     0x0008

// Battery Event.
#define AM_BATT_EVT                                     0x0010
#define AM_BATT_PERIODIC_EVT                            0x0020

// AFE event.
#define HEARTRATE_AFE_PERIODIC_EVT                      0x0040

#define HEARTRATE_MEAS_LEN                              9

// Task configuration
#define AM_TASK_PRIORITY                                1
#ifndef AM_TASK_STACK_SIZE
#define AM_TASK_STACK_SIZE                              600
#endif

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

// App event passed from profiles.
typedef struct
{
  uint8_t event;  // Which profile's event
  uint8_t state; // New state
} amEvt_t;

/*********************************************************************
 * GLOBAL VARIABLES
 */

// Profile state and parameters
gaprole_States_t gapProfileState = GAPROLE_INIT;

// Semaphore globally used to post events to the application thread
ICall_Semaphore sem;

// Global pin resources
PIN_State pinGpioState;
PIN_Handle pinGpioHandle;

extern unsigned char HeartRate;
extern char LED_Sel;

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

// Task configuration
Task_Struct amTask;
Char amTaskStack[AM_TASK_STACK_SIZE];

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

// Clock instances for internal periodic events.
static Clock_Struct measPerClock;
static Clock_Struct battPerClock;
static Clock_Struct afePerClock;

// Queue object used for app messages
static Queue_Struct appMsg;
static Queue_Handle appMsgQueue;

// Events flag for internal application events.
static uint16_t events;

// GAP - SCAN RSP data (max size = 31 bytes)
static uint8 scanRspData[] =
{
  // Complete name
  0x11,   // length of this data
  GAP_ADTYPE_LOCAL_NAME_COMPLETE,
  'A', 'c', 't', 'i', 'v', 'i', 't', 'y', ' ',
  'M', 'o', 'n', 'i', 't', 'o', 'r',

  // Connection interval range
  0x05,   // length of this data
  GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE,
  LO_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ),
  HI_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL ),
  LO_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ),
  HI_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL ),

  // Tx power level
  0x02,   // length of this data
  GAP_ADTYPE_POWER_LEVEL,
  0       // 0dBm
};

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

  // Service UUIDs, notifies central device of what services are included
  0x03,//5,
  GAP_ADTYPE_16BIT_MORE,
  LO_UINT16(HEARTRATE_SERV_UUID),
  HI_UINT16(HEARTRATE_SERV_UUID),
};

// Device name attribute value.
static uint8_t attDeviceName[GAP_DEVICE_NAME_LEN] = "Activity Monitor";

// GAP connection handle.
static uint16_t gapConnHandle;

// Heart Rate and Battery profile callback variables.
static uint8_t heartRateEvent;
static uint8_t batteryEvent;

unsigned long AFE4404_Data_buffer[6];

volatile int g_OneSecondFlag=0;
uint16_t prfcount=0;

#ifdef DEBUG_AFE
unsigned long AFE4404_DATA[500];
unsigned long AFE4404_DATA2[500];
unsigned long AFE4404_DATA3[500];
unsigned long data_index = 0;
#endif

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

// Task functions and message processing
static void ActivityMonitor_init(void);
static void ActivityMonitor_taskFxn(UArg a0, UArg a1);

static void ActivityMonitor_processStackMsg(ICall_Hdr *pMsg);
static void ActivityMonitor_processGattMsg(gattMsgEvent_t *pMsg);
static void ActivityMonitor_processAppMsg(amEvt_t *pMsg);
static uint8_t ActivityMonitor_enqueueMsg(uint8_t event, uint8_t state);

static void ActivityMonitor_clockHandler(UArg arg);
static void ActivityMonitor_measPerTask(void);
static void ActivityMonitor_battPerTask(void);
static void ActivityMonitor_afePerTask(void);
static void ActivityMonitor_measNotify(void);

// Events and callbacks for profiles and hardware interrupts.
static void ActivityMonitor_battCB(uint8_t event);
static void ActivityMonitor_battEvt(uint8_t event);
static void ActivityMonitor_heartRateCB(uint8_t event);
static void ActivityMonitor_heartRateEvt(uint8_t event);
static void ActivityMonitor_afeCB(PIN_Handle handle, PIN_Id pinId);

static void ActivityMonitor_stateChangeCB(gaprole_States_t newState);
static void ActivityMonitor_stateChangeEvt(gaprole_States_t pEvent);

void ActivityMonitor_blinkLed(uint8_t led, uint8_t nBlinks);

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

// GAP Role Callbacks
static gapRolesCBs_t activityMonitorPeripheralCB =
{
  ActivityMonitor_stateChangeCB,       // Profile State Change Callbacks
 // NULL                                 // When a valid RSSI is read from
                                       // controller (not used by application)
};

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

/*********************************************************************
 * @fn      ActivityMonitor_createTask
 *
 * @brief   Task creation function for the Simple BLE Broadcaster.
 *
 * @param   none
 *
 * @return  none
 */
void ActivityMonitor_createTask(void)
{
  Task_Params taskParams;

  // Configure task
  Task_Params_init(&taskParams);
  taskParams.stack = amTaskStack;
  taskParams.stackSize = AM_TASK_STACK_SIZE;
  taskParams.priority = AM_TASK_PRIORITY;

  Task_construct(&amTask, ActivityMonitor_taskFxn, &taskParams, NULL);
}

/*********************************************************************
 * @fn      ActivityMonitor_init
 *
 * @brief   Initialization function for the Simple BLE Broadcaster App
 *          Task. This is called during initialization and should contain
 *          any application specific initialization (ie. hardware
 *          initialization/setup, table initialization, power up
 *          notificaiton ...).
 *
 * @param   none
 *
 * @return  none
 */
static void ActivityMonitor_init(void)
{
  // Initialize PINs and I2C
  pinGpioHandle = PIN_open(&pinGpioState, BoardGpioInitTable);
  amI2cInit();

  // Register interrupt handler for ADC_RDY
  PIN_registerIntCb(pinGpioHandle, ActivityMonitor_afeCB);

  // Ensure that the AFE4404 is currently off
  AFE4404_Enable_HWPDN();

  // Initialize HRM variables
  initStatHRM();

#ifdef CALIBRATION_ENABLED
  // Initialize calibration routine variables and set default state to off
  //initCalibrationRoutine();
#endif

  // ******************************************************************
  // 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.
  ICall_registerApp(&selfEntity, &sem);

  // Hard code the DB Address till CC2650 board gets its own IEEE address
  //uint8 bdAddress[B_ADDR_LEN] = { 0x33, 0x33, 0x33, 0x33, 0x33, 0x33 };
  //HCI_EXT_SetBDADDRCmd(bdAddress);

  // Set device's Sleep Clock Accuracy
  //HCI_EXT_SetSCACmd(500);

  // Create an RTOS queue for message from profile to be sent to app.
  appMsgQueue = Util_constructQueue(&appMsg);

  // Create one-shot clocks for internal periodic events
  Util_constructClock(&measPerClock, ActivityMonitor_clockHandler,
                        DEFAULT_HEARTRATE_PERIOD, 0, false,
                        HEARTRATE_MEAS_PERIODIC_EVT);
  Util_constructClock(&battPerClock, ActivityMonitor_clockHandler,
                        DEFAULT_BATT_PERIOD, 0, false,
                        AM_BATT_PERIODIC_EVT);
  Util_constructClock(&afePerClock, ActivityMonitor_clockHandler,
                        DEFAULT_AFE_PERIOD, 0, false,
                        HEARTRATE_AFE_PERIODIC_EVT);

  // Setup the GAP
  GAP_SetParamValue(TGAP_CONN_PAUSE_PERIPHERAL, DEFAULT_CONN_PAUSE_PERIPHERAL);

  // Setup the GAP Peripheral Role Profile
  {
    // For all hardware platforms, device starts advertising upon initialization
    uint8_t initialAdvertEnable = TRUE;

    // By setting this to zero, the device will go into the waiting state after
    // being discoverable for 30.72 second, and will not being advertising again
    // until the enabler is set back to TRUE
    uint16_t advertOffTime = 0;

    uint8_t enableUpdateRequest = DEFAULT_ENABLE_UPDATE_REQUEST;
    uint16_t desiredMinInterval = DEFAULT_DESIRED_MIN_CONN_INTERVAL;
    uint16_t desiredMaxInterval = DEFAULT_DESIRED_MAX_CONN_INTERVAL;
    uint16_t desiredSlaveLatency = DEFAULT_DESIRED_SLAVE_LATENCY;
    uint16_t desiredConnTimeout = DEFAULT_DESIRED_CONN_TIMEOUT;

    // Set the GAP Role Parameters
    GAPRole_SetParameter(GAPROLE_ADVERT_ENABLED, sizeof(uint8_t),
                         &initialAdvertEnable);
    GAPRole_SetParameter(GAPROLE_ADVERT_OFF_TIME, sizeof(uint16_t),
                         &advertOffTime);

    GAPRole_SetParameter(GAPROLE_SCAN_RSP_DATA, sizeof(scanRspData),
                         scanRspData);
    GAPRole_SetParameter(GAPROLE_ADVERT_DATA, sizeof(advertData),
                         advertData);

    GAPRole_SetParameter(GAPROLE_PARAM_UPDATE_ENABLE, sizeof(uint8_t),
                         &enableUpdateRequest);
    GAPRole_SetParameter(GAPROLE_MIN_CONN_INTERVAL, sizeof(uint16_t),
                         &desiredMinInterval);
    GAPRole_SetParameter(GAPROLE_MAX_CONN_INTERVAL, sizeof(uint16_t),
                         &desiredMaxInterval);
    GAPRole_SetParameter(GAPROLE_SLAVE_LATENCY, sizeof(uint16_t),
                         &desiredSlaveLatency);
    GAPRole_SetParameter(GAPROLE_TIMEOUT_MULTIPLIER, sizeof(uint16_t),
                         &desiredConnTimeout);
  }

  // Set the GAP Characteristics
  GGS_SetParameter(GGS_DEVICE_NAME_ATT, GAP_DEVICE_NAME_LEN,
                  (void*)attDeviceName);

  // 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);
  }

  // Initialize GATT attributes
  GGS_AddService(GATT_ALL_SERVICES);
  GATTServApp_AddService(GATT_ALL_SERVICES);

  // Initialize services
  HeartRate_AddService(GATT_ALL_SERVICES);
  DevInfo_AddService();
  Batt_AddService();

  // Setup the Heart Rate characteristic values
  {
    uint8_t sensLoc = HEARTRATE_SENS_LOC_WRIST;
    HeartRate_SetParameter(HEARTRATE_SENS_LOC, sizeof(uint8_t), &sensLoc);
  }

  // Setup Battery Characteristic Values.
  {
    uint8_t critical = DEFAULT_BATT_CRITICAL_LEVEL;
    Batt_SetParameter(BATT_PARAM_CRITICAL_LEVEL, sizeof(uint8_t), &critical);
  }

  // Register for Heart Rate service callback.
  HeartRate_Register(&ActivityMonitor_heartRateCB);

  // Register for Battery service callback.
  Batt_Register(&ActivityMonitor_battCB);

  // Start the Device
  GAPRole_StartDevice(&activityMonitorPeripheralCB);
}

/*********************************************************************
 * @fn      ActivityMonitor_processEvent
 *
 * @brief   Application task entry point for the Simple BLE Broadcaster.
 *
 * @param   none
 *
 * @return  none
 */
static void ActivityMonitor_taskFxn(UArg a0, UArg a1)
{
  // Initialize application
  ActivityMonitor_init();

  // Application main loop
  for (;;)
  {
    // Get the ticks since startup
    uint32_t tickStart = Clock_getTicks();

    // Waits for a signal to the semaphore associated with the calling thread.
    // Note that the semaphore associated with a thread is signaled when a
    // message is queued to the message receive queue of the thread or when
    // ICall_signal() function is called onto the semaphore.
    ICall_Errno errno = ICall_wait(ICALL_TIMEOUT_FOREVER);

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

      if (ICall_fetchServiceMsg(&src, &dest,
                                (void **)&pMsg) == ICALL_ERRNO_SUCCESS)
      {
        if ((src == ICALL_SERVICE_CLASS_BLE) && (dest == selfEntity))
        {
          // Process inter-task message
          ActivityMonitor_processStackMsg((ICall_Hdr *)pMsg);
        }

        if (pMsg)
        {
          ICall_freeMsg(pMsg);
        }
      }

      // If RTOS queue is not empty, process app message.
      while (!Queue_empty(appMsgQueue))
      {
        amEvt_t *pMsg = (amEvt_t *)Util_dequeueMsg(appMsgQueue);
        if (pMsg)
        {
          // Process message.
          ActivityMonitor_processAppMsg(pMsg);

          // Free the space from the message.
          ICall_free(pMsg);
        }
      }
    }

    // Heart rate service callback event.
    if (events & HEARTRATE_MEAS_EVT)
    {
      events &= ~HEARTRATE_MEAS_EVT;

      ActivityMonitor_heartRateEvt(heartRateEvent);
    }

    // Heart rate service periodic task.
    if (events & HEARTRATE_MEAS_PERIODIC_EVT)
    {
      events &= ~HEARTRATE_MEAS_PERIODIC_EVT;

      ActivityMonitor_measPerTask();
    }

    // Battery service callback event.
    if (events & AM_BATT_EVT)
    {
      events &= ~AM_BATT_EVT;

      ActivityMonitor_battEvt(batteryEvent);
    }

    // Battery service periodic task.
    if (events & AM_BATT_PERIODIC_EVT)
    {
      events &= ~AM_BATT_PERIODIC_EVT;

      ActivityMonitor_battPerTask();
    }

    // AFE periodic measurement
    if (events & HEARTRATE_AFE_PERIODIC_EVT)
    {
      events &= ~HEARTRATE_AFE_PERIODIC_EVT;

      ActivityMonitor_afePerTask();
    }
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_processStackMsg
 *
 * @brief   Process an incoming stack message.
 *
 * @param   pMsg - message to process
 *
 * @return  none
 */
static void ActivityMonitor_processStackMsg(ICall_Hdr *pMsg)
{
    switch (pMsg->event)
    {
      case GATT_MSG_EVENT:
        ActivityMonitor_processGattMsg((gattMsgEvent_t *)pMsg);
        break;

      default:
        // Do nothing.
        break;
    }
}

/*********************************************************************
 * @fn      ActivityMonitor_processGattMsg
 *
 * @brief   Process GATT messages.
 *
 * @param   pMsg - pointer the the GATT message.
 *
 * @return  none
 */
static void ActivityMonitor_processGattMsg(gattMsgEvent_t *pMsg)
{
  GATT_bm_free(&pMsg->msg, pMsg->method);
}

/*********************************************************************
 * @fn      ActivityMonitor_processAppMsg
 *
 * @brief   Process an incoming callback from a profile.
 *
 * @param   pMsg - message to process
 *
 * @return  none
 */
static void ActivityMonitor_processAppMsg(amEvt_t *pMsg)
{
  switch (pMsg->event)
  {
    case AM_STATE_CHANGE_EVT:
      ActivityMonitor_stateChangeEvt((gaprole_States_t)pMsg->state);
      break;

    default:
      // Do nothing.
      break;
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_enqueueMsg
 *
 * @brief   Creates a message and puts the message in RTOS queue.
 *
 * @param   event - message event.
 * @param   state - new state after event.
 *
 * @return  TRUE or FALSE
 */
static uint8_t ActivityMonitor_enqueueMsg(uint8_t event, uint8_t state)
{
  amEvt_t *pMsg;

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

    // Enqueue the message.
    return Util_enqueueMsg(appMsgQueue, sem, (uint8_t *)pMsg);
  }

  return FALSE;
}

/*********************************************************************
 * @fn      ActivityMonitor_clockHandler
 *
 * @brief   Handler function for clock timeouts.
 *
 * @param   none
 *
 * @return  none
 */
static void ActivityMonitor_clockHandler(UArg arg)
{
  // Store the event.
  events |= arg;

  // Wake up the application.
  Semaphore_post(sem);
}

/*********************************************************************
 * @fn      ActivityMonitor_perTask
 *
 * @brief   Perform a periodic heart rate notification.
 *
 * @param   none
 *
 * @return  none
 */
static void ActivityMonitor_measPerTask(void)
{
  if (gapProfileState == GAPROLE_CONNECTED)
  {
    // Send heart rate measurement notification.
    ActivityMonitor_measNotify();

    // Restart timer.
    Util_startClock(&measPerClock);
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_battPerTask
 *
 * @brief   Perform a periodic task for battery measurement.
 *
 * @param   none
 *
 * @return  none
 */
static void ActivityMonitor_battPerTask(void)
{
  if (gapProfileState == GAPROLE_CONNECTED)
  {
    // Perform battery level check.
    Batt_MeasLevel();

    // Restart timer.
    Util_startClock(&battPerClock);
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_afePerTask
 *
 * @brief   Perform a periodic measurement of the AFE4404.
 *
 * @param   none
 *
 * @return  none
 */

static void ActivityMonitor_afePerTask(void)
{
  if (gapProfileState == GAPROLE_CONNECTED)
  {
    AFE4404_Data_buffer[0] = AFE4404_Reg_Read(42);  //read LED2 Data
    AFE4404_Data_buffer[1] = AFE4404_Reg_Read(43);  //read LED3 data
    AFE4404_Data_buffer[2] = AFE4404_Reg_Read(44);  //read LED1 Data
    AFE4404_Data_buffer[3] = AFE4404_Reg_Read(45);  //read Ambient Data
    AFE4404_Data_buffer[4] = AFE4404_Reg_Read(46);  //read LED2 - LED3 Data
    AFE4404_Data_buffer[5] = AFE4404_Reg_Read(47);  //read LED1 - Ambient Data

#ifdef DEBUG_AFE
    AFE4404_DATA[data_index] = AFE4404_Data_buffer[0];
    AFE4404_DATA2[data_index] = AFE4404_Data_buffer[1];
    AFE4404_DATA3[data_index] = AFE4404_Data_buffer[2];
    data_index++;
    if(data_index == 500)
        data_index = 0;
#endif

    statHRMAlgo(AFE4404_Data_buffer[5]);
    //HeartRate = AFE4404_Data_buffer[2]*(1.2/2097152);
    //HeartRate = 500*(AFE4404_Data_buffer[5]*(1.2/2097152));

#ifdef CALIBRATION_ENABLED
    if (LED_Sel == 2)
        CalibrateAFE4404(AFE4404_Data_buffer[0], AFE4404_Data_buffer[3],0);
    else if (LED_Sel == 3)
        CalibrateAFE4404(AFE4404_Data_buffer[1], AFE4404_Data_buffer[3],0);
    else // Default LED_Sel = 1
        CalibrateAFE4404(AFE4404_Data_buffer[2], AFE4404_Data_buffer[3],0);

    prfcount++;
    if(prfcount==100)
    {
        g_OneSecondFlag=1;
        prfcount=0;
    }
#endif
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_measNotify
 *
 * @brief   Prepare and send a heart rate measurement notification.
 *
 * @return  none
 */
static void ActivityMonitor_measNotify(void)
{
  attHandleValueNoti_t heartRateMeas;

  heartRateMeas.pValue = GATT_bm_alloc(gapConnHandle, ATT_HANDLE_VALUE_NOTI,
                                       HEARTRATE_MEAS_LEN, NULL);
  if (heartRateMeas.pValue != NULL)
  {
    uint8_t *p = heartRateMeas.pValue;
    uint8_t flags = HEARTRATE_FLAGS_CONTACT_NOT_SUP;

    // Build heart rate measurement structure from simulated values.
    *p++ = flags;
    *p++ = HeartRate;//heartRateBpm;

    heartRateMeas.len = (uint8)(p - heartRateMeas.pValue);

    // Send notification.
    if (HeartRate_MeasNotify(gapConnHandle, &heartRateMeas) != SUCCESS)
    {
      GATT_bm_free((gattMsg_t *)&heartRateMeas, ATT_HANDLE_VALUE_NOTI);
    }
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_battCB
 *
 * @brief   Callback function for battery service.
 *
 * @param   event - service event
 *
 * @return  none
 */
static void ActivityMonitor_battCB(uint8_t event)
{
  // Set variable.
  batteryEvent = event;

  // Set event.
  events |= AM_BATT_EVT;

  // Wake up the application.
  Semaphore_post(sem);
}

/*********************************************************************
 * @fn      ActivityMonitor_battEvt
 *
 * @brief   Event handler for battery service callbacks.
 *
 * @param   event - service event
 *
 * @return  none
 */
static void ActivityMonitor_battEvt(uint8_t event)
{
  if (event == BATT_LEVEL_NOTI_ENABLED)
  {
    // If connected start periodic measurement.
    if (gapProfileState == GAPROLE_CONNECTED)
    {
      Util_startClock(&battPerClock);
    }
  }
  else if (event == BATT_LEVEL_NOTI_DISABLED)
  {
    // Stop periodic measurement.
    Util_stopClock(&battPerClock);
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_heartRateCB
 *
 * @brief   Perform a periodic heart rate notification.
 *
 * @param   event - service event
 *
 * @return  none
 */
static void ActivityMonitor_heartRateCB(uint8_t event)
{
  // Set event variable.
  heartRateEvent = event;

  // Set event.
  events |= HEARTRATE_MEAS_EVT;

  // Wake up the application.
  Semaphore_post(sem);
}

/*********************************************************************
 * @fn      ActivityMonitor_heartRateEvt
 *
 * @brief   event handler for heart rate service callbacks.
 *
 * @param   event - service event
 *
 * @return  none
 */
static void ActivityMonitor_heartRateEvt(uint8_t event)
{
  if (event == HEARTRATE_MEAS_NOTI_ENABLED)
  {
    // If connected start periodic measurement.
    if (gapProfileState == GAPROLE_CONNECTED)
    {
      Util_startClock(&measPerClock);
    }
  }
  else if (event == HEARTRATE_MEAS_NOTI_DISABLED)
  {
    // Stop periodic measurement.
    Util_stopClock(&measPerClock);
  }
  else if (event == HEARTRATE_COMMAND_SET)
  {
    // Reset energy expended.
    //* heartRateEnergyLvl = 0;
  }
}

/*********************************************************************
 * @fn      ActivityMonitor_afeCB
 *
 * @brief   Callback function for AFE ADC_RDY.
 *
 * @param   event - service event
 *
 * @return  none
 */
static void ActivityMonitor_afeCB(PIN_Handle handle, PIN_Id pinId)
{
    if(pinId == Board_ADC_RDY)
    {
        Util_startClock(&afePerClock);
    }
}


/*********************************************************************
 * @fn      ActivityMonitor_stateChangeCB
 *
 * @brief   Callback from GAP Role indicating a role state change.
 *
 * @param   newState - new state
 *
 * @return  none
 */
static void ActivityMonitor_stateChangeCB(gaprole_States_t newState)
{
  // Enqueue the event.
  ActivityMonitor_enqueueMsg(AM_STATE_CHANGE_EVT, newState);
}

/*********************************************************************
 * @fn      ActivityMonitor_stateChangeEvt
 *
 * @brief   Notification from the profile of a state change.
 *
 * @param   newState - new state
 *
 * @return  none
 */
static void ActivityMonitor_stateChangeEvt(gaprole_States_t newState)
{
  switch (newState)
  {
    case GAPROLE_STARTED:
      {
          uint8_t ownAddress[B_ADDR_LEN];
          uint8_t systemId[DEVINFO_SYSTEM_ID_LEN];

          // Blink LED2 5 times
          ActivityMonitor_blinkLed(Board_LED2, 5);

          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];

          // Pass systemId to the Device Info service.
          DevInfo_SetParameter(DEVINFO_SYSTEM_ID, DEVINFO_SYSTEM_ID_LEN, systemId);
      }
      break;

    case GAPROLE_ADVERTISING:
      {
      }
      break;

    case GAPROLE_CONNECTED:
      {
         // Blink LED2 3 times
         ActivityMonitor_blinkLed(Board_LED2, 3);

         // Check connection handle
         GAPRole_GetParameter(GAPROLE_CONNHANDLE, &gapConnHandle);

         // Initialize the AFE4404 and set default registers
         AFE4404_Init();

#ifdef CALIBRATION_ENABLED
         initCalibrationRoutine();
#endif
         // Enable interrupts on ADC_RDY
         PIN_setInterrupt(pinGpioHandle, Board_ADC_RDY | PIN_IRQ_NEGEDGE);
      }
      break;

    case GAPROLE_WAITING:
    case GAPROLE_WAITING_AFTER_TIMEOUT:
      {
         // Reset and turn off AFE4404
         AFE4404_Enable_HWPDN();

         // Disable interrupts on ADC_RDY
         PIN_setInterrupt(pinGpioHandle, Board_ADC_RDY | PIN_IRQ_DIS);
      }
      break;

    case GAPROLE_ERROR:
      {
         // Reset and turn off AFE4404
          AFE4404_Enable_HWPDN();

         // Disable interrupts on ADC_RDY
         PIN_setInterrupt(pinGpioHandle, Board_ADC_RDY | PIN_IRQ_DIS);
      }
      break;

    default:
      {

      }
      break;
  }
  gapProfileState = newState;
}

/*********************************************************************
 * @fn      ActivityMonitor_blinkLed
 *
 * @brief   Blink an led 'n' times, duty-cyle 50-50
 * @param   led - led identifier
 * @param   nBlinks - number of blinks
 *
 * @return  none
 */
void ActivityMonitor_blinkLed(uint8_t led, uint8_t nBlinks)
{
  uint8_t i;

  for (i=0; i<nBlinks; i++)
  {
    PIN_setOutputValue(pinGpioHandle, led, Board_LED_ON);
    delay_ms(33);
    PIN_setOutputValue(pinGpioHandle, led, Board_LED_OFF);
    delay_ms(33);
  }
}

/*********************************************************************
*********************************************************************/
