#include <SystemK.h>
#include "esp_log.h"
/* BLE */
#include "esp_nimble_hci.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "host/ble_hs.h"
#include "host/util/util.h"
#include "console/console.h"
#include "services/gap/ble_svc_gap.h"

static const char *TAG = "BLE";
static bool Host_And_Controller_Synced = false;
static bool Is_Scanning_And_Advertising = false;

static uint8_t Advertising_Data[BLE_KTAG_PACKET_TOTAL_SIZE] = {0x1E, 0xFF, 0xFF, 0xFF, 'K', 'T', 'a', 'g'};

// Forward declarations of NimBLE functions.
void ble_store_config_init(void);
static int ble_gap_event(struct ble_gap_event *event, void *arg);

/**
 * Initiates the GAP general discovery procedure.
 */
static void ble_scan(void)
{
    uint8_t own_addr_type;
    struct ble_gap_disc_params disc_params;
    int rc;

    /* Figure out address to use while advertising (no privacy for now) */
    rc = ble_hs_id_infer_auto(0, &own_addr_type);
    if (rc != 0)
    {
        KLOG_ERROR(TAG, "Error determining address type; rc=%d", rc);
        return;
    }

    /* Tell the controller not to filter duplicates; we want to process
     * repeated advertisements from the same device.
     */
    disc_params.filter_duplicates = 0;

    /**
     * Perform a passive scan.  I.e., don't send follow-up scan requests to
     * each advertiser.
     */
    disc_params.passive = 1;

    /* Use defaults for the rest of the parameters. */
    disc_params.itvl = BLE_GAP_SCAN_ITVL_MS(40);
    disc_params.window = BLE_GAP_SCAN_WIN_MS(30);
    disc_params.filter_policy = 0;
    disc_params.limited = 0;

    rc = ble_gap_disc(own_addr_type, BLE_HS_FOREVER, &disc_params, ble_gap_event, NULL);
    if (rc != 0)
    {
        KLOG_ERROR(TAG, "Error initiating GAP discovery procedure; rc=%d", rc);
    }
}

static void ble_advertise(void)
{
    struct ble_gap_adv_params adv_params;

    int rc = ble_gap_adv_set_data((const uint8_t *)&Advertising_Data, sizeof(Advertising_Data));
    if (rc != 0)
    {
        KLOG_ERROR(TAG, "Error setting advertisement data; rc=%d", rc);
        return;
    }

    /* Begin advertising. */
    memset(&adv_params, 0, sizeof adv_params);
    adv_params.conn_mode = BLE_GAP_CONN_MODE_NON;
    adv_params.disc_mode = BLE_GAP_DISC_MODE_NON;

    // N.B. High duty-cycle mode requires modification to NimBLE, or you will get an "Error enabling advertisement; rc=530"
    // adv_params.high_duty_cycle = 1;
    // adv_params.itvl_min = BLE_GAP_ADV_ITVL_MS(20);
    // adv_params.itvl_max = BLE_GAP_ADV_ITVL_MS(25);

    adv_params.itvl_min = BLE_GAP_ADV_ITVL_MS(32);
    adv_params.itvl_max = BLE_GAP_ADV_ITVL_MS(37);

    rc = ble_gap_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, BLE_HS_FOREVER,
                           &adv_params, ble_gap_event, NULL);
    if (rc != 0)
    {
        KLOG_ERROR(TAG, "Error enabling advertisement; rc=%d", rc);
        return;
    }
}

// https://mynewt.apache.org/latest/network/ble_setup/ble_sync_cb.html#reset
static void ble_on_reset(int reason)
{
    KLOG_ERROR(TAG, "Resetting state for reason %d.", reason);
    Host_And_Controller_Synced = false;
}

// https://mynewt.apache.org/latest/network/ble_setup/ble_sync_cb.html#sync
static void ble_on_sync(void)
{
    KLOG_INFO(TAG, "Host and controller synchronized.");

    /* Make sure we have proper identity address set (public preferred) */
    int rc = ble_hs_util_ensure_addr(0);
    assert(rc == 0);

    Host_And_Controller_Synced = true;
}

/**
 * The NimBLE host executes this callback when a GAP event occurs.  The
 * application associates a GAP event callback with each connection that is
 * established.  KTag uses the same callback for all connections.
 *
 * @param event                 The event being signalled.
 * @param arg                   Application-specified argument; unused by KTag.
 *
 * @return                      0 if the application successfully handled the
 *                              event; nonzero on failure.  The semantics
 *                              of the return code is specific to the
 *                              particular GAP event being signalled.
 */
