xref: /btstack/src/ble/gatt-service/cycling_power_service_server.c (revision 52c964be65e0d5221f289eae20e0d8aee1fd5b94)

/*
 * Copyright (C) 2014 BlueKitchen GmbH
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name of the copyright holders nor the names of
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 * 4. Any redistribution, use, or modification is done solely for
 *    personal benefit and not for any commercial purpose or for
 *    monetary gain.
 *
 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH 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 BLUEKITCHEN
 * GMBH 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.
 *
 * Please inquire about commercial licensing options at
 * [email protected]
 *
 */

#define BTSTACK_FILE__ "cycling_power_service_server.c"


#include "bluetooth.h"
#include "btstack_defines.h"
#include "bluetooth_data_types.h"
#include "btstack_event.h"
#include "ble/att_db.h"
#include "ble/att_server.h"
#include "btstack_util.h"
#include "bluetooth_gatt.h"
#include "btstack_debug.h"
#include "l2cap.h"
#include "hci.h"

#include "ble/gatt-service/cycling_power_service_server.h"

#define CYCLING_POWER_MAX_BROACAST_MSG_SIZE         31
#define CONTROL_POINT_PROCEDURE_TIMEOUT_MS          30
#define CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED     0xFFFF

typedef enum {
    CP_MASK_BIT_PEDAL_POWER_BALANCE = 0,
    CP_MASK_BIT_ACCUMULATED_TORQUE,
    CP_MASK_BIT_WHEEL_REVOLUTION_DATA,
    CP_MASK_BIT_CRANK_REVOLUTION_DATA,
    CP_MASK_BIT_EXTREME_MAGNITUDES,
    CP_MASK_BIT_EXTREME_ANGLES,
    CP_MASK_BIT_TOP_DEAD_SPOT_ANGLE,
    CP_MASK_BIT_BOTTOM_DEAD_SPOT_ANGLE,
    CP_MASK_BIT_ACCUMULATED_ENERGY,
    CP_MASK_BIT_RESERVED
} cycling_power_mask_bit_t;

typedef enum {
    CP_OPCODE_IDLE = 0,
    CP_OPCODE_SET_CUMULATIVE_VALUE,
    CP_OPCODE_UPDATE_SENSOR_LOCATION,
    CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS,
    CP_OPCODE_SET_CRANK_LENGTH,
    CP_OPCODE_REQUEST_CRANK_LENGTH,
    CP_OPCODE_SET_CHAIN_LENGTH,
    CP_OPCODE_REQUEST_CHAIN_LENGTH,
    CP_OPCODE_SET_CHAIN_WEIGHT,
    CP_OPCODE_REQUEST_CHAIN_WEIGHT,
    CP_OPCODE_SET_SPAN_LENGTH,
    CP_OPCODE_REQUEST_SPAN_LENGTH,
    CP_OPCODE_START_OFFSET_COMPENSATION,
    CP_OPCODE_MASK_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT,
    CP_OPCODE_REQUEST_SAMPLING_RATE,
    CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE,
    CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION,
    CP_OPCODE_RESPONSE_CODE = 32
} cycling_power_opcode_t;

typedef enum {
    CP_RESPONSE_VALUE_SUCCESS = 1,
    CP_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED,
    CP_RESPONSE_VALUE_INVALID_PARAMETER,
    CP_RESPONSE_VALUE_OPERATION_FAILED,
    CP_RESPONSE_VALUE_NOT_AVAILABLE,
    CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE
} cycling_power_response_value_t;

typedef enum {
    CP_CONNECTION_INTERVAL_STATUS_NONE = 0,
    CP_CONNECTION_INTERVAL_STATUS_RECEIVED,
    CP_CONNECTION_INTERVAL_STATUS_ACCEPTED,
    CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE,
    CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE,
    CP_CONNECTION_INTERVAL_STATUS_REJECTED
} cycling_power_con_interval_status_t;

typedef struct {
    hci_con_handle_t con_handle;
    // GATT connection management
    uint16_t con_interval;
    uint16_t con_interval_min;
    uint16_t con_interval_max;
    cycling_power_con_interval_status_t  con_interval_status;

    // Cycling Power Measurement
    uint16_t measurement_value_handle;
    int16_t  instantaneous_power_W;

    cycling_power_pedal_power_balance_reference_t  pedal_power_balance_reference;
    uint8_t  pedal_power_balance_percentage;    // percentage, resolution 1/2,
                                                // If the sensor provides the power balance referenced to the left pedal,
                                                // the power balance is calculated as [LeftPower/(LeftPower + RightPower)]*100 in units of percent

    cycling_power_torque_source_t torque_source;
    uint16_t accumulated_torque_Nm;             // newton-meters, resolution 1/32,
                                                // The Accumulated Torque value may decrease
    // wheel revolution data:
    uint32_t cumulative_wheel_revolutions;      // CANNOT roll over
    uint16_t last_wheel_event_time_s;           // seconds, resolution 1/2048
    // crank revolution data:
    uint16_t cumulative_crank_revolutions;
    uint16_t last_crank_event_time_s;           // seconds, resolution 1/1024
    // extreme force magnitudes
    int16_t  maximum_force_magnitude_N;
    int16_t  minimum_force_magnitude_N;
    int16_t  maximum_torque_magnitude_Nm;       // newton-meters, resolution 1/32
    int16_t  minimum_torque_magnitude_Nm;       // newton-meters, resolution 1/32
    // extreme angles
    uint16_t maximum_angle_degree;                 // 12bit, degrees
    uint16_t minimum_angle_degree;                 // 12bit, degrees, concatenated with previous into 3 octets
                                                // i.e. if the Maximum Angle is 0xABC and the Minimum Angle is 0x123, the transmitted value is 0x123ABC.
    uint16_t top_dead_spot_angle_degree;
    uint16_t bottom_dead_spot_angle_degree;            // The Bottom Dead Spot Angle field represents the crank angle when the value of the Instantaneous Power value becomes negative.
    uint16_t accumulated_energy_kJ;             // kilojoules; CANNOT roll over

    // uint8_t  offset_compensation;

    // CP Measurement Notification (Client Characteristic Configuration)
    uint16_t measurement_client_configuration_descriptor_handle;
    uint16_t measurement_client_configuration_descriptor_notify;
    btstack_context_callback_registration_t measurement_notify_callback;

    // CP Measurement Broadcast (Server Characteristic Configuration)
    uint16_t measurement_server_configuration_descriptor_handle;
    uint16_t measurement_server_configuration_descriptor_broadcast;
    btstack_context_callback_registration_t measurement_broadcast_callback;

    // Cycling Power Feature
    uint16_t feature_value_handle;
    uint32_t feature_flags;                             // see cycling_power_feature_flag_t
    uint16_t masked_measurement_flags;
    uint16_t default_measurement_flags;

    // Sensor Location
    uint16_t sensor_location_value_handle;
    cycling_power_sensor_location_t sensor_location;    // see cycling_power_sensor_location_t
    cycling_power_sensor_location_t * supported_sensor_locations;
    uint16_t num_supported_sensor_locations;
    uint16_t crank_length_mm;                           // resolution 1/2 mm
    uint16_t chain_length_mm;                           // resolution 1 mm
    uint16_t chain_weight_g;                            // resolution 1 gram
    uint16_t span_length_mm;                            // resolution 1 mm

    gatt_date_time_t factory_calibration_date;

    uint8_t  sampling_rate_Hz;                          // resolution 1 Herz

    uint16_t  current_force_magnitude_N;
    uint16_t  current_torque_magnitude_Nm;               // newton-meters, resolution 1/32
    uint16_t manufacturer_company_id;
    uint8_t num_manufacturer_specific_data;
    uint8_t * manufacturer_specific_data;

    // Cycling Power Vector
    uint16_t vector_value_handle;
    uint16_t vector_cumulative_crank_revolutions;
    uint16_t vector_last_crank_event_time_s;                           // seconds, resolution 1/1024
    uint16_t vector_first_crank_measurement_angle_degree;
    int16_t  * vector_instantaneous_force_magnitude_N_array;           // newton
    uint16_t force_magnitude_count;
    int16_t  * vector_instantaneous_torque_magnitude_Nm_array;         // newton-meter, resolution 1/32
    uint16_t torque_magnitude_count;
    cycling_power_instantaneous_measurement_direction_t vector_instantaneous_measurement_direction;

    // CP Vector Notification (Client Characteristic Configuration)
    uint16_t vector_client_configuration_descriptor_handle;
    uint16_t vector_client_configuration_descriptor_notify;
    btstack_context_callback_registration_t vector_notify_callback;

    // CP Control Point
    uint16_t control_point_value_handle;
    // CP Control Point Indication (Client Characteristic Configuration)
    uint16_t control_point_client_configuration_descriptor_handle;
    uint16_t control_point_client_configuration_descriptor_indicate;
    btstack_context_callback_registration_t control_point_indicate_callback;

    cycling_power_opcode_t request_opcode;
    cycling_power_response_value_t response_value;

    btstack_packet_handler_t calibration_callback;
    uint8_t w4_indication_complete;
} cycling_power_t;

