error_t tcs34725Init(void)
{
  uint8_t id = 0;

  /* Initialise I2C */
  i2cInit(I2CMASTER);

  /* Ping the I2C device first to see if it exists! */
  ASSERT(i2cCheckAddress(TCS34725_ADDRESS), ERROR_I2C_DEVICENOTFOUND);

  /* Make sure we have the right IC (0x44 = TCS34725 and TCS34721) */
  ASSERT_STATUS(tcs34725Read8(TCS34725_ID, &id));
  ASSERT(id == 0x44, ERROR_DEVICENOTINITIALISED);

  /* Enable the device */
  ASSERT_STATUS(tcs34725Enable());

  /* Ready to go ... set the initialised flag */
  _tcs34725Initialised = true;

  /* This needs to take place after the initialisation flag! */
  ASSERT_STATUS(tcs34725SetIntegrationTime(TCS34725_INTEGRATIONTIME_2_4MS));
  ASSERT_STATUS(tcs34725SetGain(TCS34725_GAIN_60X));

  return ERROR_NONE;
}

error_t pca9685SetPWM(uint16_t channel, uint16_t on, uint16_t off)
{
  ASSERT(_pca9685Initialised, ERROR_DEVICENOTINITIALISED);

  if (on > 0xFFF)
  {
    on = 0xFFF;
  }

  if (off < on)
  {
    off = on;
  }

  if (off > 0xFFF)
  {
    off = 0xFFF;
  }

  /* Set the on and off values */
  ASSERT_STATUS(pca9685Write8(PCA9685_REG_LED0_ON_L+4*channel, on & 0xFF));
  ASSERT_STATUS(pca9685Write8(PCA9685_REG_LED0_ON_H+4*channel, on >> 8));
  ASSERT_STATUS(pca9685Write8(PCA9685_REG_LED0_OFF_L+4*channel, off & 0xFF));
  ASSERT_STATUS(pca9685Write8(PCA9685_REG_LED0_OFF_H+4*channel, off >> 8));

  return ERROR_NONE;
}

err_t tsl2561GetData (uint16_t *broadband, uint16_t *ir)
{
  /* Enable the device by setting the control bit to 0x03 */
  ASSERT_STATUS(tsl2561Enable());

  /* Wait x ms for ADC to complete */
  switch (_tsl2561IntegrationTime)
  {
    case TSL2561_INTEGRATIONTIME_13MS:
      delay(14);
      break;
    case TSL2561_INTEGRATIONTIME_101MS:
      delay(102);
      break;
    default:
      delay(403);
      break;
  }

  /* Reads a two byte value from channel 0 (visible + infrared) */
  ASSERT_STATUS(tsl2561Read16(TSL2561_COMMAND_BIT | TSL2561_WORD_BIT | TSL2561_REGISTER_CHAN0_LOW, broadband));

  /* Reads a two byte value from channel 1 (infrared) */
  ASSERT_STATUS(tsl2561Read16(TSL2561_COMMAND_BIT | TSL2561_WORD_BIT | TSL2561_REGISTER_CHAN1_LOW, ir));

  /* Turn the device off to save power */
  ASSERT_STATUS(tsl2561Disable());

  return ERROR_NONE;
}

error_t btle_init(void)
{
    nrf_clock_lf_cfg_t clockConfiguration;

    // Configure the LF clock according to values provided by btle_clock.h.
    // It is input from the chain of the yotta configuration system.
    clockConfiguration.source        = LFCLK_CONF_SOURCE;
    clockConfiguration.xtal_accuracy = LFCLK_CONF_ACCURACY;
    clockConfiguration.rc_ctiv       = LFCLK_CONF_RC_CTIV;
    clockConfiguration.rc_temp_ctiv  = LFCLK_CONF_RC_TEMP_CTIV;

    SOFTDEVICE_HANDLER_INIT(&clockConfiguration, signalEvent);

    // Enable BLE stack
    /**
     * Using this call, the application can select whether to include the
     * Service Changed characteristic in the GATT Server. The default in all
     * previous releases has been to include the Service Changed characteristic,
     * but this affects how GATT clients behave. Specifically, it requires
     * clients to subscribe to this attribute and not to cache attribute handles
     * between connections unless the devices are bonded. If the application
     * does not need to change the structure of the GATT server attributes at
     * runtime this adds unnecessary complexity to the interaction with peer
     * clients. If the SoftDevice is enabled with the Service Changed
     * Characteristics turned off, then clients are allowed to cache attribute
     * handles making applications simpler on both sides.
     */
    static const bool IS_SRVC_CHANGED_CHARACT_PRESENT = true;

    ble_enable_params_t ble_enable_params;
    uint32_t err_code = softdevice_enable_get_default_config(CENTRAL_LINK_COUNT,
                                                    PERIPHERAL_LINK_COUNT,
                                                    &ble_enable_params);

    ble_enable_params.gatts_enable_params.attr_tab_size  = GATTS_ATTR_TAB_SIZE;
    ble_enable_params.gatts_enable_params.service_changed  = IS_SRVC_CHANGED_CHARACT_PRESENT;
    ble_enable_params.common_enable_params.vs_uuid_count = UUID_TABLE_MAX_ENTRIES;

    if(err_code  != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }

    if (softdevice_enable(&ble_enable_params) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }

    ble_gap_addr_t addr;
    if (sd_ble_gap_address_get(&addr) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }
    if (sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_NONE, &addr) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }

    ASSERT_STATUS( softdevice_ble_evt_handler_set(btle_handler));
    ASSERT_STATUS( softdevice_sys_evt_handler_set(sys_evt_dispatch));

    return btle_gap_init();
}

