#include <stdint.h>
#include <stddef.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gap.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/util_macro.h>
#include <zephyr/types.h>

#include "ble.h"

/**
 * @brief Begin BLE advertising with the default parameters
 */
static int _start_advertising(void);


/** Callbacks **/
/**
 * @brief Callback registered for when a BLE client connects to us
 */
static void _connected_callback(struct bt_conn *connection, uint8_t error);
/**
 * @brief Callback registered for when a BLE client disconnects from us
 */
static void _disconnected_callback(struct bt_conn *connection, uint8_t reason);

static struct bt_conn_cb connection_callbacks = {
    .connected = _connected_callback,
    .disconnected = _disconnected_callback,
};


/** Advertising data **/
/** Our name to advertise */
#define BLE_NAME_DEFAULT ("FM25 Kit 1")

/** Advertising/Scanning parameters */
static struct bt_le_adv_param advertise_params = {
    .id = BT_ID_DEFAULT,
    .interval_min = 800, /* (800 * 0.625) = 500ms */
    .interval_max = 800,
    .options = BT_LE_ADV_OPT_CONN,
};

/** Advertising buffer data */
static struct bt_data advertise_data[] = {
    {
        .type = BT_DATA_NAME_COMPLETE,
        .data = BLE_NAME_DEFAULT,
        .data_len = sizeof(BLE_NAME_DEFAULT) - 1,
    },
};

/** Scan buffer data */
static struct bt_data scan_data[] = {
    {
        .type = BT_DATA_NAME_COMPLETE,
        .data = BLE_NAME_DEFAULT,
        .data_len = sizeof(BLE_NAME_DEFAULT) - 1,
    },
};



/** BLE Service Data **/
/**
 * @brief Attribute read callback function for attributes that have a user data
 * type of ble_attribute_userdata_t
 */
static ssize_t _generic_attribute_read(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    void *buffer,
    uint16_t buffer_len,
    uint16_t offset
);

/**
 * @brief Attribute write callback function for attributes that have a user data
 * type of ble_attribute_userdata_t
 */
static ssize_t _generic_attribute_write(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    const void *buffer,
    uint16_t buffer_len,
    uint16_t offset,
    uint8_t flags
);

/*** UUID Data ***/
static struct bt_uuid_16 service_declaration_uuid = {
    .uuid = { .type = BT_UUID_TYPE_16 },
    .val = BT_UUID_GATT_PRIMARY_VAL,
};

static struct bt_uuid_16 characteristic_declaration_uuid = {
    .uuid = { .type = BT_UUID_TYPE_16 },
    .val = BT_UUID_GATT_CHRC_VAL,
};

static struct bt_uuid_16 characteristic_description_uuid = {
    .uuid = { .type = BT_UUID_TYPE_16 },
    .val = BT_UUID_GATT_CUD_VAL,
};

static struct bt_uuid_128 service_uuid = {
    .uuid = { .type = BT_UUID_TYPE_128 },
    .val = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
};

static struct bt_uuid_128 input_characteristic_uuid = {
    .uuid = { .type = BT_UUID_TYPE_128 },
    .val = { 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 },
};

