#include <stdlib.h>
#include <linux/v4l2-subdev.h>
#include <strings.h>
#include <assert.h>
#include <stdio.h>
#include "mode.h"
#include "libmegapixels.h"


// TODO: The 16 bit formats are imported from millipixels and seem broken
static struct libmegapixels_modename mode_lut[] = {
	{
		.name = "unsupported",
		.v4l_pixel_format = 0,
		.media_bus_format = 0,
		.bpc = 0,
		.bpp = 0,
		.cfa = LIBMEGAPIXELS_CFA_NONE,
	},
	{
		.name = "BGGR8",
		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR8,
		.media_bus_format = MEDIA_BUS_FMT_SBGGR8_1X8,
		.bpc = 8,
		.bpp = 8,
		.cfa = LIBMEGAPIXELS_CFA_BGGR,
	},
	{
		.name = "GBRG8",
		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG8,
		.media_bus_format = MEDIA_BUS_FMT_SGBRG8_1X8,
		.bpc = 8,
		.bpp = 8,
		.cfa = LIBMEGAPIXELS_CFA_GBRG,
	},
	{
		.name = "GRBG8",
		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG8,
		.media_bus_format = MEDIA_BUS_FMT_SGRBG8_1X8,
		.bpc = 8,
		.bpp = 8,
		.cfa = LIBMEGAPIXELS_CFA_GRBG,
	},
	{
		.name = "RGGB8",
		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB8,
		.media_bus_format = MEDIA_BUS_FMT_SRGGB8_1X8,
		.bpc = 8,
		.bpp = 8,
		.cfa = LIBMEGAPIXELS_CFA_RGGB,
	},
	{
		.name = "BGGR10P",
		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR10P,
		.media_bus_format = MEDIA_BUS_FMT_SBGGR10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_BGGR,
	},
	{
		.name = "GBRG10P",
		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG10P,
		.media_bus_format = MEDIA_BUS_FMT_SGBRG10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_GBRG,
	},
	{
		.name = "GRBG10P",
		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG10P,
		.media_bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_GRBG,
	},
	{
		.name = "RGGB10P",
		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB10P,
		.media_bus_format = MEDIA_BUS_FMT_SRGGB10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_RGGB,
	},
	{
		.name = "BGGR10",
		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR10,
		.media_bus_format = MEDIA_BUS_FMT_SBGGR10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_BGGR,
	},
	{
		.name = "GBRG10",
		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG10,
		.media_bus_format = MEDIA_BUS_FMT_SGBRG10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_GBRG,
	},
	{
		.name = "GRBG10",
		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG10,
		.media_bus_format = MEDIA_BUS_FMT_SGRBG10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_GRBG,
	},
	{
		.name = "RGGB10",
		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB10,
		.media_bus_format = MEDIA_BUS_FMT_SRGGB10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_RGGB,
	},
	{
		.name = "BGGR16",
		.v4l_pixel_format = V4L2_PIX_FMT_SBGGR16,
		.media_bus_format = MEDIA_BUS_FMT_SBGGR10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_BGGR,
	},
	{
		.name = "GBRG16",
		.v4l_pixel_format = V4L2_PIX_FMT_SGBRG16,
		.media_bus_format = MEDIA_BUS_FMT_SGBRG10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_GBRG,
	},
	{
		.name = "GRBG16",
		.v4l_pixel_format = V4L2_PIX_FMT_SGRBG16,
		.media_bus_format =MEDIA_BUS_FMT_SGRBG10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_GRBG,
	},
	{
		.name = "RGGB16",
		.v4l_pixel_format = V4L2_PIX_FMT_SRGGB16,
		.media_bus_format = MEDIA_BUS_FMT_SRGGB10_1X10,
		.bpc = 10,
		.bpp = 10,
		.cfa = LIBMEGAPIXELS_CFA_RGGB,
	},
	{
		.name = "UYVY",
		.v4l_pixel_format = V4L2_PIX_FMT_UYVY,
		.media_bus_format = MEDIA_BUS_FMT_UYVY8_2X8,
		.bpc = 8,
		.bpp = 16,
		.cfa = LIBMEGAPIXELS_CFA_NONE,
	},
	{
		.name = "YUYV",
		.v4l_pixel_format = V4L2_PIX_FMT_YUYV,
		.media_bus_format = MEDIA_BUS_FMT_YUYV8_2X8,
		.bpc = 8,
		.bpp = 16,
		.cfa = LIBMEGAPIXELS_CFA_NONE,
	},
	{
		.name = "VYUY",
		.v4l_pixel_format = V4L2_PIX_FMT_VYUY,
		.media_bus_format = MEDIA_BUS_FMT_VYUY8_2X8,
		.bpc = 8,
		.bpp = 16,
		.cfa = LIBMEGAPIXELS_CFA_NONE,
	},
	{
		.name = "YVYU",
		.v4l_pixel_format = V4L2_PIX_FMT_YVYU,
		.media_bus_format = MEDIA_BUS_FMT_YVYU8_2X8,
		.bpc = 8,
		.bpp = 16,
		.cfa = LIBMEGAPIXELS_CFA_NONE,
	},
};

int
libmegapixels_format_name_to_index(const char *name)
{
	int count = sizeof(mode_lut) / sizeof(mode_lut[0]);
	for (int i = 0; i < count; i++) {
		if (strcasecmp(mode_lut[i].name, name) == 0) {
			return i;
		}
	}
	return 0;
}

