#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>

#include <pthread.h>
#include <semaphore.h>

#include <drm_fourcc.h>

#include <list.h>
#include <biqueue.h>
#include <drm_util.h>
#include <swblend.h>

#include <errno.h>
#include <math.h>

#if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
#define CONFIG_REMOTE_SERVICE_ENDPOINT (13) 
#define CONFIG_LOCAL_SERVICE_ENDPOINT (8192) 
#include <rpmsg_char_helper.h>
#endif

extern bool fps_show;

struct needle_image {
	char *filename;
	int width;
	int height;
	int angle;
	char *data;
};

struct needle_asset {
	int pos_x;
	int pos_y;
	int angle;
};

struct needle_layer_asset {
	char *feature;
	int width;
	int height;
	int pos_x;
	int pos_y;
	char *filename;
	char *data;
	int texture; 
};

struct digit_position {
	char *name;
	int pos_x;
	int pos_y;
};

struct digit_image {
	char *filename;
	int width;
	int height;
	char value;
	char *data;
};


struct needle_param {
#if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
	int can_fd;
#endif
	struct biqueue *bq;
};

struct damage {
	struct rect *rects;
	int num_rects;
};

#include <assets/asset-config.h>
#include <assets/needles-assets.h>
#include <assets/digit-assets.h>

#define NUM_NEEDLE_FEATURES (sizeof(nlassets) / sizeof(struct needle_layer_asset))
#define NUM_NEEDLE_ANGLES (sizeof(needle_images) / sizeof(struct needle_image))
#define NUM_DIGIT_VALUES (sizeof(dig_images) / sizeof(struct digit_image))
#define NUM_FPS_DIGIT_VALUES (sizeof(fps_dig_images) / sizeof(struct digit_image))

#define SPEED_RESOLUTION_PER_DEG (1.0f)
#define RPM_RESOLUTION_PER_DEG (33.3333333f)

#define MPH_NEEDLE_ANGLE_OFFSET (120)
#define RPM_NEEDLE_ANGLE_OFFSET (120)

#define MAX_SPEED (240)
#define MAX_RPM (8000)

#define NUM_FPS_ENTRIES (60)
struct fps_entry {
	unsigned long long int timestamp;
	struct list_head link;
};
struct list_head fps;
#define CONFIG_FPS_REFRESH_INTERVAL (1)

static unsigned long long int get_time_ms()
{
	struct timespec t;
	clock_gettime(CLOCK_MONOTONIC, &t);
	return (((unsigned long long int)t.tv_sec * 1000) + (t.tv_nsec / 1000000));
}

static void init_fps()
{
	int i;

	INIT_LIST_HEAD(&fps);
	
	for(i = 0 ; i < NUM_FPS_ENTRIES; i++) {
		struct fps_entry *entry = calloc(sizeof(*entry), 1);
		entry->timestamp = get_time_ms();
		list_add_tail(&entry->link, &fps);
	}
}

static int calculate_fps()
{
	struct fps_entry *entry = list_first_entry(&fps, struct fps_entry, link);
	entry->timestamp = get_time_ms();
	list_del(&entry->link);
	list_add_tail(&entry->link, &fps);

	unsigned long long int total_ms = 0;
	list_for_each_entry_reverse(entry, &fps, link) {
		struct list_head *prev_link = entry->link.prev;
		if(prev_link != &fps) {
			struct fps_entry *prev = container_of(prev_link, struct fps_entry, link);
			unsigned long long int diff_ms = entry->timestamp - prev->timestamp;
			total_ms += diff_ms;
		}
	}

	return total_ms ? (int)(((NUM_FPS_ENTRIES - 1) * 1000) / (float)total_ms) : 0;
}

static int load_needle_assets(struct needle_layer_asset *d)
{
	int r;
	int fd;
	int sz;

	fd = open(d->filename, O_RDONLY);
	if(fd < 0) {
		printf("could not open %s, %d\n", d->filename, errno);
		goto err1;
	}

	sz = d->width * d->height * 4;
	d->data = calloc(sz, 1);
	if(!d->data) {
		printf("could not allocate data for %s\n", d->feature);
		goto err2;
	}

	r = read(fd, d->data, sz);
	if(r != sz) {
		printf("could not read data for %s\n", d->feature);
		goto err3;
	}

	close(fd);
	return 0;

err3:
	free(d->data);
err2:
	close(fd);
err1:
	return -1;
}