static int ble_gap_event(struct ble_gap_event *event, void *arg)
{
    struct ble_hs_adv_fields fields;
    int rc;

    switch (event->type)
    {
    case BLE_GAP_EVENT_DISC:
        rc = ble_hs_adv_parse_fields(&fields, event->disc.data,
                                     event->disc.length_data);
        if (rc != 0)
        {
            return 0;
        }

        BLE_Packet_T *packet = BLE_DecodeKTagPacket(event->disc.data,
                                                    event->disc.length_data,
                                                    &(event->disc.addr.val[0]),
                                                    event->disc.rssi);

        if (packet != NULL)
        {
            if (packet->Generic.type == BLE_PACKET_TYPE_CONSOLE)
            {
                packet->Console.console_data[BLE_KTAG_PACKET_DATA_SIZE - 1] = '\0';
                HW_Execute_Console_Command(packet->Console.console_data);
            }
            else
            {
                KEvent_T packet_received_event = {.ID = KEVENT_BLE_PACKET_RECEIVED, .Data = packet};
                Post_KEvent(&packet_received_event);
            }
        }
        return 0;

    case BLE_GAP_EVENT_ADV_COMPLETE:
        KLOG_INFO(TAG, "Advertise complete; reason=%d", event->adv_complete.reason);
        ble_advertise();
        return 0;

    default:
        return 0;
    }
}

void ble_host_task(void *param)
{
    KLOG_INFO(TAG, "BLE Host Task Started");

    // This function will return only after nimble_port_stop() is called:
    nimble_port_run();

    nimble_port_freertos_deinit();
}

void Initialize_BLE(void)
{
    KLOG_INFO(TAG, "Initializing BLE...");

    esp_err_t ret = nimble_port_init();
    if (ret != ESP_OK)
    {
        KLOG_ERROR(TAG, "Failed to initialize NimBLE for reason %d.", ret);
        return;
    }
    /* Configure the host. */
    ble_hs_cfg.reset_cb = ble_on_reset;
    ble_hs_cfg.sync_cb = ble_on_sync;
    ble_hs_cfg.store_status_cb = ble_store_util_status_rr;

    ble_store_config_init();

    nimble_port_freertos_init(ble_host_task);
}

void Disable_BLE(void)
{
    KLOG_INFO(TAG, "Disabling BLE...");

    esp_err_t ret = nimble_port_deinit();

    if (ret != ESP_OK)
    {
        KLOG_ERROR(TAG, "Failed to deinitialize NimBLE for reason %d.", ret);
    }
    else
    {
        KLOG_INFO(TAG, "NimBLE deinitialized--BLE disabled.");
    }
}

SystemKResult_T BLE_GetMyAddress(uint8_t *BD_ADDR)
{
    /* Try to load a public address. */
    int result = ble_hs_id_copy_addr(BLE_ADDR_PUBLIC, BD_ADDR, NULL);
    if (result == BLE_HS_ENOADDR)
    {
        return SYSTEMK_RESULT_NOT_IMPLEMENTED;
    }
    else
    {
        return SYSTEMK_RESULT_SUCCESS;
    }
}

SystemKResult_T BLE_SetAdvertisingData(BLE_AdvertisingData_T *data)
{
    SystemKResult_T result = SYSTEMK_RESULT_SUCCESS;

    if (data->length > BLE_MAX_ADVERTISING_BYTES)
    {
        result = SYSTEMK_RESULT_TOO_MANY_DATA;
    }
    else if (data->length < BLE_KTAG_PACKET_TOTAL_SIZE)
    {
        result = SYSTEMK_RESULT_TOO_FEW_DATA;
    }
    else
    {
        memcpy(Advertising_Data, data, BLE_KTAG_PACKET_TOTAL_SIZE);
    }

    if (Is_Scanning_And_Advertising == true)
    {
        // Restart advertising to put the new data into the advertisement.
        ble_gap_adv_stop();
        ble_advertise();
    }

    return result;
}

SystemKResult_T BLE_ScanAndAdvertise(void)
{
    SystemKResult_T result = SYSTEMK_RESULT_NOT_READY;

    if (Host_And_Controller_Synced == true)
    {
        if (Is_Scanning_And_Advertising == false)
        {
            ble_scan();
            ble_advertise();
            Is_Scanning_And_Advertising = true;
        }
        result = SYSTEMK_RESULT_SUCCESS;
    }

    return result;
}