#include <zephyr/types.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <zephyr/sys/printk.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/kernel.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>

#include "bt_services.h"

#include <zephyr/logging/log.h>

LOG_MODULE_DECLARE(main, CONFIG_APP_LOG_LEVEL);

static sensors_raw_t sensors;
static struct bt_gatt_indicate_params ind_params;

// Callback definitions
static ssize_t read_sensors(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf,
                            uint16_t len, uint16_t offset)
{
    const char *value = attr->user_data;
    size_t size = sizeof(sensors_raw_t);

    // LOG_INF("Attribute read, handle: %u, conn: %p, bytes: %d", attr->handle, (void *)conn, size);

    return bt_gatt_attr_read(conn, attr, buf, len, offset, value, size);
}

// Service declaration

static ssize_t read_string(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset)
{
    const char *value = attr->user_data;
    LOG_INF("String read, handle: %u, conn: %p\t%s", attr->handle, (void *)conn, value);
    return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, strlen((char *)attr->user_data));
}

static void myimu_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
    ARG_UNUSED(attr);

    bool notif_enabled = (value == BT_GATT_CCC_NOTIFY);

    LOG_INF("IMU Notifications %s", notif_enabled ? "enabled" : "disabled");
}

BT_GATT_SERVICE_DEFINE(my_imu_svc, BT_GATT_PRIMARY_SERVICE(BT_UUID_IMU),
                       // Read raw sensor values
                       BT_GATT_CHARACTERISTIC(BT_UUID_IMU_SENSORS,
                                              BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
                                              BT_GATT_PERM_READ,
                                              read_sensors,
                                              NULL,
                                              &sensors),
                       BT_GATT_CCC(myimu_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
                       BT_GATT_DESCRIPTOR(BT_UUID_GATT_CUD,
                                          BT_GATT_PERM_READ,
                                          read_string,
                                          NULL,
                                          "Read raw sensor values"),
                       // TODO: add characteristic to read state estimate
);

void set_sensors(const sensors_raw_t *values)
{
    sensors = *values;

    int rc = bt_gatt_notify(NULL, &my_imu_svc.attrs[1], &sensors, sizeof(sensors));

    // return rc == -ENOTCONN ? 0 : rc;
}