static int load_needle_images(struct needle_image *n)
{
	int r;
	int fd;
	int sz;

	fd = open(n->filename, O_RDONLY);
	if(fd < 0) {
		printf("could not open %s, %d\n", n->filename, errno);
		goto err1;
	}

	sz = n->width * n->height * 4;
	n->data = calloc(sz, 1);
	if(!n->data) {
		printf("could not allocate data for needle angle %u\n", n->angle);
		goto err2;
	}

	r = read(fd, n->data, sz);
	if(r != sz) {
		printf("could not read data for needle angle %u\n", n->angle);
		goto err3;
	}

	close(fd);
	return 0;

err3:
	free(n->data);
err2:
	close(fd);
err1:
	return -1;
}

static int load_digit_images(struct digit_image *d)
{
	int r;
	int fd;
	int sz;

	fd = open(d->filename, O_RDONLY);
	if(fd < 0) {
		printf("could not open %s, %d\n", d->filename, errno);
		goto err1;
	}

	sz = d->width * d->height * 4;
	d->data = calloc(sz, 1);
	if(!d->data) {
		printf("could not allocate data for digit \"%c\"\n", d->value);
		goto err2;
	}

	r = read(fd, d->data, sz);
	if(r != sz) {
		printf("could not read data for digit \"%c\"\n", d->value);
		goto err3;
	}

	close(fd);
	return 0;

err3:
	free(d->data);
err2:
	close(fd);
err1:
	return -1;
}

static int load_fps_digit_images(struct digit_image *d)
{
	int r;
	int fd;
	int sz;

	fd = open(d->filename, O_RDONLY);
	if(fd < 0) {
		printf("could not open %s, %d\n", d->filename, errno);
		goto err1;
	}

	sz = d->width * d->height * 4;
	d->data = calloc(sz, 1);
	if(!d->data) {
		printf("could not allocate data for digit \"%c\"\n", d->value);
		goto err2;
	}

	r = read(fd, d->data, sz);
	if(r != sz) {
		printf("could not read data for digit \"%c\"\n", d->value);
		goto err3;
	}

	close(fd);
	return 0;

err3:
	free(d->data);
err2:
	close(fd);
err1:
	return -1;
}

static void needle_wait_for_next_slot(struct needle_param *prm)
{
}

#if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
#define REQUEST_CAN_VALUES (0xdeadcafe)

struct can_values {
	uint32_t speed;
	uint32_t rpm;
};

static void needle_get_values(struct needle_param *prm, int *mph, int *rpm)
{
	struct can_values v;
	uint32_t req = REQUEST_CAN_VALUES;
	int base_mph, base_rpm;

	write(prm->can_fd, &req, sizeof(req));
	read(prm->can_fd, &v, sizeof(v));

	*mph = v.speed;
	*rpm = v.rpm;
}
#else
static void needle_get_values(struct needle_param *prm, int *mph, int *rpm)
{
	static int i = 0;
	float speed_val, rpm_val;

	speed_val = ((sinf(i * 0.01) + 1) / 2) * MAX_SPEED;
	rpm_val = ((sinf(i * 0.01) + 1) / 2) * MAX_RPM;

	*mph = (int)speed_val;
	*rpm = (int)rpm_val;

	i++;
}
#endif

static struct damage *alloc_zero_damage(int number)
{
	struct damage *d = calloc(sizeof(*d), 1);
	d->rects = calloc(sizeof(struct rect), number);
	d->num_rects = 0;
	return d;
}

static void add_to_damage(struct damage *d, struct rect *r)
{
	d->rects[d->num_rects].top = r->top;
	d->rects[d->num_rects].left = r->left;
	d->rects[d->num_rects].width = r->width;
	d->rects[d->num_rects].height = r->height;
	d->num_rects++;
}

static void free_damage(struct damage *d)
{
	free(d->rects);
	free(d);
}


static struct sw_blend_layer *find_layer(struct sw_blend_layer *layers, int n, char *name)
{
	int i;
	for(i = 0; i < n; i++)
		if(strcmp(layers[i].name, name) == 0)
			return &layers[i];
	return NULL;
}

