/* Standard and Zephyr libraries */
#include <zephyr.h>
#include <stdio.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <soc.h>
#include <sys/byteorder.h>
#include <sys/util.h>
#include <device.h>
#include <devicetree.h>
#include <drivers/sensor.h>

/* Bluetooth Stack Libraries */
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/conn.h>
#include <bluetooth/uuid.h>
#include <bluetooth/gatt.h>
#include <bluetooth/services/bas.h>
#include <bluetooth/services/dis.h>
#include <bluetooth/services/hrs.h>

/* Settings librairy for DIS */
#include <settings/settings.h>

/* Watchdog and task_wdt */
#include <drivers/watchdog.h>
#include <sys/reboot.h>
#include <task_wdt/task_wdt.h>

/* Logging module */
#include <logging/log.h>
LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);

/**************************************************************************
 * DECLARATIONS
 *************************************************************************/

void sensor_stop(void);
void sensor_start(void);

/**************************************************************************
 * BLUETOOTH FUNCTIONS
 *************************************************************************/

/** Device Name as defined in the prj.conf */
#define DEVICE_NAME CONFIG_BT_DEVICE_NAME
/** device name length */
#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)

static const struct bt_data ad[] = {
	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
	BT_DATA_BYTES(BT_DATA_UUID16_ALL,
		      BT_UUID_16_ENCODE(BT_UUID_HRS_VAL),
		      BT_UUID_16_ENCODE(BT_UUID_BAS_VAL),
		      BT_UUID_16_ENCODE(BT_UUID_DIS_VAL))
};

static void connected(struct bt_conn *conn, uint8_t err)
{
	if (err) {
		LOG_ERR("Connection failed (err 0x%02x)", err);
	} else {
		LOG_INF("Connected\n");
        sensor_start();
	}
}

static void disconnected(struct bt_conn *conn, uint8_t reason)
{
	LOG_INF("Disconnected (reason 0x%02x)", reason);
    sensor_stop();
}

static struct bt_conn_cb conn_callbacks = {
	.connected = connected,
	.disconnected = disconnected,
};

static void bt_ready(void)
{
	int err;

	LOG_INF("Bluetooth initialized");

	err = bt_le_adv_start(BT_LE_ADV_CONN_NAME, ad, ARRAY_SIZE(ad), NULL, 0);
	if (err) {
		LOG_ERR("Advertising failed to start (err %d)", err);
		return;
	}

	LOG_INF("Advertising successfully started");
}

static void bas_notify(void)
{
	uint8_t battery_level = bt_bas_get_battery_level();

	battery_level--;

	if (!battery_level) {
		battery_level = 100U;
	}

	bt_bas_set_battery_level(battery_level);
}

static void hrs_notify(uint8_t heartrate)
{
	bt_hrs_notify(heartrate);
}

static void ble_init(void)
{
    int err;

	err = bt_enable(NULL);
	if (err) {
		LOG_ERR("Bluetooth init failed (err %d)", err);
		return;
	}

	bt_ready();

	bt_conn_cb_register(&conn_callbacks);
}

/**************************************************************************
 * WATCHDOG FUNCTIONS
 *************************************************************************/

#define WATCHDOG_ACTIVE

/**
 * If the devicetree has a watchdog node, we get the watchdog device
 * from there. Otherwise, the task watchdog will be used without a
 * hardware watchdog fallback.
 */
#if DT_NODE_HAS_STATUS(DT_ALIAS(watchdog0), okay)
#define WDT_NODE DT_ALIAS(watchdog0)
#elif DT_HAS_COMPAT_STATUS_OKAY(nordic_nrf_watchdog)
#define WDT_NODE DT_COMPAT_GET_ANY_STATUS_OKAY(nordic_nrf_watchdog)
#endif