uint32_t
libmegapixels_format_to_v4l_pixfmt(int index)
{
	return mode_lut[index].v4l_pixel_format;
}

uint32_t
libmegapixels_format_to_media_busfmt(int index)
{
	return mode_lut[index].media_bus_format;
}

int
libmegapixels_v4l_pixfmt_to_index(uint32_t pixfmt)
{
	int count = sizeof(mode_lut) / sizeof(mode_lut[0]);
	for (int i = 0; i < count; i++) {
		if (mode_lut[i].v4l_pixel_format == pixfmt) {
			return i;
		}
	}
	return 0;
}

char *
libmegapixels_v4l_pixfmt_to_string(uint32_t pixfmt)
{
	int count = sizeof(mode_lut) / sizeof(mode_lut[0]);
	for (int i = 0; i < count; i++) {
		if (mode_lut[i].v4l_pixel_format == pixfmt) {
			return mode_lut[i].name;
		}
	}
	return "unknown";
}

char *
libmegapixels_format_name(int format)
{
	return mode_lut[format].name;
}

// mp_pixel_format_bits_per_pixel
uint32_t
libmegapixels_format_bits_per_pixel(int format)
{
	return mode_lut[format].bpp;
}

const char *
libmegapixels_format_cfa(int format)
{
	switch (mode_lut[format].cfa) {
		case LIBMEGAPIXELS_CFA_BGGR:
			return "BGGR";
		case LIBMEGAPIXELS_CFA_GBRG:
			return "GBRG";
		case LIBMEGAPIXELS_CFA_GRBG:
			return "GRBG";
		case LIBMEGAPIXELS_CFA_RGGB:
			return "RGGB";
		default:
			return NULL;
	}
}

const char *
libmegapixels_format_cfa_pattern(int format)
{
	switch (mode_lut[format].cfa) {
		case LIBMEGAPIXELS_CFA_BGGR:
			return "\002\001\001\000";
		case LIBMEGAPIXELS_CFA_GBRG:
			return "\001\002\000\001";
		case LIBMEGAPIXELS_CFA_GRBG:
			return "\001\000\002\001";
		case LIBMEGAPIXELS_CFA_RGGB:
			return "\000\001\001\002";
		default:
			return NULL;
	}
}

// mp_pixel_format_width_to_bytes
uint32_t
libmegapixels_mode_width_to_bytes(int index, uint32_t width)
{
	uint32_t bits_per_pixel = mode_lut[index].bpp;
	uint64_t bits_per_width = width * (uint64_t) bits_per_pixel;
	uint64_t remainder = bits_per_width % 8;
	if (remainder == 0)
		return bits_per_width / 8;

	return (bits_per_width + 8 - remainder) / 8;
}

uint32_t
libmegapixels_mode_width_to_padding(int index, uint32_t width)
{
	if (mode_lut[index].bpp == 8) {
		return 0;
	}
	uint64_t bytes_per_width = libmegapixels_mode_width_to_bytes(index, width);
	uint64_t remainder = bytes_per_width % 8;
	if (remainder == 0)
		return remainder;

	return 8 - remainder;
}

uint32_t
libmegapixels_mode_raw_width_to_width(int index, uint32_t width)
{
	switch (mode_lut[index].v4l_pixel_format) {
		case V4L2_PIX_FMT_SBGGR8:
		case V4L2_PIX_FMT_SGBRG8:
		case V4L2_PIX_FMT_SGRBG8:
		case V4L2_PIX_FMT_SRGGB8:
			return width / 2;
		case V4L2_PIX_FMT_SBGGR10P:
		case V4L2_PIX_FMT_SGBRG10P:
		case V4L2_PIX_FMT_SGRBG10P:
		case V4L2_PIX_FMT_SRGGB10P:
			return width / 2 * 5;
		case V4L2_PIX_FMT_UYVY:
		case V4L2_PIX_FMT_YUYV:
			return width;
		default:
			return 0;
	}
}

uint32_t
libmegapixels_mode_raw_height_to_height(int index, uint32_t height)
{
	switch (mode_lut[index].v4l_pixel_format) {
		case V4L2_PIX_FMT_SBGGR8:
		case V4L2_PIX_FMT_SGBRG8:
		case V4L2_PIX_FMT_SGRBG8:
		case V4L2_PIX_FMT_SRGGB8:
		case V4L2_PIX_FMT_SBGGR10P:
		case V4L2_PIX_FMT_SGBRG10P:
		case V4L2_PIX_FMT_SGRBG10P:
		case V4L2_PIX_FMT_SRGGB10P:
			return height / 2;
		case V4L2_PIX_FMT_UYVY:
		case V4L2_PIX_FMT_YUYV:
			return height;
		default:
			return 0;
	}
}

int
libmegapixels_mode_equals(libmegapixels_mode *a, libmegapixels_mode *b)
{
	if (a == NULL || b == NULL) {
		return 0;
	}
	if (a->width != b->width) {
		return 0;
	}
	if (a->height != b->height) {
		return 0;
	}
	if (a->rate != b->rate) {
		return 0;
	}
	if (a->format != b->format) {
		return 0;
	}
	return 1;
}

int
mode_snprintf(char *buf, size_t maxlen, libmegapixels_mode *mode)
{
	return snprintf(buf, maxlen, "%dx%d@%d %s", mode->width, mode->height, mode->rate, mode_lut[mode->format].name);
}