error_t tcs34725Enable(void)
{
  ASSERT_STATUS(tcs34725Write8(TCS34725_ENABLE, TCS34725_ENABLE_PON));
  delay(3);
  ASSERT_STATUS(tcs34725Write8(TCS34725_ENABLE, TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN));

  return ERROR_NONE;
}

error_t tcs34725Disable(void)
{
  /* Turn the device off to save power */
  uint8_t reg = 0;
  ASSERT_STATUS(tcs34725Read8(TCS34725_ENABLE, &reg));
  ASSERT_STATUS(tcs34725Write8(TCS34725_ENABLE, reg & ~(TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN)));

  return ERROR_NONE;
}

error_t lm75bConfigWrite (uint8_t configValue)
{
  if (!_lm75bInitialised)
  {
    ASSERT_STATUS(lm75bInit());
  }

  ASSERT_STATUS(lm75bWrite8(LM75B_REGISTER_CONFIGURATION, configValue));

  return ERROR_NONE;
}

static void test_cycles_in_replacement() {
  upb_symtab *s = upb_symtab_new(&s);
  upb_msgdef *m = upb_msgdef_newnamed("M", &s);
  upb_status status = UPB_STATUS_INIT;

  upb_msgdef_addfield(m, newfield("m", 1, UPB_TYPE_MESSAGE,
                                  UPB_LABEL_OPTIONAL, ".M", &s),
                      &s, NULL);
  ASSERT_STATUS(upb_symtab_add(s, (upb_def**)&m, 1, &s, &status), &status);
  ASSERT_STATUS(upb_symtab_add(s, NULL, 0, &s, &status), &status);
}

error_t tcs34725SetIntegrationTime(tcs34725IntegrationTime_t it)
{
  if (!_tcs34725Initialised)
  {
    ASSERT_STATUS(tcs34725Init());
  }

  ASSERT_STATUS(tcs34725Write8(TCS34725_ATIME, it));
  _tcs34725IntegrationTime = it;

  return ERROR_NONE;
}

error_t tcs34725SetGain(tcs34725Gain_t gain)
{
  if (!_tcs34725Initialised)
  {
    ASSERT_STATUS(tcs34725Init());
  }

  ASSERT_STATUS(tcs34725Write8(TCS34725_CONTROL, gain));
  _tcs34725Gain = gain;

  return ERROR_NONE;
}

error_t btle_init(void)
{
    const bool useScheduler = false;
#ifdef TARGET_HRM1017
    SOFTDEVICE_HANDLER_INIT(NRF_CLOCK_LFCLKSRC_RC_250_PPM_4000MS_CALIBRATION, useScheduler);
#else
    SOFTDEVICE_HANDLER_INIT(NRF_CLOCK_LFCLKSRC_XTAL_20_PPM, useScheduler);
#endif

    // Enable BLE stack
    /**
     * Using this call, the application can select whether to include the
     * Service Changed characteristic in the GATT Server. The default in all
     * previous releases has been to include the Service Changed characteristic,
     * but this affects how GATT clients behave. Specifically, it requires
     * clients to subscribe to this attribute and not to cache attribute handles
     * between connections unless the devices are bonded. If the application
     * does not need to change the structure of the GATT server attributes at
     * runtime this adds unnecessary complexity to the interaction with peer
     * clients. If the SoftDevice is enabled with the Service Changed
     * Characteristics turned off, then clients are allowed to cache attribute
     * handles making applications simpler on both sides.
     */
    static const bool IS_SRVC_CHANGED_CHARACT_PRESENT = true;
    ble_enable_params_t enableParams = {
        .gatts_enable_params = {
            .service_changed = IS_SRVC_CHANGED_CHARACT_PRESENT
        }
    };
    if (sd_ble_enable(&enableParams) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }

    ble_gap_addr_t addr;
    if (sd_ble_gap_address_get(&addr) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }
    if (sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_NONE, &addr) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }

    ASSERT_STATUS( softdevice_ble_evt_handler_set(btle_handler));
    ASSERT_STATUS( softdevice_sys_evt_handler_set(sys_evt_dispatch));