static att_service_handler_t cycling_power_service;
static cycling_power_t cycling_power;
static btstack_packet_callback_registration_t hci_event_callback_registration;
static btstack_packet_callback_registration_t l2cap_event_callback_registration;

static uint16_t cycling_power_service_read_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){
    UNUSED(con_handle);
    UNUSED(attribute_handle);
    UNUSED(offset);
    cycling_power_t * instance = &cycling_power;

    if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){
        if (buffer && (buffer_size >= 2u)){
            little_endian_store_16(buffer, 0, instance->measurement_client_configuration_descriptor_notify);
        }
        return 2;
    }

    if (attribute_handle == instance->measurement_server_configuration_descriptor_handle){
        if (buffer && (buffer_size >= 2u)){
            little_endian_store_16(buffer, 0, instance->measurement_server_configuration_descriptor_broadcast);
        }
        return 2;
    }

    if (attribute_handle == instance->vector_client_configuration_descriptor_handle){
        if (buffer && (buffer_size >= 2u)){
            little_endian_store_16(buffer, 0, instance->vector_client_configuration_descriptor_notify);
        }
        return 2;
    }

    if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){
        if (buffer && (buffer_size >= 2u)){
            little_endian_store_16(buffer, 0, instance->control_point_client_configuration_descriptor_indicate);
        }
        return 2;
    }

    if (attribute_handle == instance->feature_value_handle){
        if (buffer && (buffer_size >= 4u)){
            little_endian_store_32(buffer, 0, instance->feature_flags);
        }
        return 4;
    }

    if (attribute_handle == instance->sensor_location_value_handle){
        if (buffer && (buffer_size >= 1u)){
            buffer[0] = instance->sensor_location;
        }
        return 1;
    }
    return 0;
}

static int has_feature(cycling_power_feature_flag_t feature){
    cycling_power_t * instance = &cycling_power;
    return (instance->feature_flags & (1u << feature)) != 0u;
}

static int cycling_power_vector_instantaneous_measurement_direction(void){
    cycling_power_t * instance = &cycling_power;
    return instance->vector_instantaneous_measurement_direction;
}

static uint16_t cycling_power_service_default_measurement_flags(void){
    cycling_power_t * instance = &cycling_power;
    uint16_t measurement_flags = 0;
    uint8_t flag[] = {
        (uint8_t) has_feature(CP_FEATURE_FLAG_PEDAL_POWER_BALANCE_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_PEDAL_POWER_BALANCE_SUPPORTED) && instance->pedal_power_balance_reference,
        (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_TORQUE_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_TORQUE_SUPPORTED) && instance->torque_source,
        (uint8_t) has_feature(CP_FEATURE_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE),
        (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE),
        (uint8_t) has_feature(CP_FEATURE_FLAG_EXTREME_ANGLES_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_TOP_AND_BOTTOM_DEAD_SPOT_ANGLE_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_TOP_AND_BOTTOM_DEAD_SPOT_ANGLE_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_ACCUMULATED_ENERGY_SUPPORTED),
        (uint8_t) has_feature(CP_FEATURE_FLAG_OFFSET_COMPENSATION_INDICATOR_SUPPORTED)
    };

    int i;
    for (i = CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT; i <= CP_MEASUREMENT_FLAG_OFFSET_COMPENSATION_INDICATOR; i++){
        measurement_flags |= flag[i] << i;
    }

    return measurement_flags;
}

static uint16_t cycling_power_service_get_measurement_flags(cycling_power_t * instance){
    if (!instance) return 0;
    if (instance->masked_measurement_flags != CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED){
        return instance->masked_measurement_flags;
    }
    if (instance->default_measurement_flags == CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED){
        instance->default_measurement_flags = cycling_power_service_default_measurement_flags();
    }
    return instance->default_measurement_flags;
}


uint16_t cycling_power_service_measurement_flags(void){
    cycling_power_t * instance = &cycling_power;
    return cycling_power_service_get_measurement_flags(instance);
}

uint8_t cycling_power_service_vector_flags(void){
    uint8_t vector_flags = 0;
    uint8_t flag[] = {
        (uint8_t )has_feature(CP_FEATURE_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED),
        (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_ANGLES_SUPPORTED),
        (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE),
        (uint8_t )has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED) && (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE),
        (uint8_t )has_feature(CP_FEATURE_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION_SUPPORTED) && cycling_power_vector_instantaneous_measurement_direction()
    };

    int i;
    for (i = CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT; i <= CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION; i++){
        vector_flags |= flag[i] << i;
    }
    return vector_flags;
}