static void init_rect_with_layer_bb(struct rect *rect, struct sw_blend_layer *l)
{
	if(!l)
		return;
	rect->top = l->y;
	rect->left = l->x;
	rect->width = l->buf.width;
	rect->height = l->buf.height;
}

static void upd_rect_with_layer_bb(struct rect *rect, struct sw_blend_layer *l)
{
	if(!l)
		return;
	if(l->y < rect->top)
		rect->top = l->y;
	if(l->x < rect->left)
		rect->left = l->y;
	if((l->x + l->buf.width) > (rect->left + rect->width))
		rect->width = l->x + l->buf.width - rect->left;
	if((l->y + l->buf.height) > (rect->top + rect->height))
		rect->height = l->y + l->buf.height - rect->top;
}

static void add_feature_to_layer(struct sw_blend_layer *layers, int index, char *name, struct needle_layer_asset *d)
{

	layers[index].buf.width = d->width;
	layers[index].buf.height = d->height;
	layers[index].buf.format = DRM_FORMAT_ARGB8888;
	layers[index].buf.stride = d->width * 4;
	layers[index].buf.vaddr = d->data;
	layers[index].buf.stride2 = 0;
	layers[index].buf.vaddr2 = NULL;
	layers[index].x = d->pos_x;
	layers[index].y = d->pos_y;
	layers[index].blend = true;
	layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
	layers[index].name = name;
}

static void add_needle_to_layer(struct sw_blend_layer *layers, int index, char *name, struct needle_asset *nassets, int nindex, struct rect *d)
{
	layers[index].buf.width = needle_images[nindex].width;
	layers[index].buf.height = needle_images[nindex].height;
	layers[index].buf.format = DRM_FORMAT_ARGB8888;
	layers[index].buf.stride = needle_images[nindex].width * 4;
	layers[index].buf.vaddr = needle_images[nindex].data;
	layers[index].buf.stride2 = 0;
	layers[index].buf.vaddr2 = NULL;
	layers[index].x = nassets[nindex].pos_x;
	layers[index].y = nassets[nindex].pos_y;
	layers[index].blend = true;
	layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
	layers[index].name = name;

	d->top = layers[index].y;
	d->left = layers[index].x;
	d->width = layers[index].buf.width;
	d->height = layers[index].buf.height;
}

static void add_fps_digit_to_layer(struct sw_blend_layer *layers, int index, char *name, char *id, int val)
{
	struct digit_image *img;
	struct digit_position *pos;

	if(strcmp(id, "fps-digit-0") == 0) {
		pos = &fps_dig_pos[0];
		if(val < 100)
			img = &fps_dig_images[10];
		else
			img = &fps_dig_images[val / 100];
	} else if(strcmp(id, "fps-digit-1") == 0) {
		pos = &fps_dig_pos[1];
		if(val < 10)
			img = &fps_dig_images[val];
		else
			img = &fps_dig_images[(val / 10) % 10];
	} else if(strcmp(id, "fps-digit-2") == 0) {
		pos = &fps_dig_pos[2];
		img = &fps_dig_images[val % 10];
	}

	layers[index].buf.width = img->width;
	layers[index].buf.height = img->height;
	layers[index].buf.format = DRM_FORMAT_ARGB8888;
	layers[index].buf.stride = img->width * 4;
	layers[index].buf.vaddr = img->data;
	layers[index].buf.stride2 = 0;
	layers[index].buf.vaddr2 = NULL;
	layers[index].x = pos->pos_x;
	layers[index].y = pos->pos_y;
	layers[index].blend = true;
	layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
	layers[index].name = name;
}
static void add_digit_to_layer(struct sw_blend_layer *layers, int index, char *name, char *id, int val)
{
	struct digit_image *img;
	struct digit_position *pos;

	if(strcmp(id, "digit-0") == 0) {
		pos = &dig_pos[0];
		if(val < 10)
			img = &dig_images[10];
		else if(val < 100)
			img = &dig_images[10];
		else
			img = &dig_images[val / 100];
	} else if(strcmp(id, "digit-1") == 0) {
		pos = &dig_pos[1];
		if(val < 10)
			img = &dig_images[val];
		else if(val < 100)
			img = &dig_images[10];
		else
			img = &dig_images[(val % 100) / 10];
	} else if(strcmp(id, "digit-2") == 0) {
		pos = &dig_pos[2];
		if(val < 10)
			img = &dig_images[10];
		else if(val < 100)
			img = &dig_images[10];
		else
			img = &dig_images[val % 10];
	} else if(strcmp(id, "digit-0.5") == 0) {
		pos = &dig_pos[3];
		if(val < 10)
			img = &dig_images[10];
		else if(val < 100)
			img = &dig_images[val / 10];
		else
			img = &dig_images[10];
	} else if(strcmp(id, "digit-1.5") == 0) {
		pos = &dig_pos[4];
		if(val < 10)
			img = &dig_images[10];
		else if(val < 100)
			img = &dig_images[val % 10];
		else
			img = &dig_images[10];
	}

	layers[index].buf.width = img->width;
	layers[index].buf.height = img->height;
	layers[index].buf.format = DRM_FORMAT_ARGB8888;
	layers[index].buf.stride = img->width * 4;
	layers[index].buf.vaddr = img->data;
	layers[index].buf.stride2 = 0;
	layers[index].buf.vaddr2 = NULL;
	layers[index].x = pos->pos_x;
	layers[index].y = pos->pos_y;
	layers[index].blend = true;
	layers[index].blendfuncs = SRC_GL_SRC_ALPHA | DST_GL_ONE_MINUS_SRC_ALPHA;
	layers[index].name = name;
}