static struct bt_uuid_128 telemetry_characteristic_uuid = {
    .uuid = { .type = BT_UUID_TYPE_128 },
    .val = { 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
};

/*** Characteristic Data ***/
typedef struct ble_attribute_userdata_struct {
    /** Buffer allocated for this attribute's data */
    void *buffer;
    /** Size of the buffer in bytes */
    uint16_t buffer_size;
    /** Size of the current value in the buffer */
    uint16_t value_size;
    /** Lock to protect the data in the buffer */
    struct k_mutex lock;
} ble_attribute_userdata_t;

/**** Input Characteristic ****/
/** Zephyr GATT characteristic struct data for the Input characteristic */
struct bt_gatt_chrc input_characteristic = {
    .uuid = (void *)&input_characteristic_uuid,
    .properties = BT_GATT_CHRC_WRITE,
};
/** Buffer for the contents of the Input characteristic */
uint8_t input_characteristic_buffer[32];
/** Userdata utilized in the write callback for the Input characteristic */
ble_attribute_userdata_t input_characteristic_userdata = {
    .buffer = input_characteristic_buffer,
    .buffer_size = sizeof(input_characteristic_buffer),
    .value_size = 0,
    /* lock initialized in ble_init */
};
/** Description of the Input characteristic */
char input_characteristic_description[] = "Input Data";


/**** Telemetry Characteristic ****/
struct bt_gatt_chrc telemetry_characteristic = {
    .uuid = (void *)&telemetry_characteristic_uuid,
    .properties = BT_GATT_CHRC_READ,
};
/** Buffer for the contents of the Telemetry characteristic */
uint8_t telemetry_characteristic_buffer[512];
/** Userdata utilized in the write callback for the Input characteristic */
ble_attribute_userdata_t telemetry_characteristic_userdata = {
    .buffer = telemetry_characteristic_buffer,
    .buffer_size = sizeof(telemetry_characteristic_buffer),
    .value_size = 0,
    /* lock initialized in ble_init */
};
/** Description of the telemetry characteristic */
char telemetry_characteristic_description[] = "Telemetry Data";


/*** Service Layout ***/
static struct bt_gatt_attr primary_service_attributes[] = {
    /* Service declaration */
    {
        .uuid = (struct bt_uuid *)&service_declaration_uuid,
        .read = bt_gatt_attr_read_service,
        .user_data = (void *)&service_uuid,
        .perm = BT_GATT_PERM_READ,
    },
    /* Characteristics */
    /** Control Input Characteristic **/
    {
        .uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
        .read = bt_gatt_attr_read_chrc,
        .user_data = (void *)&input_characteristic,
        .perm = BT_GATT_PERM_READ,
    },
    {
        .uuid = (struct bt_uuid *)&input_characteristic_uuid,
        .write = _generic_attribute_write,
        .user_data = (void *)&input_characteristic_userdata,
        .perm = BT_GATT_PERM_WRITE,
    },
    {
        .uuid = (struct bt_uuid *)&characteristic_description_uuid,
        .read = bt_gatt_attr_read_cud,
        .user_data = (void *)input_characteristic_description,
        .perm = BT_GATT_PERM_READ,
    },
    /** Telemetry Data Characteristic **/
    {
        .uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
        .read = bt_gatt_attr_read_chrc,
        .user_data = (void *)&telemetry_characteristic,
        .perm = BT_GATT_PERM_READ,
    },
    {
        .uuid = (struct bt_uuid *)&telemetry_characteristic_uuid,
        .read = _generic_attribute_read,
        .user_data = (void *)&telemetry_characteristic_userdata,
        .perm = BT_GATT_PERM_READ,
    },
    {
        .uuid = (struct bt_uuid *)&characteristic_description_uuid,
        .read = bt_gatt_attr_read_cud,
        .user_data = (void *)telemetry_characteristic_description,
        .perm = BT_GATT_PERM_READ,
    }
};

static struct bt_gatt_service primary_service = {
    .attrs = primary_service_attributes,
    .attr_count = sizeof(primary_service_attributes) / sizeof(struct bt_gatt_attr),
};


/** Other BLE Data **/
typedef struct ble_state_struct {
    /** Current callback for a complete input write */
    void (*oninput_callback)(const void *buffer, uint16_t buffer_len);
    /** Lock for data in the BLE state */
    struct k_mutex lock;
} ble_state_t;

static ble_state_t ble = {
    .oninput_callback = NULL,
    /* lock initialized in ble_init */
};

int
ble_init()
{
    int ret;

    k_mutex_init(&ble.lock);

    ret = bt_enable(NULL);
    if (ret) {
        return ret;
    }

    bt_conn_cb_register(&connection_callbacks);

    /* Initialize the service data */
    k_mutex_init(&input_characteristic_userdata.lock);
    k_mutex_init(&telemetry_characteristic_userdata.lock);

    ret = bt_gatt_service_register(&primary_service);
    if (ret) {
        return ret;
    }

    _start_advertising();

    return 0;
}

void
ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buffer_len))
{
    k_mutex_lock(&ble.lock, K_FOREVER);
    ble.oninput_callback = callback;
    k_mutex_unlock(&ble.lock);
}

void
ble_set_telemetry_data(const void *buffer, uint16_t buffer_len)
{
    uint16_t copy_len = (
        buffer_len < telemetry_characteristic_userdata.buffer_size ?
            buffer_len : telemetry_characteristic_userdata.buffer_size
    );

    k_mutex_lock(&telemetry_characteristic_userdata.lock, K_FOREVER);
    memcpy(telemetry_characteristic_userdata.buffer, buffer, copy_len);
    telemetry_characteristic_userdata.value_size = copy_len;
    k_mutex_unlock(&telemetry_characteristic_userdata.lock);
}

static int
_start_advertising()
{
    return bt_le_adv_start(
        &advertise_params,
        advertise_data, sizeof(advertise_data) / sizeof(struct bt_data),
        scan_data, sizeof(scan_data) / sizeof(struct bt_data)
    );
}

static void
_connected_callback(struct bt_conn *connection, uint8_t error)
{
    if (error) {
        return;
    }

    /* Zephyr automatically stops advertising on a new connection */
}

static void
_disconnected_callback(struct bt_conn *connection, uint8_t reason)
{
    /* Resume advertising so we can be found again */
    _start_advertising();
}

static ssize_t
_generic_attribute_read(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    void *buffer,
    uint16_t buffer_len,
    uint16_t offset
) {
    ble_attribute_userdata_t *data = attribute->user_data;
    ssize_t read_len;

    /* Copy nothing on a read */
    if (offset >= data->value_size) {
        return 0;
    }

    /* Truncate reads if they would be too long */
    read_len = buffer_len;
    if ((offset + read_len) > data->value_size) {
        read_len = data->value_size - offset;
    }

    k_mutex_lock(&data->lock, K_FOREVER);
    memcpy(buffer, &((uint8_t *)data->buffer)[offset], read_len);
    k_mutex_unlock(&data->lock);

    return read_len;
}

static int _generic_attribute_write(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    const void *buffer,
    uint16_t buffer_len,
    uint16_t offset,
    uint8_t flags
) {
    ble_attribute_userdata_t *data = attribute->user_data;
    ssize_t write_len;

    if (offset >= data->buffer_size) {
        return 0;
    }

    write_len = buffer_len;
    if ((offset + write_len) > data->buffer_size) {
        write_len = data->buffer_size - offset;
    }

    k_mutex_lock(&data->lock, K_FOREVER);
    memcpy(&((uint8_t *)data->buffer)[offset], buffer, write_len);
    data->value_size = offset + write_len;
    k_mutex_unlock(&data->lock);

    if (data == &input_characteristic_userdata) {
        k_mutex_lock(&ble.lock, K_FOREVER);
        if (ble.oninput_callback) {
            ble.oninput_callback(data->buffer, data->value_size);
        }
        k_mutex_unlock(&ble.lock);
    }

    return write_len;
}