static void cycling_power_service_vector_can_send_now(void * context){
    cycling_power_t * instance = (cycling_power_t *) context;
    if (!instance){
        log_error("cycling_power_service_measurement_can_send_now: instance is null");
        return;
    }
    uint8_t value[50];
    uint8_t vector_flags = cycling_power_service_vector_flags();
    int pos = 0;

    value[pos++] = vector_flags;
    int i;
    for (i = CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT; i <= CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION; i++){
        if ((vector_flags & (1u << i)) == 0u) continue;
        switch ((cycling_power_vector_flag_t) i){
            case CP_VECTOR_FLAG_CRANK_REVOLUTION_DATA_PRESENT:
                little_endian_store_16(value, pos, instance->cumulative_crank_revolutions);
                pos += 2;
                little_endian_store_16(value, pos, instance->last_crank_event_time_s);
                pos += 2;
                break;
            case CP_VECTOR_FLAG_INSTANTANEOUS_FORCE_MAGNITUDE_ARRAY_PRESENT:{
                uint16_t att_mtu = att_server_get_mtu(instance->con_handle);
                uint16_t bytes_left = 0;
                if (att_mtu > (pos + 3u)){
                    bytes_left = btstack_min(sizeof(value), att_mtu - 3u - pos);
                }
                while ((bytes_left > 2u) && instance->force_magnitude_count){
                    little_endian_store_16(value, pos, instance->vector_instantaneous_force_magnitude_N_array[0]);
                    pos += 2;
                    bytes_left -= 2u;
                    instance->vector_instantaneous_force_magnitude_N_array++;
                    instance->force_magnitude_count--;
                }
                break;
            }
            case CP_VECTOR_FLAG_INSTANTANEOUS_TORQUE_MAGNITUDE_ARRAY_PRESENT:{
                uint16_t att_mtu = att_server_get_mtu(instance->con_handle);
                uint16_t bytes_left = 0;
                if (att_mtu > (pos + 3u)){
                    bytes_left = btstack_min(sizeof(value), att_mtu - 3u - pos);
                }

                while ((bytes_left > 2u) && instance->torque_magnitude_count){
                    little_endian_store_16(value, pos, instance->vector_instantaneous_torque_magnitude_Nm_array[0]);
                    pos += 2;
                    bytes_left -= 2u;
                    instance->vector_instantaneous_torque_magnitude_Nm_array++;
                    instance->torque_magnitude_count--;
                }
                break;
            }
            case CP_VECTOR_FLAG_FIRST_CRANK_MEASUREMENT_ANGLE_PRESENT:
                little_endian_store_16(value, pos, instance->vector_first_crank_measurement_angle_degree);
                pos += 2;
                break;
            case CP_VECTOR_FLAG_INSTANTANEOUS_MEASUREMENT_DIRECTION:
                break;
            default:
                break;
        }
    }

    att_server_notify(instance->con_handle, instance->vector_value_handle, &value[0], pos);
}

static int cycling_power_measurement_flag_value_size(cycling_power_measurement_flag_t flag){
    switch (flag){
        case CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT:
            return 1;
        case CP_MEASUREMENT_FLAG_WHEEL_REVOLUTION_DATA_PRESENT:
            return 6;
        case CP_MEASUREMENT_FLAG_CRANK_REVOLUTION_DATA_PRESENT:
        case CP_MEASUREMENT_FLAG_EXTREME_FORCE_MAGNITUDES_PRESENT:
        case CP_MEASUREMENT_FLAG_EXTREME_TORQUE_MAGNITUDES_PRESENT:
            return 4;
        case CP_MEASUREMENT_FLAG_EXTREME_ANGLES_PRESENT:
            return 3;
        case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_PRESENT:
        case CP_MEASUREMENT_FLAG_TOP_DEAD_SPOT_ANGLE_PRESENT:
        case CP_MEASUREMENT_FLAG_BOTTOM_DEAD_SPOT_ANGLE_PRESENT:
        case CP_MEASUREMENT_FLAG_ACCUMULATED_ENERGY_PRESENT:
            return 2;
        default:
            return 0;
    }
}

static int cycling_power_store_measurement_flag_value(cycling_power_t * instance, cycling_power_measurement_flag_t flag, uint8_t * value){
    if (!instance) return 0;

    int pos = 0;
    switch (flag){
        case CP_MEASUREMENT_FLAG_PEDAL_POWER_BALANCE_PRESENT:
            value[pos++] = instance->pedal_power_balance_percentage;
            break;
        case CP_MEASUREMENT_FLAG_ACCUMULATED_TORQUE_PRESENT:
            little_endian_store_16(value, pos, instance->accumulated_torque_Nm);
            pos += 2;
            break;
        case CP_MEASUREMENT_FLAG_WHEEL_REVOLUTION_DATA_PRESENT:
            little_endian_store_32(value, pos, instance->cumulative_wheel_revolutions);
            pos += 4;
            little_endian_store_16(value, pos, instance->last_wheel_event_time_s);
            pos += 2;
            break;
        case CP_MEASUREMENT_FLAG_CRANK_REVOLUTION_DATA_PRESENT:
            little_endian_store_16(value, pos, instance->cumulative_crank_revolutions);
            pos += 2;
            little_endian_store_16(value, pos, instance->last_crank_event_time_s);
            pos += 2;
            break;
        case CP_MEASUREMENT_FLAG_EXTREME_FORCE_MAGNITUDES_PRESENT:
            little_endian_store_16(value, pos, (uint16_t)instance->maximum_force_magnitude_N);
            pos += 2;
            little_endian_store_16(value, pos, (uint16_t)instance->minimum_force_magnitude_N);
            pos += 2;
            break;
        case CP_MEASUREMENT_FLAG_EXTREME_TORQUE_MAGNITUDES_PRESENT:
            little_endian_store_16(value, pos, (uint16_t)instance->maximum_torque_magnitude_Nm);
            pos += 2;
            little_endian_store_16(value, pos, (uint16_t)instance->minimum_torque_magnitude_Nm);
            pos += 2;
            break;
        case CP_MEASUREMENT_FLAG_EXTREME_ANGLES_PRESENT:
            little_endian_store_24(value, pos, (instance->maximum_angle_degree << 12) | instance->minimum_angle_degree);
            pos += 3;
            break;
        case CP_MEASUREMENT_FLAG_TOP_DEAD_SPOT_ANGLE_PRESENT:
            little_endian_store_16(value, pos, (uint16_t)instance->top_dead_spot_angle_degree);
            pos += 2;
            break;
        case CP_MEASUREMENT_FLAG_BOTTOM_DEAD_SPOT_ANGLE_PRESENT:
            little_endian_store_16(value, pos, (uint16_t)instance->bottom_dead_spot_angle_degree);
            pos += 2;
            break;
        case CP_MEASUREMENT_FLAG_ACCUMULATED_ENERGY_PRESENT:
            little_endian_store_16(value, pos, (uint16_t)instance->accumulated_energy_kJ);
            pos += 2;
            break;
        default:
            break;
    }
    return pos;
}