static void *needle_run(void *arg)
{
	struct needle_param *prm = arg;
	struct sw_blend_buffer dst;
	struct sw_blend_layer layers[11];
	int cfps, fps;
	int i = 0;

	while(true) {
		struct buffer *buf;
		int mph_val, rpm_val;
		int mph_angle, rpm_angle;
		struct rect rect;
		struct rect digit_rect = {0};
		struct rect fps_rect = {0};
		int num_layers = 0;

		needle_wait_for_next_slot(prm);

		buf = list_entry(bq_next_empty(prm->bq), struct buffer, link);
		struct damage *old_damage = drm_buffer_get_priv(buf);
		struct damage *new_damage = alloc_zero_damage(4);
		struct damage *store_damage = alloc_zero_damage(3);

		dst.width = buf->width;
		dst.height = buf->height;
		dst.format = buf->format;
		dst.stride = buf->stride;
		dst.vaddr = buf->vaddr;
		dst.stride2 = 0;
		dst.vaddr2 = NULL;

		needle_get_values(prm, &mph_val, &rpm_val);

		//mph value
		mph_angle = ((int)(mph_val/ SPEED_RESOLUTION_PER_DEG) + 360 - MPH_NEEDLE_ANGLE_OFFSET) % 360;
		add_needle_to_layer(layers, num_layers, "mph-needle", mphassets, mph_angle, &rect); num_layers++;
		add_to_damage(new_damage, &rect);
		add_to_damage(store_damage, &rect);

		//rpm value
		rpm_angle = ((int)(rpm_val / RPM_RESOLUTION_PER_DEG) + 360 - RPM_NEEDLE_ANGLE_OFFSET) % 360;
		add_needle_to_layer(layers, num_layers, "rpm-needle", rpmassets, rpm_angle, &rect); num_layers++;
		add_to_damage(new_damage, &rect);
		add_to_damage(store_damage, &rect);

		add_digit_to_layer(layers, num_layers, "digit-0",   "digit-0",   mph_val); num_layers++;
		add_digit_to_layer(layers, num_layers, "digit-1",   "digit-1",   mph_val); num_layers++;
		add_digit_to_layer(layers, num_layers, "digit-2",   "digit-2",   mph_val); num_layers++;
		add_digit_to_layer(layers, num_layers, "digit-0.5", "digit-0.5", mph_val); num_layers++;
		add_digit_to_layer(layers, num_layers, "digit-1.5", "digit-1.5", mph_val); num_layers++;

		init_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-0"));
		upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-1"));
		upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-2"));
		upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-0.5"));
		upd_rect_with_layer_bb(&digit_rect, find_layer(layers, num_layers, "digit-1.5"));
		add_to_damage(new_damage, &digit_rect);

		cfps = calculate_fps();
		if(cfps > 999)
			cfps = 999;
		if((i % CONFIG_FPS_REFRESH_INTERVAL) == 0)
			fps = cfps;

		if(fps_show) {
			struct needle_layer_asset *n = NULL;
			int c;
			for(c = 0; c < NUM_NEEDLE_FEATURES; c++)
				if(strcmp("fps-text", nlassets[c].feature) == 0) {
					n = &nlassets[c];
					break;
				}
			add_feature_to_layer(layers, num_layers, "fps-text", n); num_layers++;
			add_fps_digit_to_layer(layers, num_layers, "fps-digit-0", "fps-digit-0", fps); num_layers++;
			add_fps_digit_to_layer(layers, num_layers, "fps-digit-1", "fps-digit-1", fps); num_layers++;
			add_fps_digit_to_layer(layers, num_layers, "fps-digit-2", "fps-digit-2", fps); num_layers++;

			init_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-text"));
			upd_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-digit-0"));
			upd_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-digit-1"));
			upd_rect_with_layer_bb(&fps_rect, find_layer(layers, num_layers, "fps-digit-2"));

			add_to_damage(new_damage, &fps_rect);
			add_to_damage(store_damage, &fps_rect);
		}

		if(!old_damage)
			sw_blend(&dst, layers, num_layers, true, 0x00000000, NULL, 0, new_damage->rects, new_damage->num_rects);
		else
			sw_blend(&dst, layers, num_layers, true, 0x00000000, old_damage->rects, old_damage->num_rects, new_damage->rects, new_damage->num_rects);

		if(old_damage)
			free_damage(old_damage);

		free_damage(new_damage);

		drm_buffer_set_priv(buf, store_damage);

		bq_queue_full(prm->bq, &buf->link);

		i++;
	}
}