static void watchdog_main_init(void)
{
#ifdef WDT_NODE
    const struct device *hw_wdt_dev = DEVICE_DT_GET(WDT_NODE);
#else
    const struct device *hw_wdt_dev = NULL;
#endif

    if (!device_is_ready(hw_wdt_dev))
    {
        LOG_INF("Hardware watchdog %s is not ready; ignoring it.\n",
                log_strdup(hw_wdt_dev->name));
        hw_wdt_dev = NULL;
    }
    task_wdt_init(hw_wdt_dev);
}

void task_wdt_callback(int channel_id, void *user_data)
{
    LOG_ERR("Task watchdog channel %d callback, thread: %s\n",
            channel_id, log_strdup(k_thread_name_get((k_tid_t)user_data)));
    /*
	 * If the issue could be resolved, call task_wdt_feed(channel_id) here
	 * to continue operation.
	 *
	 * Otherwise we can perform some cleanup and reset the device.
	 */
    LOG_ERR("Resetting device...\n");

    sys_reboot(SYS_REBOOT_COLD);
}

/**************************************************************************
 * UPDATE VALUES FUNCTIONS
 *************************************************************************/

#define START_DELAY_MS              1000
#define SENSOR_SAMPLING_TIME_MS     1000
#define WATCHDOG_PROCESS_TIMEOUT_MS 10000

static K_SEM_DEFINE(sensor_ok, 0, 1);
static K_SEM_DEFINE(sensor_init_ok, 0, 1);

static void sensor_timer_handler(struct k_timer *dummy)
{
    k_sem_give(&sensor_ok);
}

K_TIMER_DEFINE(sensor_timer, sensor_timer_handler, NULL);

int sensor_init(void)
{
    int status = 0;

    /* Indicate to the processing thread that initialization is finished */
    k_sem_give(&sensor_init_ok);

    return status;
}

#ifdef WATCHDOG_ACTIVE
/** ID for the process watchdog */
static int processor_wdt_id;
#endif /* WATCHDOG_ACTIVE */

void sensor_stop(void)
{
    k_timer_stop(&sensor_timer);

#ifdef WATCHDOG_ACTIVE
    task_wdt_delete(processor_wdt_id);
#endif /* WATCHDOG_ACTIVE */

    LOG_INF("Sensor stopped.\n");
}

void sensor_start(void)
{
    /* start periodic timer that expires once every "samplingTime" */
    k_timer_start(&sensor_timer, K_MSEC(START_DELAY_MS),
                  K_MSEC(SENSOR_SAMPLING_TIME_MS));

#ifdef WATCHDOG_ACTIVE
    processor_wdt_id = task_wdt_add(WATCHDOG_PROCESS_TIMEOUT_MS,
                                        task_wdt_callback,
                                        (void *)k_current_get());
#endif /* WATCHDOG_ACTIVE */

    LOG_INF("Sensor started.\n");
}

void sensor_process_thread(void)
{
    /* Wait for the sensor to be initialized before starting the thread loop. */
    k_sem_take(&sensor_init_ok, K_FOREVER);

    static uint8_t sensor_data = 90;

    while (1)
    {
        /* Semaphore is available every sampling time */
        k_sem_take(&sensor_ok, K_FOREVER);

        /* Notify new sensor value */
	    sensor_data++;
	    if (sensor_data == 160U) {
		    sensor_data = 90U;
	    }
        hrs_notify(sensor_data);

#ifdef WATCHDOG_ACTIVE
        task_wdt_feed(processor_wdt_id);
#endif /* WATCHDOG_ACTIVE */
    }
}

/* Defines the processor thread with its stacksize and priority. */
K_THREAD_DEFINE(sensor_producer_thread_id, 512,
                sensor_process_thread, NULL, NULL,
                NULL, 5, 0, 0);

/**************************************************************************
 * MAIN
 *************************************************************************/

void main(void)
{
    watchdog_main_init();

    sensor_init();

    ble_init();

    while(1)
    {
        bas_notify();
        k_sleep(K_MSEC(5000));
    }
}