static int cycling_power_store_measurement(cycling_power_t * instance, uint8_t * value, uint16_t max_value_size){
    if (max_value_size < 4u) return 0u;
    if (!instance) return 0;

    uint16_t measurement_flags = cycling_power_service_get_measurement_flags(instance);
    int pos = 0;
    little_endian_store_16(value, 0, measurement_flags);
    pos += 2;
    little_endian_store_16(value, 2, instance->instantaneous_power_W);
    pos += 2;
    int flag_index;
    uint16_t bytes_left = max_value_size - pos;
    for (flag_index = 0; flag_index < CP_MEASUREMENT_FLAG_RESERVED; flag_index++){
        if ((measurement_flags & (1u << flag_index)) == 0u) continue;
        cycling_power_measurement_flag_t flag = (cycling_power_measurement_flag_t) flag_index;
        uint16_t value_size = cycling_power_measurement_flag_value_size(flag);
        if (value_size > bytes_left ) return pos;
        cycling_power_store_measurement_flag_value(instance, flag, &value[pos]);
        pos += value_size;
        bytes_left -= value_size;
    }
    return pos;
}

int cycling_power_get_measurement_adv(uint16_t adv_interval, uint8_t * adv_buffer, uint16_t adv_size){
    if (adv_size < 12u) return 0u;
    cycling_power_t * instance =  &cycling_power;
    int pos = 0;
    // adv flags
    adv_buffer[pos++] = 2;
    adv_buffer[pos++] = BLUETOOTH_DATA_TYPE_FLAGS;
    adv_buffer[pos++] = 0x4;

    // adv interval
    adv_buffer[pos++] = 3;
    adv_buffer[pos++] = BLUETOOTH_DATA_TYPE_ADVERTISING_INTERVAL;
    little_endian_store_16(adv_buffer, pos, adv_interval);
    pos += 2;
    //
    int value_len = cycling_power_store_measurement(instance, &adv_buffer[pos + 4], CYCLING_POWER_MAX_BROACAST_MSG_SIZE - (pos + 4));
    adv_buffer[pos++] = 3 + value_len;
    adv_buffer[pos++] = BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID;
    little_endian_store_16(adv_buffer, pos, ORG_BLUETOOTH_SERVICE_CYCLING_POWER);
    pos += 2;
    // value data already in place cycling_power_get_measurement
    pos += value_len;
    // set ADV_NONCONN_IND
    return pos;
}

static void cycling_power_service_broadcast_can_send_now(void * context){
    cycling_power_t * instance = (cycling_power_t *) context;
    if (!instance){
        log_error("cycling_power_service_broadcast_can_send_now: instance is null");
        return;
    }
    uint8_t value[CYCLING_POWER_MAX_BROACAST_MSG_SIZE];
    int pos = cycling_power_store_measurement(instance, &value[0], sizeof(value));
    att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos);
}

static void cycling_power_service_measurement_can_send_now(void * context){
    cycling_power_t * instance = (cycling_power_t *) context;
    if (!instance){
        log_error("cycling_power_service_measurement_can_send_now: instance is null");
        return;
    }
    uint8_t value[40];
    int pos = cycling_power_store_measurement(instance, &value[0], sizeof(value));
    att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos);
}

static void cycling_power_service_response_can_send_now(void * context){
    cycling_power_t * instance = (cycling_power_t *) context;
    if (!instance){
        log_error("cycling_power_service_response_can_send_now: instance is null");
        return;
    }

    if (instance->response_value == CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE){
        log_error("cycling_power_service_response_can_send_now: CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE");
        return;
    }

    // use preprocessor instead of btstack_max to get compile-time constant
#if (CP_SENSOR_LOCATION_RESERVED > (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE + 5))
    #define MAX_RESPONSE_PAYLOAD CP_SENSOR_LOCATION_RESERVED
#else
    #define MAX_RESPONSE_PAYLOAD (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE + 5)
#endif

    uint8_t value[3 + MAX_RESPONSE_PAYLOAD];
    int pos = 0;
    value[pos++] = CP_OPCODE_RESPONSE_CODE;
    value[pos++] = instance->request_opcode;
    value[pos++] = instance->response_value;
    if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){
        switch (instance->request_opcode){
            case CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS:{
                int i;
                for (i=0; i<instance->num_supported_sensor_locations; i++){
                    value[pos++] = instance->supported_sensor_locations[i];
                }
                break;
            }
            case CP_OPCODE_REQUEST_CRANK_LENGTH:
                little_endian_store_16(value, pos, instance->crank_length_mm);
                pos += 2;
                break;
            case CP_OPCODE_REQUEST_CHAIN_LENGTH:
                little_endian_store_16(value, pos, instance->chain_length_mm);
                pos += 2;
                break;
            case CP_OPCODE_REQUEST_CHAIN_WEIGHT:
                little_endian_store_16(value, pos, instance->chain_weight_g);
                pos += 2;
                break;
            case CP_OPCODE_REQUEST_SPAN_LENGTH:
                little_endian_store_16(value, pos, instance->span_length_mm);
                pos += 2;
                break;
            case CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE:
                little_endian_store_16(value, pos, instance->factory_calibration_date.year);
                pos += 2;
                value[pos++] = instance->factory_calibration_date.month;
                value[pos++] = instance->factory_calibration_date.day;
                value[pos++] = instance->factory_calibration_date.hours;
                value[pos++] = instance->factory_calibration_date.minutes;
                value[pos++] = instance->factory_calibration_date.seconds;
                break;
            case CP_OPCODE_REQUEST_SAMPLING_RATE:
                value[pos++] = instance->sampling_rate_Hz;
                break;
            case CP_OPCODE_START_OFFSET_COMPENSATION:
            case CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION:{
                uint16_t calibrated_value = 0xffff;
                if (has_feature(CP_FEATURE_FLAG_EXTREME_MAGNITUDES_SUPPORTED)){
                    if (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_FORCE) {
                        calibrated_value = instance->current_force_magnitude_N;
                    } else if (has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) == CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE){
                        calibrated_value = instance->current_torque_magnitude_Nm;
                    }
                }

                if (calibrated_value == CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION){
                     value[pos++] = (uint8_t) calibrated_value;
                     // do not include manufacturer ID and data
                     break;
                } else if (calibrated_value == CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS){
                    value[pos++] = (uint8_t) calibrated_value;
                } else {
                    little_endian_store_16(value, pos, calibrated_value);
                    pos += 2;
                }

                if (instance->request_opcode == CP_OPCODE_START_OFFSET_COMPENSATION) break;
                little_endian_store_16(value, pos, instance->manufacturer_company_id);
                pos += 2;
                int data_len = (instance->num_manufacturer_specific_data < CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE) ? instance->num_manufacturer_specific_data : (CYCLING_POWER_MANUFACTURER_SPECIFIC_DATA_MAX_SIZE - 1);
                value[pos++] = data_len;
                (void)memcpy(&value[pos],
                             instance->manufacturer_specific_data, data_len);
                pos += data_len;
                value[pos++] = 0;
                break;
            }
            case CP_OPCODE_MASK_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT:
                break;
            default:
                break;
        }
    }
    uint8_t status = att_server_indicate(instance->con_handle, instance->control_point_value_handle, &value[0], pos);
    if (status == ERROR_CODE_SUCCESS){
        instance->w4_indication_complete = 1;
        instance->request_opcode = CP_OPCODE_IDLE;
    } else {
        log_error("can_send_now failed 0x%2x", status);
    }
}