void needle_start(struct biqueue *bq)
{
	int i;
	pthread_t tid;

#if defined(CONFIG_RPMSG_CHAR_CAN_EMULATION)
	char *remote_core_name = "r5f-main-0-core-0";
	int remote_service_endpt = CONFIG_REMOTE_SERVICE_ENDPOINT;
	int local_endpt = CONFIG_LOCAL_SERVICE_ENDPOINT;
	char *local_name = "can-virtual";
	rproc_device_t *dev;
	rproc_char_device_t *cdev;
	rproc_char_endpt_t *ept = NULL;
	char *path;
	int can_fd;

	dev = rproc_device_find_for_name(remote_core_name);
	if(!dev) {
		fprintf(stderr, "rproc_device_find_for_name failed\n");
		return;
	}
	
	cdev = rproc_device_find_chrdev_by_remote_port(dev, remote_service_endpt);
	if(!cdev) {
		fprintf(stderr, "rproc_device_find_chrdev_by_remote_port failed\n");
		return;
	}

	if(!ept)
		ept = rproc_char_device_find_endpt_by_name(cdev, local_name);
	if(!ept)
		ept = rproc_char_device_find_endpt_by_local_port(cdev, local_endpt);
	if(!ept)
		ept = rproc_char_device_create_endpt(cdev, local_name, local_endpt);
	if(!ept) {
		fprintf(stderr, "rproc_char_device_create_endpt failed\n");
		return;
	}

	path = rproc_char_endpt_get_dev_name(ept);
	if(!path) {
		fprintf(stderr, "rproc_char_endpt_get_dev_name failed\n");
		return;
	}

	can_fd = open(path, O_RDWR);
	if(can_fd < 0) {
		fprintf(stderr, "could not open can_fd\n");
		return;
	}
#endif

	init_fps();

	for(i = 0; i < NUM_NEEDLE_FEATURES; i++) {
		load_needle_assets(&nlassets[i]);
	}
	for(i = 0; i < NUM_NEEDLE_ANGLES; i++) {
		load_needle_images(&needle_images[i]);
	}
	for(i = 0; i < NUM_DIGIT_VALUES; i++) {
		load_digit_images(&dig_images[i]);
	}

	for(i = 0; i < NUM_FPS_DIGIT_VALUES; i++) {
		load_fps_digit_images(&fps_dig_images[i]);
	}

	struct needle_param *prm = calloc(sizeof(*prm), 1);
#if defined (CONFIG_RPMSG_CHAR_CAN_EMULATION)
	prm->can_fd = can_fd;
#endif
	prm->bq = bq;

	pthread_create(&tid, NULL, needle_run, prm);
}