/*
 * VESC BLE Bridge für ESP32-C3
 *
 * Reicht alles 1:1 zwischen Bluetooth LE und dem UART-Port vom VESC durch.
 * Nutzt den Nordic UART Service, den VESC Tool und die VESC-App erwarten,
 * damit taucht die Bridge dort ganz normal in der BLE-Liste auf.
 *
 * Verkabelung (TX und RX über Kreuz):
 *   ESP32-C3 GPIO21 (TX)  ->  VESC RX
 *   ESP32-C3 GPIO20 (RX)  <-  VESC TX
 *   5V und GND            ->  5V und GND
 * Im VESC unter App Settings > General den Port auf UART stellen, 115200 Baud.
 */

#include <string.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/uart.h"
#include "driver/gpio.h"
#include "esp_bt.h"
#include "esp_log.h"
#include "nvs_flash.h"

#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "host/ble_hs.h"
#include "host/util/util.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"

#define DEVICE_NAME     "VESC BLE Bridge"

#define VESC_UART       UART_NUM_1
#define VESC_UART_TX    21
#define VESC_UART_RX    20
#define VESC_UART_BAUD  115200

static const char *TAG = "bridge";

/* Nordic UART Service, Bytes rückwärts (little endian) */
static const ble_uuid128_t nus_svc_uuid = BLE_UUID128_INIT(
	0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e);
/* RX: hier schreibt das Handy rein */
static const ble_uuid128_t nus_rx_uuid = BLE_UUID128_INIT(
	0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x02, 0x00, 0x40, 0x6e);
/* TX: darüber gehen die Antworten vom VESC raus */
static const ble_uuid128_t nus_tx_uuid = BLE_UUID128_INIT(
	0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x03, 0x00, 0x40, 0x6e);

static uint16_t tx_val_handle;
static uint8_t own_addr_type;

static volatile uint16_t conn_handle = BLE_HS_CONN_HANDLE_NONE;
static volatile bool notify_on = false;
static volatile uint16_t att_mtu = BLE_ATT_MTU_DFLT;

static void advertise(void);

/* ---- GATT ---- */

static int rx_access(uint16_t conn, uint16_t attr, struct ble_gatt_access_ctxt *ctxt, void *arg) {
	if (ctxt->op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
		return BLE_ATT_ERR_UNLIKELY;
	}

	uint8_t buf[512];
	uint16_t len = 0;
	if (ble_hs_mbuf_to_flat(ctxt->om, buf, sizeof(buf), &len) != 0) {
		return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
	}

	if (len > 0) {
		uart_write_bytes(VESC_UART, buf, len);
	}
	return 0;
}

static int tx_access(uint16_t conn, uint16_t attr, struct ble_gatt_access_ctxt *ctxt, void *arg) {
	/* Nur Notify, lesen gibt es hier nichts */
	return BLE_ATT_ERR_READ_NOT_PERMITTED;
}

static const struct ble_gatt_svc_def gatt_svcs[] = {
	{
		.type = BLE_GATT_SVC_TYPE_PRIMARY,
		.uuid = &nus_svc_uuid.u,
		.characteristics = (struct ble_gatt_chr_def[]) {
			{
				.uuid = &nus_rx_uuid.u,
				.access_cb = rx_access,
				.flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP,
			},
			{
				.uuid = &nus_tx_uuid.u,
				.access_cb = tx_access,
				.val_handle = &tx_val_handle,
				.flags = BLE_GATT_CHR_F_NOTIFY,
			},
			{ 0 }
		},
	},
	{ 0 }
};

/* ---- GAP ---- */

static int gap_event(struct ble_gap_event *event, void *arg) {
	switch (event->type) {
	case BLE_GAP_EVENT_CONNECT:
		if (event->connect.status == 0) {
			conn_handle = event->connect.conn_handle;
			att_mtu = BLE_ATT_MTU_DFLT;

			/* Kurzes Verbindungsintervall, damit VESC Tool flott reagiert */
			struct ble_gap_upd_params params = {
				.itvl_min = 6,   /* 7,5 ms */
				.itvl_max = 12,  /* 15 ms */
				.latency = 0,
				.supervision_timeout = 400,
			};
			ble_gap_update_params(event->connect.conn_handle, &params);
		} else {
			advertise();
		}
		break;

	case BLE_GAP_EVENT_DISCONNECT:
		conn_handle = BLE_HS_CONN_HANDLE_NONE;
		notify_on = false;
		advertise();
		break;

	case BLE_GAP_EVENT_SUBSCRIBE:
		if (event->subscribe.attr_handle == tx_val_handle) {
			notify_on = event->subscribe.cur_notify;
		}
		break;

	case BLE_GAP_EVENT_MTU:
		att_mtu = event->mtu.value;
		break;

	case BLE_GAP_EVENT_ADV_COMPLETE:
		advertise();
		break;

	default:
		break;
	}
	return 0;
}