static void cycling_power_service_server_emit_start_calibration(const cycling_power_t *instance, bool enhanced) {

    cycling_power_sensor_measurement_context_t measurement_type =
            has_feature(CP_FEATURE_FLAG_SENSOR_MEASUREMENT_CONTEXT) ? CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE : CP_SENSOR_MEASUREMENT_CONTEXT_FORCE;

    uint8_t event[7];
    int index = 0;
    event[index++] = HCI_EVENT_GATTSERVICE_META;
    event[index++] = sizeof(event) - 2u;
    event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_START_CALIBRATION;
    little_endian_store_16(event, index, instance->con_handle);
    index += 2;
    event[index++] = (uint8_t) measurement_type;
    event[index++] = enhanced ? 1 : 0;
    (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
}

static int cycling_power_service_write_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t transaction_mode, uint16_t offset, uint8_t *buffer, uint16_t buffer_size){
    UNUSED(con_handle);
    UNUSED(offset);
    UNUSED(buffer_size);
    int i;
    cycling_power_sensor_location_t location;
    cycling_power_t * instance = &cycling_power;

    if (transaction_mode != ATT_TRANSACTION_MODE_NONE){
        return 0;
    }

    if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){
        if (buffer_size < 2u){
            return ATT_ERROR_INVALID_OFFSET;
        }
        instance->measurement_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0);
        instance->con_handle = con_handle;
        log_info("cycling_power_service_write_callback: measurement enabled %d", instance->measurement_client_configuration_descriptor_notify);
        return 0;
    }

    if (attribute_handle == instance->measurement_server_configuration_descriptor_handle){
        if (buffer_size < 2u){
            return ATT_ERROR_INVALID_OFFSET;
        }
        instance->measurement_server_configuration_descriptor_broadcast = little_endian_read_16(buffer, 0);
        instance->con_handle = con_handle;
        uint8_t event[5];
        int index = 0;
        event[index++] = HCI_EVENT_GATTSERVICE_META;
        event[index++] = sizeof(event) - 2u;

        if (instance->measurement_server_configuration_descriptor_broadcast){
            event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_START;
            log_info("cycling_power_service_write_callback: start broadcast");
        } else {
            event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_STOP;
            log_info("cycling_power_service_write_callback: stop broadcast");
        }
        little_endian_store_16(event, index, con_handle);
        index += 2;
        (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));
        return 0;
    }

    if (attribute_handle == instance->vector_client_configuration_descriptor_handle){
        if (buffer_size < 2u){
            return ATT_ERROR_INVALID_OFFSET;
        }
        instance->con_handle = con_handle;

#ifdef ENABLE_ATT_DELAYED_RESPONSE
        switch (instance->con_interval_status){
            case CP_CONNECTION_INTERVAL_STATUS_REJECTED:
                return CYCLING_POWER_ERROR_CODE_INAPPROPRIATE_CONNECTION_PARAMETERS;

            case CP_CONNECTION_INTERVAL_STATUS_ACCEPTED:
            case CP_CONNECTION_INTERVAL_STATUS_RECEIVED:
                if ((instance->con_interval > instance->con_interval_max) || (instance->con_interval < instance->con_interval_min)){
                    instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE;
                    gap_request_connection_parameter_update(instance->con_handle, instance->con_interval_min, instance->con_interval_max, 4, 100);    // 15 ms, 4, 1s
                    return ATT_ERROR_WRITE_RESPONSE_PENDING;
                }
                instance->vector_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0);
                return 0;
            default:
                return ATT_ERROR_WRITE_RESPONSE_PENDING;

        }
