#include <string.h>
#include <zephyr/drivers/uart.h>

#include "data_link.h"

ssize_t dl_encode_frame(uint8_t *payload, size_t len, uint8_t *dst) {
	// FIXME check for maximum size

	size_t flen = 8 + 8 + 2 + len + 4;

	memset(dst, 0, flen);
	memcpy((void *)(dst + 18), payload, len);

	return flen;
}

void dl_send_frame(const struct device *const uart_dev, uint8_t *buffer,
                   size_t len) {
	for (size_t i = 0; i < len; i++) {
		uint8_t octet = *(buffer + i);
		switch (octet) {
			case DL_SLIP_END:
				uart_poll_out(uart_dev, DL_SLIP_ESC);
				uart_poll_out(uart_dev, DL_SLIP_ESC_END);
				break;
			case DL_SLIP_ESC:
				uart_poll_out(uart_dev, DL_SLIP_ESC);
				uart_poll_out(uart_dev, DL_SLIP_ESC_ESC);
				break;
			default:
				uart_poll_out(uart_dev, octet);
				break;
		}
	}
	uart_poll_out(uart_dev, DL_SLIP_END);
}