#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 "core/ble.h"
#include "core/routine.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 x")

/** 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_16 characteristic_format_uuid = {
    .uuid = { .type = BT_UUID_TYPE_16 },
    .val = BT_UUID_GATT_CPF_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 },
};

static struct bt_uuid_128 routine_list_characteristic_uuid = {
    .uuid = { .type = BT_UUID_TYPE_128 },
    .val = { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 },
};

static struct bt_uuid_128 current_routine_characteristic_uuid = {
    .uuid = { .type = BT_UUID_TYPE_128 },
    .val = { 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5 },
};

/*** Characteristic Data ***/
static struct bt_gatt_cpf string_format = {
    .format = 0x19, /* UTF8 string */
    .exponent = 0,
    .unit = 0,
    .name_space = 0x01, /* Bluetooth SIG */
    .description = 0,
};

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 = (struct bt_uuid *)&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";


/**** Routine List Characteristic ****/
extern const routine_listing_t *routine_list;
struct bt_gatt_chrc routine_list_characteristic = {
    .uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
    .properties = BT_GATT_CHRC_READ,
};
char routine_list_characteristic_buffer[512];
ble_attribute_userdata_t routine_list_characterist_userdata = {
    .buffer = routine_list_characteristic_buffer,
    .buffer_size = sizeof(routine_list_characteristic_buffer),
    .value_size = 0,
};
char routine_list_characteristic_description[] = "Routine List";



/**** Current Routine Characteristic ****/
struct bt_gatt_chrc current_routine_characteristic = {
    .uuid = (struct bt_uuid *)&current_routine_characteristic_uuid,
    .properties = BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
};
static ssize_t _read_current_routine(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    void *buffer,
    uint16_t buffer_len,
    uint16_t offset
);
static ssize_t _write_current_routine(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    const void *buffer,
    uint16_t buffer_len,
    uint16_t offset,
    uint8_t flags
);
char current_routine_characteristic_description[] = "Current Routine";


/*** 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,
    },
    {
        .uuid = (struct bt_uuid *)&characteristic_format_uuid,
        .read = bt_gatt_attr_read_cpf,
        .user_data = (void *)&string_format,
        .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,
    },
    {
        .uuid = (struct bt_uuid *)&characteristic_format_uuid,
        .read = bt_gatt_attr_read_cpf,
        .user_data = (void *)&string_format,
        .perm = BT_GATT_PERM_READ,
    },
    /** Routine List Characteristic **/
    {
        .uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
        .read = bt_gatt_attr_read_chrc,
        .user_data = (void *)&routine_list_characteristic,
        .perm = BT_GATT_PERM_READ,
    },
    {
        .uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
        .read = _generic_attribute_read,
        .user_data = (void *)&routine_list_characterist_userdata,
        .perm = BT_GATT_PERM_READ,
    },
    {
        .uuid = (struct bt_uuid *)&characteristic_description_uuid,
        .read = bt_gatt_attr_read_cud,
        .user_data = (void *)routine_list_characteristic_description,
        .perm = BT_GATT_PERM_READ,
    },
    {
        .uuid = (struct bt_uuid *)&characteristic_format_uuid,
        .read = bt_gatt_attr_read_cpf,
        .user_data = (void *)&string_format,
        .perm = BT_GATT_PERM_READ,
    },
    /** Current Routine Characteristic */
    {
        .uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
        .read = bt_gatt_attr_read_chrc,
        .user_data = (void *)&current_routine_characteristic,
        .perm = BT_GATT_PERM_READ,
    },
    {
        .uuid = (struct bt_uuid *)&current_routine_characteristic_uuid,
        .read = _read_current_routine,
        .write = _write_current_routine,
        .perm = BT_GATT_PERM_READ | BT_GATT_PERM_WRITE
    },
    {
        .uuid = (struct bt_uuid *)&characteristic_description_uuid,
        .read = bt_gatt_attr_read_cud,
        .user_data = (void *)current_routine_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);
#define BLE_FLAG_DISCONNECTED (0x01)
    uint32_t flags;
    /** 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);
    routine_get_listing(routine_list_characteristic_buffer, sizeof(routine_list_characteristic_buffer));
    routine_list_characterist_userdata.value_size = strlen(routine_list_characteristic_buffer);

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

    _start_advertising();

    return 0;
}

void
ble_tick()
{
    k_mutex_lock(&ble.lock, K_FOREVER);
    if (ble.flags & BLE_FLAG_DISCONNECTED) {
        _start_advertising();
        ble.flags &= ~BLE_FLAG_DISCONNECTED;
    }
    k_mutex_unlock(&ble.lock);
}

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_clear_oninput_callback()
{
    k_mutex_lock(&ble.lock, K_FOREVER);
    ble.oninput_callback = NULL;
    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 */
    k_mutex_lock(&ble.lock, K_FOREVER);
    ble.flags |= BLE_FLAG_DISCONNECTED;
    k_mutex_unlock(&ble.lock);
}

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

static ssize_t _read_current_routine(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    void *buffer,
    uint16_t buffer_len,
    uint16_t offset
) {
    if ((offset != 0) || (buffer_len < sizeof(int8_t))) {
        return (0);
    }

    ((int8_t *)buffer)[0] = routine_get_current();

    return (sizeof(int8_t));
}

static ssize_t _write_current_routine(
    struct bt_conn *connection,
    const struct bt_gatt_attr *attribute,
    const void *buffer,
    uint16_t buffer_len,
    uint16_t offset,
    uint8_t flags
) {
    uint8_t index;

    if ((offset != 0) || (buffer_len < sizeof(uint8_t))) {
        return (0);
    }

    index = ((uint8_t *)buffer)[0];

    if (routine_select(index)) {
        return (0);
    } else {
        return (sizeof(uint8_t));
    }
}