/**
 * libdi Linux SocketCAN application
 * Setup and show linux socketcan network interface (where iface e.g can0)
 * * ip -details link show [iface]
 * * ip link set [iface] type can bitrate 1000000
 * * candump [iface]
 * It is also possible to use a virtual CAN port driver for testing purposes
 * * modprobe vcan
 * * ip link add dev vcan0 type vcan
 * * ip link set up vcan0
 */
#define DI_CAN_CFG_FRAME_POOL_SIZE 1024
#define DI_CAN_CFG_MSG_POOL_SIZE 1024
#define DI_CAN_CFG_MSG_DATA_SIZE 1024

#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <inttypes.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>

#include <di/bitops.h>
#include <di/can.h>
#include <di/time.h>
#include <di/buffer.h>

/* Define verbose when more debugging is needed */
//#define VERBOSE 1

/* CAN nodeid */
#define CAN_NODEID 31

/* CAN context */
static struct di_can_ctx ctx;
DI_CAN_FRAME_RX_DECL_ARRAY(can_frames, DI_CAN_CFG_FRAME_POOL_SIZE);
DI_CAN_MSG_DECL_ARRAY(pool, DI_CAN_CFG_MSG_POOL_SIZE, DI_CAN_CFG_MSG_DATA_SIZE);

static int g_fd = -1;

uint64_t get_time_ms(void)
{
	struct timeval tv;

	gettimeofday(&tv, NULL);

	return (tv.tv_sec * 1000) + (tv.tv_usec / 1000);
}

#ifdef VERBOSE
static const char *socketcan_msgtype_str(enum di_can_msgtype t)
{
	switch (t) {
		case DI_CAN_MSGTYPE_RAW:
			return "raw";
		case DI_CAN_MSGTYPE_RPC:
			return "rpc";
		default:
			return "unknown";
	}
}

void socketcan_printf_frame_rx(struct di_can_frame_rx *frame)
{
	printf("---   nodeid: %u\n", DI_CAN_GET_NODEID(frame->canid));
	printf("        size: %u\n", frame->size);
	printf("     msgtype: %s\n", socketcan_msgtype_str((enum di_can_msgtype)DI_CAN_GET_MSGTYPE(frame->canid)));
	printf("    datatype: 0x%02x (%u)\n", DI_CAN_GET_DATATYPE(frame->canid), DI_CAN_GET_DATATYPE(frame->canid));
	printf("       frame: %u\n", DI_CAN_GET_FRAME(frame->canid));
	printf(" transaction: %u\n", DI_CAN_GET_TRANSACTION(frame->canid));
	printf("        last: %s\n", DI_CAN_GET_LAST_FRAME(frame->canid) ? "true" : "false");

	if (frame->size > 0) {
		printf("        data: ");
		for (size_t n = 0; n < frame->size; n++)
			printf("%02x ", frame->buf[n]);
		printf("\n");
	}
	printf("---\n");
}
#endif

static di_errno_t socketcan_recv(struct di_can_frame_rx *frame)
{
	(void)frame;

	di_errno_t ret = DNE_CAN_IO;
	ssize_t n;
	struct can_frame f;
	struct timeval timeout = {0, 10000};
	fd_set readSet;
	FD_ZERO(&readSet);
	FD_SET(g_fd, &readSet);

	if (select((g_fd + 1), &readSet, NULL, NULL, &timeout) > 0) {
		if (FD_ISSET(g_fd, &readSet)) {
			n = read(g_fd, &f, sizeof(struct can_frame));
			if (n) {
				frame->canid = f.can_id;
				frame->size  = f.can_dlc;
				memcpy(frame->buf, f.data, frame->size);
				ret = DNOK;
#ifdef VERBOSE
				socketcan_printf_frame_rx(frame);
#endif
			}
		}
	}

	return ret;
}

static di_errno_t socketcan_send(struct di_can_frame_tx *frame)
{
	ssize_t ret;
	struct can_frame f;

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

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

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

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

#ifdef VERBOSE
	printf("send [%08x] ", frame->canid);
	for (size_t n = 0; n < frame->size; n++)
		printf("%02x ", ((uint8_t *)frame->buf)[n]);
	printf("\n");
#endif

	return DNOK;
}

static int socketcan_open(const char *iface)
{
	int fd;
	struct ifreq ifr;
	struct sockaddr_can addr;

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

	addr.can_family = AF_CAN;
	strcpy(ifr.ifr_name, iface);
	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;
}