#endif
    }

    if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){
        if (buffer_size < 2u){
            return ATT_ERROR_INVALID_OFFSET;
        }
        instance->control_point_client_configuration_descriptor_indicate = little_endian_read_16(buffer, 0);
        instance->con_handle = con_handle;
        log_info("cycling_power_service_write_callback: indication enabled %d", instance->control_point_client_configuration_descriptor_indicate);
        return 0;
    }

    if (attribute_handle == instance->feature_value_handle){
        if (buffer_size < 4u){
            return ATT_ERROR_INVALID_OFFSET;
        }
        instance->feature_flags = little_endian_read_32(buffer, 0);
        return 0;
    }

    if (attribute_handle == instance->control_point_value_handle){
        if (instance->control_point_client_configuration_descriptor_indicate == 0u) return CYCLING_POWER_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED;
        if (instance->w4_indication_complete != 0u){
            return CYCLING_POWER_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS;
        }
        int pos = 0;
        instance->request_opcode = (cycling_power_opcode_t) buffer[pos++];
        instance->response_value = CP_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED;

        switch (instance->request_opcode){
            case CP_OPCODE_SET_CUMULATIVE_VALUE:
                if (!has_feature(CP_FEATURE_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED)) break;
                instance->cumulative_wheel_revolutions = little_endian_read_32(buffer, pos);
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS:
                if (!has_feature(CP_FEATURE_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED)) break;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_UPDATE_SENSOR_LOCATION:
                if (!has_feature(CP_FEATURE_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED)) break;
                location = (cycling_power_sensor_location_t) buffer[pos];
                instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER;
                for (i=0; i<instance->num_supported_sensor_locations; i++){
                    if (instance->supported_sensor_locations[i] == location){
                        instance->sensor_location = location;
                        instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                        break;
                    }
                }
                break;

            case CP_OPCODE_REQUEST_CRANK_LENGTH:
                if (!has_feature(CP_FEATURE_FLAG_CRANK_LENGTH_ADJUSTMENT_SUPPORTED)) break;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;
            case CP_OPCODE_SET_CRANK_LENGTH:
                if (!has_feature(CP_FEATURE_FLAG_CRANK_LENGTH_ADJUSTMENT_SUPPORTED)) break;
                instance->crank_length_mm = little_endian_read_16(buffer, pos);
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_REQUEST_CHAIN_LENGTH:
                if (!has_feature(CP_FEATURE_FLAG_CHAIN_LENGTH_ADJUSTMENT_SUPPORTED)) break;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;
            case CP_OPCODE_SET_CHAIN_LENGTH:
                if (!has_feature(CP_FEATURE_FLAG_CHAIN_LENGTH_ADJUSTMENT_SUPPORTED)) break;
                instance->chain_length_mm = little_endian_read_16(buffer, pos);
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_REQUEST_CHAIN_WEIGHT:
                if (!has_feature(CP_FEATURE_FLAG_CHAIN_WEIGHT_ADJUSTMENT_SUPPORTED)) break;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;
            case CP_OPCODE_SET_CHAIN_WEIGHT:
                if (!has_feature(CP_FEATURE_FLAG_CHAIN_WEIGHT_ADJUSTMENT_SUPPORTED)) break;
                instance->chain_weight_g = little_endian_read_16(buffer, pos);
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_REQUEST_SPAN_LENGTH:
                if (!has_feature(CP_FEATURE_FLAG_SPAN_LENGTH_ADJUSTMENT_SUPPORTED)) break;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;
            case CP_OPCODE_SET_SPAN_LENGTH:
                if (!has_feature(CP_FEATURE_FLAG_SPAN_LENGTH_ADJUSTMENT_SUPPORTED)) break;
                instance->span_length_mm = little_endian_read_16(buffer, pos);
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_REQUEST_FACTORY_CALIBRATION_DATE:
                if (!has_feature(CP_FEATURE_FLAG_FACTORY_CALIBRATION_DATE_SUPPORTED)) break;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_REQUEST_SAMPLING_RATE:
                if (!instance->vector_value_handle) break;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;

            case CP_OPCODE_START_OFFSET_COMPENSATION:
                if (has_feature(CP_FEATURE_FLAG_OFFSET_COMPENSATION_SUPPORTED)){
                    instance->response_value = CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE;
                    cycling_power_service_server_emit_start_calibration(instance, false);
                } else {
                    instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER;
                }
                break;

            case CP_OPCODE_START_ENHANCED_OFFSET_COMPENSATION:
                if (has_feature(CP_FEATURE_FLAG_ENHANCED_OFFSET_COMPENSATION_SUPPORTED)){
                    instance->response_value = CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE;
                    cycling_power_service_server_emit_start_calibration(instance, true);
                } else {
                    instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER;
                }
                break;

            case CP_OPCODE_MASK_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT:{
                if (!has_feature(CP_FEATURE_FLAG_CYCLING_POWER_MEASUREMENT_CHARACTERISTIC_CONTENT_MASKING_SUPPORTED)) break;
                uint16_t mask_bitmap = little_endian_read_16(buffer, pos);
                uint16_t masked_measurement_flags = instance->default_measurement_flags;
                uint16_t index = 0;

                for (i = 0; i < CP_MASK_BIT_RESERVED; i++){
                    uint8_t clear_bit = (mask_bitmap & (1u << i)) ? 1u : 0u;

                    masked_measurement_flags &= ~(clear_bit << index);
                    index++;
                    // following measurement flags have additional flag
                    switch ((cycling_power_mask_bit_t)i){
                        case CP_MASK_BIT_PEDAL_POWER_BALANCE:
                        case CP_MASK_BIT_ACCUMULATED_TORQUE:
                        case CP_MASK_BIT_EXTREME_MAGNITUDES:
                            masked_measurement_flags &= ~(clear_bit << index);
                            index++;
                            break;
                        default:
                            break;
                    }
                }
                instance->masked_measurement_flags = masked_measurement_flags;
                instance->response_value = CP_RESPONSE_VALUE_SUCCESS;
                break;
            }
            default:
                break;
        }

        if (instance->control_point_client_configuration_descriptor_indicate){
            instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now;
            instance->control_point_indicate_callback.context  = (void*) instance;
            att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle);
        }
        return 0;
    }
    return 0;
}

static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
    UNUSED(channel);
    UNUSED(size);
    cycling_power_t * instance = &cycling_power;
    uint8_t event_type = hci_event_packet_get_type(packet);
    uint16_t con_handle;

    if (packet_type != HCI_EVENT_PACKET) return;
    switch (event_type){
        case HCI_EVENT_META_GAP:
            switch (hci_event_gap_meta_get_subevent_code(packet)) {
                case GAP_SUBEVENT_LE_CONNECTION_COMPLETE:
                    instance->con_handle = gap_subevent_le_connection_complete_get_connection_handle(packet);
                    // print connection parameters (without using float operations)
                    instance->con_interval = gap_subevent_le_connection_complete_get_conn_interval(packet);
                    instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_RECEIVED;
                    break;
                default:
                    break;
            }
            break;
        case HCI_EVENT_LE_META:
            switch (hci_event_le_meta_get_subevent_code(packet)){
#ifdef ENABLE_ATT_DELAYED_RESPONSE
                case HCI_SUBEVENT_LE_CONNECTION_UPDATE_COMPLETE:
                    if (instance->con_interval_status != CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE) return;

                    if ((instance->con_interval > instance->con_interval_max) || (instance->con_interval < instance->con_interval_min)){
                        instance->con_interval = hci_subevent_le_connection_update_complete_get_conn_interval(packet);
                        instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_ACCEPTED;
                    } else {
                        instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_REJECTED;
                    }
                    att_server_response_ready(l2cap_event_connection_parameter_update_response_get_handle(packet));
                    break;
#endif
                default:
                    break;
            }
            break;

#ifdef ENABLE_ATT_DELAYED_RESPONSE
        case L2CAP_EVENT_CONNECTION_PARAMETER_UPDATE_RESPONSE:
            if (instance->con_interval_status != CP_CONNECTION_INTERVAL_STATUS_W4_L2CAP_RESPONSE) return;

            if (l2cap_event_connection_parameter_update_response_get_result(packet) == ERROR_CODE_SUCCESS){
                instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_W4_UPDATE;
            } else {
                instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_REJECTED;
                att_server_response_ready(l2cap_event_connection_parameter_update_response_get_handle(packet));
            }
            break;
#endif

        case HCI_EVENT_DISCONNECTION_COMPLETE:{
            if (!instance) return;
            con_handle = hci_event_disconnection_complete_get_connection_handle(packet);
            if (con_handle == HCI_CON_HANDLE_INVALID) return;

            instance->masked_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED;
            instance->w4_indication_complete = 0;

            uint8_t event[5];
            int index = 0;
            event[index++] = HCI_EVENT_GATTSERVICE_META;
            event[index++] = sizeof(event) - 2u;

            event[index++] = GATTSERVICE_SUBEVENT_CYCLING_POWER_BROADCAST_STOP;
            little_endian_store_16(event, index, con_handle);
            index += 2;
            (*instance->calibration_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event));

            break;
        }
        case ATT_EVENT_HANDLE_VALUE_INDICATION_COMPLETE:
            instance->w4_indication_complete = 0;
            break;
        default:
            break;
     }
}