#if NEED_BOND_MANAGER /* disabled by default */
    bond_manager_init();
#endif
    btle_gap_init();

    return ERROR_NONE;
}

error_t tcs34725GetRawData(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c)
{
  if (!_tcs34725Initialised)
  {
    ASSERT_STATUS(tcs34725Init());
  }

  /* ToDo: Insert a blocky delay until the data is ready! */
  ASSERT_STATUS(tcs34725Read16(TCS34725_CDATAL, c));
  ASSERT_STATUS(tcs34725Read16(TCS34725_RDATAL, r));
  ASSERT_STATUS(tcs34725Read16(TCS34725_GDATAL, g));
  ASSERT_STATUS(tcs34725Read16(TCS34725_BDATAL, b));

  return ERROR_NONE;
}

error_t btle_init(void)
{
    APP_TIMER_INIT(0, 8, 5, false);
    SOFTDEVICE_HANDLER_INIT(NRF_CLOCK_LFCLKSRC_XTAL_20_PPM, false);

    ASSERT_STATUS( softdevice_ble_evt_handler_set(btle_handler));
    ASSERT_STATUS( softdevice_sys_evt_handler_set(sys_evt_dispatch));

#if NEED_BOND_MANAGER /* disabled by default */
    bond_manager_init();
#endif
    btle_gap_init();

    return ERROR_NONE;
}

error_t btle_init(void)
{
    nrf_clock_lfclksrc_t clockSource;
    if (NRF_CLOCK->LFCLKSRC & (CLOCK_LFCLKSRC_SRC_Xtal << CLOCK_LFCLKSRC_SRC_Pos)) {
        clockSource = NRF_CLOCK_LFCLKSRC_XTAL_20_PPM;
    } else {
        clockSource = NRF_CLOCK_LFCLKSRC_RC_250_PPM_4000MS_CALIBRATION;
    }
    SOFTDEVICE_HANDLER_INIT(clockSource, eventHandler);

    // Enable BLE stack
    /**
     * Using this call, the application can select whether to include the
     * Service Changed characteristic in the GATT Server. The default in all
     * previous releases has been to include the Service Changed characteristic,
     * but this affects how GATT clients behave. Specifically, it requires
     * clients to subscribe to this attribute and not to cache attribute handles
     * between connections unless the devices are bonded. If the application
     * does not need to change the structure of the GATT server attributes at
     * runtime this adds unnecessary complexity to the interaction with peer
     * clients. If the SoftDevice is enabled with the Service Changed
     * Characteristics turned off, then clients are allowed to cache attribute
     * handles making applications simpler on both sides.
     */
    static const bool IS_SRVC_CHANGED_CHARACT_PRESENT = true;
    ble_enable_params_t enableParams = {
        .gatts_enable_params = {
            .service_changed = IS_SRVC_CHANGED_CHARACT_PRESENT,
            .attr_tab_size = gatt_table_size
        }
    };
    if (sd_ble_enable(&enableParams) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }

    ble_gap_addr_t addr;
    if (sd_ble_gap_address_get(&addr) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }
    if (sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_NONE, &addr) != NRF_SUCCESS) {
        return ERROR_INVALID_PARAM;
    }

    ASSERT_STATUS( softdevice_ble_evt_handler_set(btle_handler));
    ASSERT_STATUS( softdevice_sys_evt_handler_set(sys_evt_dispatch));

    return btle_gap_init();
}

error_t custom_add_in_descriptor(uint16_t    char_handle,
                                             ble_uuid_t *p_uuid,
                                             uint8_t    *p_data,
                                             uint16_t    length,
                                             uint16_t    max_length,
                                             uint16_t   *p_desc_handle)
{
    /* Descriptor metadata */
    ble_gatts_attr_md_t   desc_md = {0};

    desc_md.vloc = BLE_GATTS_VLOC_STACK;
    /* Always set variable size */
    desc_md.vlen = 1;

    /* Make it readable and writable */
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&desc_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&desc_md.write_perm);

    ble_gatts_attr_t attr_desc = {0};

    attr_desc.p_uuid    = p_uuid;
    attr_desc.p_attr_md = &desc_md;
    attr_desc.init_len  = length;
    attr_desc.max_len   = max_length;
    attr_desc.p_value   = p_data;

    ASSERT_STATUS ( sd_ble_gatts_descriptor_add(char_handle,
                                                &attr_desc,
                                                p_desc_handle));

    return ERROR_NONE;
}