static void socketcan_close(int fd)
{
	close(fd);
}

static int socketcan_init(void)
{
	di_time_init();
	DI_CAN_MSG_INIT_ARRAY(pool, DI_CAN_CFG_MSG_POOL_SIZE, DI_CAN_CFG_MSG_DATA_SIZE);
	di_can_init(&ctx,
		    CAN_NODEID);
	di_can_frame_init(&ctx, can_frames, DI_CAN_CFG_FRAME_POOL_SIZE);
	di_can_msg_init(&ctx, pool, DI_CAN_CFG_MSG_POOL_SIZE);

	ctx.send = socketcan_send;
	ctx.recv = socketcan_recv;

	/** @todo this is really ugly, we sort of disable the garbage collector here */
	ctx.frame.timeout = 86400000;
	ctx.msg.timeout   = 86400000;

	return 0;
}

void socketcan_send_heartbeat(void)
{
	struct di_can_msg msg;
	struct di_can_msg *m;

	msg.canid = 0;
	DI_CAN_SET_MSGTYPE(msg.canid, DI_CAN_MSGTYPE_RAW);
	DI_CAN_SET_DATATYPE(msg.canid, DI_CAN_RAW_HEARTBEAT_COMM);

	msg.size  = 0;
	msg.buf   = NULL;
	/** @todo ... else di_can_send crashes */
	msg.private_data = &ctx;

	m = &msg;

	di_can_send(&m);
}

void socketcan_send_time(void)
{
	uint64_t time;
	struct di_can_msg msg;
	struct di_can_msg *m;

	msg.canid = 0;
	DI_CAN_SET_MSGTYPE(msg.canid, DI_CAN_MSGTYPE_RAW);
	DI_CAN_SET_DATATYPE(msg.canid, DI_CAN_RAW_TIME);
	DI_CAN_SET_TRANSFERTYPE(msg.canid, DI_CAN_TRANSFERTYPE_PUB);

	di_store_native_u64_at((char *)&time, get_time_ms());

	msg.size  = sizeof(time);
	msg.msg   = (uint8_t *)&time;
	msg.buf   = NULL;
	/** @todo ... else di_can_send crashes */
	msg.private_data = &ctx;

	m = &msg;

	di_can_send(&m);
}

void socketcan_send_device_uid(void)
{
#if 0
	uint32_t uid[4] = {
		0xefbeadde,
		0xefbeadde,
		0xefbeadde,
		0xefbeadde
	};
#endif
	uint32_t uid[4] = {
		0x00112233,
		0x44556677,
		0x8899aabb,
		0xccddeeff
	};

	uid[0] = htobe32(uid[0]);
	uid[1] = htobe32(uid[1]);
	uid[2] = htobe32(uid[2]);
	uid[3] = htobe32(uid[3]);

	di_can_raw_send(&ctx, DI_CAN_TRANSFERTYPE_PUB, DI_CAN_RAW_DEVICE_UID, &uid, 16);
}

int main(int argc, char **argv)
{
	uint64_t time = 0;
	struct di_can_msg *msg = NULL;
	if (argc == 1) {
		fprintf(stderr, "Usage: %s <iface>\n", argv[0]);
		return -1;
	}

	g_fd = socketcan_open(argv[1]);
	if (g_fd < 0)
		return -1;

	printf("Opened %s on fd %d\n", argv[1], g_fd);

	socketcan_init();

	while (true) {
		di_can_msg_gc(&ctx, DI_CAN_GC_ALL);
		di_can_frame_gc(&ctx, DI_CAN_GC_ALL);
		di_can_reassemble(&ctx);

		di_can_lowlevel_recv(&ctx);

		msg = di_can_recv(&ctx, DI_CAN_MSGTYPE_RAW);
		if (msg) {
			if (DI_CAN_GET_DATATYPE(msg->canid) == DI_CAN_RAW_TIME) {
				time = di_load_native_u64((const char *)msg->msg);
				printf("[%016" PRIxPTR "] time recv (nodeid: %d): %" PRIu64 "\n", (uintptr_t)msg, DI_CAN_GET_NODEID(msg->canid), time);
			}
			di_can_msg_free(&msg);
		}

		msg = di_can_recv(&ctx, DI_CAN_MSGTYPE_RPC);
		if (msg) {
			di_can_msg_free(&msg);
		}

//		socketcan_send_heartbeat();
//		socketcan_send_time();
//		socketcan_send_device_uid();
	}

	socketcan_close(g_fd);

	return 0;
}