void cycling_power_service_server_init(uint32_t feature_flags,
                                       cycling_power_pedal_power_balance_reference_t pedal_power_balance_reference, cycling_power_torque_source_t torque_source,
                                       cycling_power_sensor_location_t * supported_sensor_locations, uint16_t num_supported_sensor_locations,
                                       cycling_power_sensor_location_t   current_sensor_location){

    cycling_power_t * instance = &cycling_power;
    // TODO: remove hardcoded initialization
    instance->con_interval_min = 6;
    instance->con_interval_max = 6;
    instance->con_interval_status = CP_CONNECTION_INTERVAL_STATUS_NONE;
    instance->w4_indication_complete = 0;

    hci_event_callback_registration.callback = &packet_handler;
    hci_add_event_handler(&hci_event_callback_registration);

    l2cap_event_callback_registration.callback = &packet_handler;
    l2cap_add_event_handler(&l2cap_event_callback_registration);

    instance->sensor_location = current_sensor_location;
    instance->num_supported_sensor_locations = 0;
    if (supported_sensor_locations != NULL){
        instance->num_supported_sensor_locations = num_supported_sensor_locations;
        instance->supported_sensor_locations = supported_sensor_locations;
    }

    instance->feature_flags = feature_flags;
    instance->default_measurement_flags = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED;
    instance->masked_measurement_flags  = CYCLING_POWER_MEASUREMENT_FLAGS_CLEARED;
    instance->pedal_power_balance_reference = pedal_power_balance_reference;
    instance->torque_source = torque_source;

    // get service handle range
    uint16_t start_handle = 0;
    uint16_t end_handle   = 0xffff;
    int service_found = gatt_server_get_handle_range_for_service_with_uuid16(ORG_BLUETOOTH_SERVICE_CYCLING_POWER, &start_handle, &end_handle);
    btstack_assert(service_found != 0);
	UNUSED(service_found);

    // get CP Mesurement characteristic value handle and client configuration handle
    instance->measurement_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_MEASUREMENT);
    instance->measurement_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_MEASUREMENT);
    instance->measurement_server_configuration_descriptor_handle = gatt_server_get_server_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_MEASUREMENT);

    // get CP Feature characteristic value handle and client configuration handle
    instance->feature_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_FEATURE);
    // get CP Sensor Location characteristic value handle and client configuration handle
    instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION);

    // get CP Vector characteristic value handle and client configuration handle
    instance->vector_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_VECTOR);
    instance->vector_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_VECTOR);

    // get Body Sensor Location characteristic value handle and client configuration handle
    instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION);

    // get SP Control Point characteristic value handle and client configuration handle
    instance->control_point_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_CONTROL_POINT);
    instance->control_point_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CYCLING_POWER_CONTROL_POINT);

    log_info("Measurement     value handle 0x%02x", instance->measurement_value_handle);
    log_info("M. Client Cfg   value handle 0x%02x", instance->measurement_client_configuration_descriptor_handle);
    log_info("M. Server Cfg   value handle 0x%02x", instance->measurement_server_configuration_descriptor_handle);

    log_info("Feature         value handle 0x%02x", instance->feature_value_handle);
    log_info("Sensor location value handle 0x%02x", instance->sensor_location_value_handle);

    log_info("Vector          value handle 0x%02x", instance->vector_value_handle);
    log_info("Vector Cfg.     value handle 0x%02x", instance->vector_client_configuration_descriptor_handle);

    log_info("Control Point   value handle 0x%02x", instance->control_point_value_handle);
    log_info("Control P. Cfg. value handle 0x%02x", instance->control_point_client_configuration_descriptor_handle);

    cycling_power_service.start_handle   = start_handle;
    cycling_power_service.end_handle     = end_handle;
    cycling_power_service.read_callback  = &cycling_power_service_read_callback;
    cycling_power_service.write_callback = &cycling_power_service_write_callback;
    cycling_power_service.packet_handler = &packet_handler;
    att_server_register_service_handler(&cycling_power_service);
}


void cycling_power_service_server_add_torque(int16_t torque_Nm){
    cycling_power_t * instance = &cycling_power;
    instance->accumulated_torque_Nm += torque_Nm;
}

void cycling_power_service_server_add_wheel_revolution(int32_t wheel_revolution, uint16_t wheel_event_time_s){
    cycling_power_t * instance = &cycling_power;
    instance->last_wheel_event_time_s = wheel_event_time_s;
    if (wheel_revolution < 0){
        uint32_t wheel_revolution_to_subtract = (uint32_t) (-wheel_revolution);
        if (instance->cumulative_wheel_revolutions > wheel_revolution_to_subtract){
            instance->cumulative_wheel_revolutions -= wheel_revolution_to_subtract;
        } else {
            instance->cumulative_wheel_revolutions = 0;
        }
    } else {
        if (instance->cumulative_wheel_revolutions < (0xffffffff - wheel_revolution)){
            instance->cumulative_wheel_revolutions += wheel_revolution;
        } else {
            instance->cumulative_wheel_revolutions = 0xffffffff;
        }
    }
}

void cycling_power_service_server_add_crank_revolution(uint16_t crank_revolution, uint16_t crank_event_time_s){
    cycling_power_t * instance = &cycling_power;
    instance->last_crank_event_time_s = crank_event_time_s;
    instance->cumulative_crank_revolutions += crank_revolution;
}

void cycling_power_service_add_energy(uint16_t energy_kJ){
    cycling_power_t * instance = &cycling_power;
    if (instance->accumulated_energy_kJ <= (0xffffu - energy_kJ)){
        instance->accumulated_energy_kJ += energy_kJ;
    } else {
        instance->accumulated_energy_kJ = 0xffff;
    }
}

void cycling_power_service_server_set_instantaneous_power(int16_t instantaneous_power_W){
    cycling_power_t * instance = &cycling_power;
    instance->instantaneous_power_W = instantaneous_power_W;
}

void cycling_power_service_server_set_pedal_power_balance(uint8_t pedal_power_balance_percentage){
    cycling_power_t * instance = &cycling_power;
    instance->pedal_power_balance_percentage = pedal_power_balance_percentage;
}

void cycling_power_service_server_set_force_magnitude_values(int force_magnitude_count, int16_t * force_magnitude_N_array){
    cycling_power_t * instance = &cycling_power;
    instance->force_magnitude_count = force_magnitude_count;
    instance->vector_instantaneous_force_magnitude_N_array = force_magnitude_N_array;
}

void cycling_power_service_server_set_torque_magnitude_values(int torque_magnitude_count, int16_t * torque_magnitude_Nm_array){
    cycling_power_t * instance = &cycling_power;
    instance->torque_magnitude_count = torque_magnitude_count;
    instance->vector_instantaneous_torque_magnitude_Nm_array = torque_magnitude_Nm_array;
}

void cycling_power_service_server_set_first_crank_measurement_angle(uint16_t first_crank_measurement_angle_degree){
    cycling_power_t * instance = &cycling_power;
    instance->vector_first_crank_measurement_angle_degree = first_crank_measurement_angle_degree;
}

