#include <functional>
#include <di/can.h>
#include <di/can/cfg.h>
#include <di/time.h>
#include <fcntl.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <linux/can/raw.h>

#include <gtest/gtest.h>

#define DI_CAN_CFG_FRAME_SIZE    32U
#define DI_CAN_CFG_MSG_SIZE      16U
#define DI_CAN_CFG_MSG_DATA_SIZE 4096U

static std::function<di_errno_t(struct di_can_frame_rx *)> CANTestRecvCb;
static std::function<di_errno_t(const struct di_can_frame_tx *)> CANTestSendCb;

static di_errno_t CANTestSendCallback(const struct di_can_frame_tx *frame) {
	if (CANTestSendCb)
		return CANTestSendCb(frame);
	return DNE_OPNOTSUPP;
}

static di_errno_t CANTestRecvCallback(struct di_can_frame_rx *frame) {
	if (CANTestRecvCb)
		return CANTestRecvCb(frame);
	return DNE_OPNOTSUPP;
}

class DI_CAN : public ::testing::Test {
 public:
	void SetNodeid(uint32_t nodeid) {
		_ctx.nodeid = nodeid;
		_ctx.net.self.nodeid = nodeid;
	}

	void EnableLoopbackSend(bool enable) {
		_loopback_enabled = enable;
	}

	void EnableSocketCANSend(bool enable) {
		__socketcan_enabled = enable;
		if (__socketcan_fd)
			__SocketCAN_Close();
		__socketcan_fd = __SocketCAN_Open();
	}

	void PrintMsg(const struct di_can_msg *msg) {
		printf("=== di_can_msg %p ===\n", msg);
		printf(" - canid: 0x%08x\n",  msg->canid);
		printf(" - src_id: 0x%08x\n", msg->src_id);
		printf(" - dst_id: 0x%08x\n", msg->dst_id);
		printf(" - transaction: %u\n", DI_CAN_GET_TRANSACTION(msg->canid));
		printf(" - type: ");
		switch (DI_CAN_GET_MSGTYPE(msg->canid)) {
		case DI_CAN_MSGTYPE_RAW:
			printf("raw\n");
			break;
		case DI_CAN_MSGTYPE_RPC:
			printf("rpc\n");
			break;
		case DI_CAN_MSGTYPE_NET:
			printf("net\n");
			break;
		case DI_CAN_MSGTYPE_LOG:
			printf("log\n");
			break;
		default:
			printf("unknown\n");
			break;
		}
		printf(" - size: %zu\n", msg->size);
	}

	// Get the current message timeout
	di_time_t getCurrentMsgTimeout(void) {
		return di_time_get_uptime() + _ctx.msg.timeout;
	}

	void PrintNetNodes(void) {
		printf("=== CANTest Network Nodes ===\n");
		printf("self 0x%08x type: %d, role: %d\n", _ctx.net.self.nodeid, _ctx.net.self.type, _ctx.net.self.role);
		for (size_t n = 0; n < DI_ARRAY_SIZE(_ctx.net.nodes.list); n++) {
			printf("node[%zu] 0x%08x type: 0x%02x, role: 0x%02x\n",
			       n,
			       _ctx.net.nodes.list[n].nodeid,
			       _ctx.net.nodes.list[n].type,
			       _ctx.net.nodes.list[n].role);
		}
	}

 protected:
	virtual void SetUp() {
		di_can_init(&_ctx);

		_frame_rx_pool   = new di_can_frame_rx[DI_CAN_CFG_FRAME_SIZE];
		_msg_buffer_data = new uint8_t[DI_CAN_CFG_MSG_SIZE * DI_CAN_CFG_MSG_DATA_SIZE];
		_msg_buffer_pool = new di_buffer[DI_CAN_CFG_MSG_SIZE];
		_msg_pool        = new di_can_msg[DI_CAN_CFG_MSG_SIZE];

		di_can_frame_init(&_ctx, _frame_rx_pool, DI_CAN_CFG_FRAME_SIZE);
		di_buffer_init_array(_msg_buffer_pool, _msg_buffer_data, DI_CAN_CFG_MSG_SIZE, DI_CAN_CFG_MSG_DATA_SIZE);
		di_can_msg_init_array(&_ctx, _msg_pool, DI_CAN_CFG_MSG_SIZE, _msg_buffer_pool, DI_CAN_CFG_MSG_SIZE);
		di_can_msg_init(&_ctx, _msg_pool, DI_CAN_CFG_MSG_SIZE);
		di_can_net_init(&_ctx);

		di_can_msg_recv_set_blocking(&_ctx, false);
		di_can_msg_recv_set_timeout(&_ctx, 0);

		di_can_callback_set_send(&_ctx, CANTestSendCallback);
		CANTestSendCb = std::bind(&DI_CAN::__CanSend, this, std::placeholders::_1);

		di_can_callback_set_recv(&_ctx, CANTestRecvCallback);
		CANTestRecvCb = std::bind(&DI_CAN::__CanRecv, this, std::placeholders::_1);

		_loopback_enabled   = true;
		__socketcan_enabled = false;
		__socketcan_fd      = -1;

		EnableSocketCANSend(true);
		SetNodeid(0xb034b065);
	}