static void advertise(void) {
	/* Werbepaket: Flags und die NUS-UUID, daran erkennt VESC Tool die Bridge */
	struct ble_hs_adv_fields fields;
	memset(&fields, 0, sizeof(fields));
	fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
	fields.uuids128 = &nus_svc_uuid;
	fields.num_uuids128 = 1;
	fields.uuids128_is_complete = 1;
	int rc = ble_gap_adv_set_fields(&fields);
	if (rc != 0) {
		ESP_LOGE(TAG, "adv fields: %d", rc);
		return;
	}

	/* Name passt nicht mehr mit rein, der kommt in die Scan-Antwort */
	struct ble_hs_adv_fields rsp;
	memset(&rsp, 0, sizeof(rsp));
	rsp.name = (const uint8_t *)DEVICE_NAME;
	rsp.name_len = strlen(DEVICE_NAME);
	rsp.name_is_complete = 1;
	ble_gap_adv_rsp_set_fields(&rsp);

	struct ble_gap_adv_params adv;
	memset(&adv, 0, sizeof(adv));
	adv.conn_mode = BLE_GAP_CONN_MODE_UND;
	adv.disc_mode = BLE_GAP_DISC_MODE_GEN;
	rc = ble_gap_adv_start(own_addr_type, NULL, BLE_HS_FOREVER, &adv, gap_event, NULL);
	if (rc != 0 && rc != BLE_HS_EALREADY) {
		ESP_LOGE(TAG, "adv start: %d", rc);
	}
}

static void on_sync(void) {
	ble_hs_util_ensure_addr(0);
	ble_hs_id_infer_auto(0, &own_addr_type);
	advertise();
}

static void on_reset(int reason) {
	ESP_LOGW(TAG, "BLE reset: %d", reason);
}

static void host_task(void *param) {
	nimble_port_run();
	nimble_port_freertos_deinit();
}

/* ---- UART -> BLE ---- */

static void send_notify(const uint8_t *data, size_t len) {
	size_t off = 0;
	while (off < len) {
		uint16_t conn = conn_handle;
		if (conn == BLE_HS_CONN_HANDLE_NONE || !notify_on) {
			return;
		}

		size_t chunk = att_mtu - 3;
		if (chunk > len - off) {
			chunk = len - off;
		}

		struct os_mbuf *om = ble_hs_mbuf_from_flat(data + off, chunk);
		if (!om) {
			vTaskDelay(1);
			continue;
		}

		/* notify_custom gibt das mbuf immer selbst frei */
		int rc = ble_gatts_notify_custom(conn, tx_val_handle, om);
		if (rc == BLE_HS_ENOMEM) {
			vTaskDelay(pdMS_TO_TICKS(2));
			continue;
		} else if (rc != 0) {
			return;
		}
		off += chunk;
	}
}

static void uart_task(void *param) {
	static uint8_t buf[512];

	for (;;) {
		/* Auf das erste Byte warten, dann kurz sammeln, was noch nachkommt */
		int n = uart_read_bytes(VESC_UART, buf, 1, portMAX_DELAY);
		if (n <= 0) {
			continue;
		}

		int more = uart_read_bytes(VESC_UART, buf + 1, sizeof(buf) - 1, pdMS_TO_TICKS(3));
		if (more > 0) {
			n += more;
		}

		send_notify(buf, n);
	}
}

static void uart_setup(void) {
	uart_config_t cfg = {
		.baud_rate = VESC_UART_BAUD,
		.data_bits = UART_DATA_8_BITS,
		.parity = UART_PARITY_DISABLE,
		.stop_bits = UART_STOP_BITS_1,
		.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
		.source_clk = UART_SCLK_DEFAULT,
	};

	ESP_ERROR_CHECK(uart_driver_install(VESC_UART, 2048, 2048, 0, NULL, 0));
	ESP_ERROR_CHECK(uart_param_config(VESC_UART, &cfg));
	ESP_ERROR_CHECK(uart_set_pin(VESC_UART, VESC_UART_TX, VESC_UART_RX, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));

	/* Ohne angeschlossenen VESC soll RX nicht in der Luft hängen */
	gpio_set_pull_mode(VESC_UART_RX, GPIO_PULLUP_ONLY);
}

void app_main(void) {
	esp_err_t err = nvs_flash_init();
	if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
		ESP_ERROR_CHECK(nvs_flash_erase());
		err = nvs_flash_init();
	}
	ESP_ERROR_CHECK(err);

	uart_setup();

	ESP_ERROR_CHECK(nimble_port_init());
	esp_ble_tx_power_set(ESP_BLE_PWR_TYPE_DEFAULT, ESP_PWR_LVL_P9);
	esp_ble_tx_power_set(ESP_BLE_PWR_TYPE_ADV, ESP_PWR_LVL_P9);

	ble_hs_cfg.sync_cb = on_sync;
	ble_hs_cfg.reset_cb = on_reset;

	ble_svc_gap_init();
	ble_svc_gatt_init();
	ESP_ERROR_CHECK(ble_gatts_count_cfg(gatt_svcs));
	ESP_ERROR_CHECK(ble_gatts_add_svcs(gatt_svcs));
	ble_svc_gap_device_name_set(DEVICE_NAME);

	xTaskCreate(uart_task, "uart_ble", 4096, NULL, 10, NULL);
	nimble_port_freertos_init(host_task);
}