void cycling_power_service_server_set_instantaneous_measurement_direction(cycling_power_instantaneous_measurement_direction_t direction){
    cycling_power_t * instance = &cycling_power;
    instance->vector_instantaneous_measurement_direction = direction;
}

void cycling_power_service_server_set_force_magnitude(int16_t min_force_magnitude_N, int16_t max_force_magnitude_N){
    cycling_power_t * instance = &cycling_power;
    instance->minimum_force_magnitude_N = min_force_magnitude_N;
    instance->maximum_force_magnitude_N = max_force_magnitude_N;
}

void cycling_power_service_server_set_torque_magnitude(int16_t min_torque_magnitude_Nm, int16_t max_torque_magnitude_Nm){
    cycling_power_t * instance = &cycling_power;
    instance->minimum_torque_magnitude_Nm = min_torque_magnitude_Nm;
    instance->maximum_torque_magnitude_Nm = max_torque_magnitude_Nm;
}

void cycling_power_service_server_set_angle(uint16_t min_angle_degree, uint16_t max_angle_degree){
    cycling_power_t * instance = &cycling_power;
    instance->minimum_angle_degree = min_angle_degree;
    instance->maximum_angle_degree = max_angle_degree;
}

void cycling_power_service_server_set_top_dead_spot_angle(uint16_t top_dead_spot_angle_degree){
    cycling_power_t * instance = &cycling_power;
    instance->top_dead_spot_angle_degree = top_dead_spot_angle_degree;
}

void cycling_power_service_server_set_bottom_dead_spot_angle(uint16_t bottom_dead_spot_angle_degree){
    cycling_power_t * instance = &cycling_power;
    instance->bottom_dead_spot_angle_degree = bottom_dead_spot_angle_degree;
}

static int gatt_date_is_valid(gatt_date_time_t date){
    if ((date.year != 0u) && ((date.year < 1582u) || (date.year > 9999u))) return 0u;
    if ((date.month != 0u) && (date.month > 12u)) return 0u;
    if ((date.day != 0u) && (date.day > 31u)) return 0u;

    if (date.hours > 23u) return 0u;
    if (date.minutes > 59u) return 0u;
    if (date.seconds > 59u) return 0u;
    return 1;
}

int cycling_power_service_server_set_factory_calibration_date(gatt_date_time_t date){
    if (!gatt_date_is_valid(date)) return 0;

    cycling_power_t * instance = &cycling_power;
    instance->factory_calibration_date = date;
    return 1;
}

void cycling_power_service_server_set_sampling_rate(uint8_t sampling_rate_Hz){
    cycling_power_t * instance = &cycling_power;
    instance->sampling_rate_Hz = sampling_rate_Hz;
}


void cycling_power_service_server_update_values(void){
    cycling_power_t * instance = &cycling_power;

    if (instance->measurement_server_configuration_descriptor_broadcast){
        instance->measurement_broadcast_callback.callback = &cycling_power_service_broadcast_can_send_now;
        instance->measurement_broadcast_callback.context  = (void*) instance;
        att_server_register_can_send_now_callback(&instance->measurement_broadcast_callback, instance->con_handle);
    }

    if (instance->measurement_client_configuration_descriptor_notify){
        instance->measurement_notify_callback.callback = &cycling_power_service_measurement_can_send_now;
        instance->measurement_notify_callback.context  = (void*) instance;
        att_server_register_can_send_now_callback(&instance->measurement_notify_callback, instance->con_handle);
    }

    if (instance->vector_client_configuration_descriptor_notify){
        instance->vector_notify_callback.callback = &cycling_power_service_vector_can_send_now;
        instance->vector_notify_callback.context  = (void*) instance;
        att_server_register_can_send_now_callback(&instance->vector_notify_callback, instance->con_handle);
    }
}

void cycling_power_service_server_packet_handler(btstack_packet_handler_t callback){
    if (callback == NULL){
        log_error("cycling_power_service_server_packet_handler called with NULL callback");
        return;
    }
    cycling_power_t * instance = &cycling_power;
    instance->calibration_callback = callback;
}

void cycling_power_server_calibration_done(cycling_power_sensor_measurement_context_t measurement_type, uint16_t calibrated_value){
    cycling_power_t * instance = &cycling_power;
    if (instance->response_value != CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE){
        return;
    }
    instance->response_value = CP_RESPONSE_VALUE_SUCCESS;

    switch (measurement_type){
        case CP_SENSOR_MEASUREMENT_CONTEXT_FORCE:
            instance->current_force_magnitude_N = calibrated_value;
            break;
        case CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE:
            instance->current_torque_magnitude_Nm = calibrated_value;
            break;
        default:
            instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER;
            break;
    }

    if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){
        switch (calibrated_value){
            case CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION:
            case CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS:
                instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED;
                instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED;
                break;
            default:
                break;
        }
    }

    if (instance->control_point_client_configuration_descriptor_indicate){
        instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now;
        instance->control_point_indicate_callback.context  = (void*) instance;
        att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle);
    }
}

void cycling_power_server_enhanced_calibration_done(cycling_power_sensor_measurement_context_t measurement_type,
                uint16_t calibrated_value, uint16_t manufacturer_company_id,
                uint8_t num_manufacturer_specific_data, uint8_t * manufacturer_specific_data){
    cycling_power_t * instance = &cycling_power;
    if (instance->response_value != CP_RESPONSE_VALUE_W4_VALUE_AVAILABLE) return;
    instance->response_value = CP_RESPONSE_VALUE_SUCCESS;

    switch (measurement_type){
        case CP_SENSOR_MEASUREMENT_CONTEXT_FORCE:
            instance->current_force_magnitude_N = calibrated_value;
            break;
        case CP_SENSOR_MEASUREMENT_CONTEXT_TORQUE:
            instance->current_torque_magnitude_Nm = calibrated_value;
            break;
        default:
            instance->response_value = CP_RESPONSE_VALUE_INVALID_PARAMETER;
            break;
    }

    if (instance->response_value == CP_RESPONSE_VALUE_SUCCESS){
        switch (calibrated_value){
            case CP_CALIBRATION_STATUS_INCORRECT_CALIBRATION_POSITION:
            case CP_CALIBRATION_STATUS_MANUFACTURER_SPECIFIC_ERROR_FOLLOWS:
                instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED;
                instance->response_value = CP_RESPONSE_VALUE_OPERATION_FAILED;
                break;
            default:
                break;
        }
        instance->manufacturer_company_id = manufacturer_company_id;
        instance->num_manufacturer_specific_data = num_manufacturer_specific_data;
        instance->manufacturer_specific_data = manufacturer_specific_data;
    }

    if (instance->control_point_client_configuration_descriptor_indicate){
        instance->control_point_indicate_callback.callback = &cycling_power_service_response_can_send_now;
        instance->control_point_indicate_callback.context  = (void*) instance;
        att_server_register_can_send_now_callback(&instance->control_point_indicate_callback, instance->con_handle);
    }
}