	virtual void TearDown() {
		delete[] _frame_rx_pool;
		delete[] _msg_buffer_data;
		delete[] _msg_buffer_pool;
		delete[] _msg_pool;

		if (__socketcan_enabled)
			__SocketCAN_Close();
	}

 private:
	di_errno_t __CanSend(const struct di_can_frame_tx *frame) {
		__CanSendDump(frame);

		if (_loopback_enabled)
			__CanSendLoopback(frame);
		if (__socketcan_enabled)
			__SocketCAN_Send(frame);

		return DNOK;
	}

	di_errno_t __CanRecv(struct di_can_frame_rx *frame) {
		(void)frame;
		return DNOK;
	}

	di_errno_t __CanSendLoopback(const struct di_can_frame_tx *frame) {
		struct di_can_ctx *ctx = (struct di_can_ctx *)frame->ctx;
		struct di_can_frame_rx *rx = di_can_frame_get_unused(ctx);

		if (!rx)
			return DNE_NOMEM;

		rx->canid = frame->canid;
		rx->size  = frame->size;
		memcpy(rx->buf, frame->buf, rx->size);

		di_can_frame_return_ready(rx);
		di_can_reassemble(ctx);

		return DNOK;
	}

	void __CanSendDump(const struct di_can_frame_tx *frame) {
		printf("[can tx][%08x] ", frame->canid);
		for (size_t n = 0; n < frame->size; n++)
			printf("%02x ", ((uint8_t *)frame->buf)[n]);
		for (size_t n = 0; n < (8 - frame->size); n++)
			printf("   ");
		printf("| ");
		for (size_t n = 0; n < frame->size; n++)
			printf("%c", isprint((int)frame->buf[n]) ? frame->buf[n] : '.');
		for (size_t n = 0; n < (8 - frame->size); n++)
			printf(" ");
		printf("\n");
	}

	void __CanRecvDump(const struct di_can_frame_rx *frame) {
		printf("[can rx][%08x] ", frame->canid);
		for (size_t n = 0; n < frame->size; n++)
			printf("%02x ", ((uint8_t *)frame->buf)[n]);
		for (size_t n = 0; n < (8 - frame->size); n++)
			printf("   ");
		printf("| ");
		for (size_t n = 0; n < frame->size; n++)
			printf("%c", isprint((int)frame->buf[n]) ? frame->buf[n] : '.');
		for (size_t n = 0; n < (8 - frame->size); n++)
			printf(" ");
		printf("\n");
	}

	bool _loopback_enabled;

	struct di_can_ctx _ctx;
	struct di_can_frame_rx *_frame_rx_pool;
	uint8_t *_msg_buffer_data;
	struct di_buffer *_msg_buffer_pool;
	struct di_can_msg *_msg_pool;

  private:
	bool __socketcan_enabled;
	int __socketcan_fd;

	di_errno_t __SocketCAN_Send(const struct di_can_frame_tx *frame) {
		ssize_t ret;
		struct can_frame f;

		memset(&f, 0, sizeof(f));

		f.can_id  = frame->canid;
		f.can_id |= CAN_EFF_FLAG;

		f.can_dlc = static_cast<uint8_t>(frame->size);
		if (f.can_dlc > 0)
			memcpy(f.data, frame->buf, f.can_dlc);

		ret = write(__socketcan_fd, &f, sizeof(struct can_frame));

		if (ret != sizeof(struct can_frame))
			return DNE_CAN_IO;

		return DNOK;
	}

	int __SocketCAN_Open(std::string iface = "vcan0") {
		int fd;
		struct ifreq ifr;
		struct sockaddr_can addr;

		if (iface.size() > sizeof(ifr.ifr_name))
			return -1;

		fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
		if (fd < 0)
			return -1;

		addr.can_family = AF_CAN;

		memcpy(ifr.ifr_name, iface.c_str(), iface.size());
		ifr.ifr_name[iface.size()] = 0;

		if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0)
			return -1;

		addr.can_ifindex = ifr.ifr_ifindex;
		fcntl(fd, F_SETFL, O_NONBLOCK);
		if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
			return -1;

		return fd;
	}

	void __SocketCAN_Close() {
		if (__socketcan_fd) {
			close(__socketcan_fd);
			__socketcan_fd = -1;
		}
	}
};