error_t adxl345GetSensorEvent(sensors_event_t *event)
{
  int16_t x, y, z;

  /* Clear the event */
  memset(event, 0, sizeof(sensors_event_t));

  event->version   = sizeof(sensors_event_t);
  event->sensor_id = _adxl345SensorID;
  event->type      = SENSOR_TYPE_ACCELEROMETER;
  event->timestamp = systickGetTicks();

  /* Retrieve values from the sensor */
  ASSERT_STATUS(adxl345GetXYZ(&x, &y, &z));

  /* The ADXL345 returns a raw value where each lsb represents 4mg.  To
   * convert this to a normal g value, multiply by 0.004 and then convert
   * it to the m/s^2 value that sensors_event_t is expecting. */
  event->acceleration.x = x * ADXL345_MG2G_MULTIPLIER * SENSORS_GRAVITY_STANDARD;
  event->acceleration.y = y * ADXL345_MG2G_MULTIPLIER * SENSORS_GRAVITY_STANDARD;
  event->acceleration.z = z * ADXL345_MG2G_MULTIPLIER * SENSORS_GRAVITY_STANDARD;

  printf("   %08d    %08d    %08d%s", x, y, z, CFG_PRINTF_NEWLINE);

  return ERROR_NONE;
}

err_t adxl345GetXYZ(int16_t *x, int16_t *y, int16_t *z)
{
  int32_t i2cState;

  if (!_adxl345Initialised)
  {
    ASSERT_STATUS(adxl345Init());
  }

  I2CWriteLength = 2;
  I2CReadLength = 6;
  I2CMasterBuffer[0] = ADXL345_ADDRESS;
  I2CMasterBuffer[1] = ADXL345_REG_DATAX0;
  I2CMasterBuffer[2] = ADXL345_ADDRESS | ADXL345_READBIT;
  i2cState = i2cEngine();

  /* Check if we got an ACK or TIMEOUT error */
  ASSERT_I2C_STATUS(i2cState);

  /* Shift values to create properly formed integer */
  *x = (I2CSlaveBuffer[1] << 8) | (I2CSlaveBuffer[0]);
  *y = (I2CSlaveBuffer[3] << 8) | (I2CSlaveBuffer[2]);
  *z = (I2CSlaveBuffer[5] << 8) | (I2CSlaveBuffer[4]);

  return ERROR_NONE;
}

error_t adxl345SetRange(adxl345_range_t range)
{
  uint8_t format;

  /* Red the data format register to preserve bits */
  ASSERT_STATUS(adxl345Read8(ADXL345_REG_DATA_FORMAT, &format));

  /* Update the data rate */
  format &= ~0x0F;
  format |= range;

  /* Write the register back to the IC */
  ASSERT_STATUS(adxl345Write8(ADXL345_REG_DATA_FORMAT, format));

  return ERROR_NONE;
}

error_t tcs34725GetSensorEvent(sensors_event_t *event)
{
  float maxCount;
  uint16_t r, g, b, c;

  /* Clear the event */
  memset(event, 0, sizeof(sensors_event_t));

  event->version   = sizeof(sensors_event_t);
  event->sensor_id = _tcs34725SensorID;
  event->type      = SENSOR_TYPE_COLOR;
  event->timestamp = delayGetTicks();

  /* Get the raw RGB values */
  ASSERT_STATUS(tcs34725GetRawData(&r, &g, &b, &c));

  /* Convert RGB values to floats */
  maxCount = (256 - _tcs34725IntegrationTime) * 1024;
  event->color.r = r / maxCount;
  event->color.g = g / maxCount;
  event->color.b = b / maxCount;

  /* Convert to a 24-bit ARGB value */
  event->color.rgba = (uint8_t)(event->color.r * 0xFF) << 16|
                      (uint8_t)(event->color.g * 0xFF) << 8 |
                      (uint8_t)(event->color.b * 0xFF) |
                      0xFF000000;

  return ERROR_NONE;
}

err_t adxl345GetDataRate(adxl345_dataRate_t *dataRate)
{
  uint8_t results;
  ASSERT_STATUS(adxl345Read8(ADXL345_REG_BW_RATE, &results));
  *dataRate = (adxl345_dataRate_t)(results & 0x0F);

  return ERROR_NONE;
}