#include "ka_scalers.h"
#include "ka_mem.h"
#include <string.h>

typedef void (*FNmerge_lines)(uint32_t* pdst, const uint32_t* line0, const uint32_t* line1, uint32_t sc, int width);
typedef void (*FNscale_line)(uint16_t* pdst, uint32_t width, const uint16_t* psrc, int x0, int x_mag);

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 5-6-5 bits per component(TRGB16, TBGR16).
 * Use linear interpolation (on the horizontal) when zooming in, no interpolation when zooming out.
 */
static void ka_scale_linear_halfword(ka_scale_t* scale, FNscale_line fscale_line)
{
	uint16_t* pdst = (uint16_t*) scale->wdst;
	const int sx0 = scale->x_mag * scale->wdst_x0;
	int sy0 = scale->y_mag * scale->wdst_y0;

	if (scale->y_mag < (1 << 16))
	{
		// assume increased mem usage is compensated by less computation
		const uint16_t* psrc = ((const uint16_t*) scale->src) + scale->src_pix_width * (sy0 >> 16);

		int bsize = 2 * scale->wdst_width;
		if (scale->wb_size < bsize)
		{
			void* ptr = ka_mem_alloc(bsize);
			if (!ptr)
				goto fallback; // use fallback method

			scale->wb_ptr = ptr;
			scale->wb_size = bsize;
		}
		uint16_t* line = scale->wb_ptr;
		fscale_line(line, scale->wdst_width, psrc, sx0, scale->x_mag);
		int sy16 = sy0 & 0xffff;
		sy0 >>= 16;

		for (int y = scale->wdst_height; y > 0; y--)
		{
			memcpy(pdst, line, 2 * scale->wdst_width);

			sy16 += scale->y_mag;
			if (sy16 >= (1<<16))
			{
				sy0 += 1;
				sy16 &= 0xffff;
				if (sy0 < scale->src_height - 1)
				{
					psrc += scale->src_pix_width;
					fscale_line(line, scale->wdst_width, psrc, sx0, scale->x_mag);
				}
			}

			pdst += (scale->wdst_bpr / 2);
		}
		return;
	}

fallback:

	for (int y = scale->wdst_height; y > 0; y--)
	{
		const uint16_t* psrc = ((const uint16_t*) scale->src) + scale->src_pix_width * (sy0 >> 16);

		fscale_line(pdst, scale->wdst_width, psrc, sx0, scale->x_mag);

		sy0 += scale->y_mag;
		pdst += (scale->wdst_bpr / 2);
	}
}

static void copy_line16(uint16_t* pdst, uint32_t width, const uint16_t* psrc, int x0, int x_mag)
{
	x_mag = x_mag; // unused
	memcpy(pdst, psrc + (x0 >> 16), 2 * width);
}

static void scaledown_line16(uint16_t* pdst, uint32_t width, const uint16_t* psrc, int x0, int x_mag)
{
	int x;

	for (x = width; x & 3; x--)
	{
		*pdst++ = psrc[x0 >> 16];
		x0 += x_mag;
	}
	for (; x > 0; x-= 4)
	{
		*pdst++ = psrc[x0 >> 16];
		x0 += x_mag;
		*pdst++ = psrc[x0 >> 16];
		x0 += x_mag;
		*pdst++ = psrc[x0 >> 16];
		x0 += x_mag;
		*pdst++ = psrc[x0 >> 16];
		x0 += x_mag;
	}
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 5-6-5 bits per component(TRGB16, TBGR16).
 * Use linear interpolation (on the horizontal) when zooming in, no interpolation when zooming out.
 */
static inline FNscale_line ka_select_scale_line_16bpp(ka_scale_t* scale)
{
	if (scale->x_mag < (1 << 16))
	{
		if (scale->hardware & ka_hardware_neon)
			return scaleup_line_565_neon;
		else if (scale->hardware & ka_hardware_ldrh)
			return scaleup_line_565_sh;
		else
			return scaleup_line_565;
	}
	else if (scale->x_mag == (1 << 16))
		return copy_line16;
	else
		return scaledown_line16;
}

void ka_scale_linear_16bpp(ka_scale_t* scale)
{
	ka_scale_linear_halfword(scale,  ka_select_scale_line_16bpp(scale));
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 5-5-5 bits per component(TRGB15, TBGR15).
 * Use linear interpolation (on the horizontal) when zooming in, no interpolation when zooming out.
 */
static inline FNscale_line ka_select_scale_line_15bpp(ka_scale_t* scale)
{
	if (scale->x_mag < (1 << 16))
	{
		if (scale->hardware & ka_hardware_neon)
			return scaleup_line_555_neon;
		else if (scale->hardware & ka_hardware_ldrh)
			return scaleup_line_555_sh;
		else
			return scaleup_line_555;
	}
	else if (scale->x_mag == (1 << 16))
		return copy_line16;
	else
		return scaledown_line16;
}

void ka_scale_linear_15bpp(ka_scale_t* scale)
{
	ka_scale_linear_halfword(scale,  ka_select_scale_line_15bpp(scale));
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 4-4-4 bits per component(TRGB12, TBGR12).
 * Use linear interpolation (on the horizontal) when zooming in, no interpolation when zooming out.
 */
static inline FNscale_line ka_select_scale_line_12bpp(ka_scale_t* scale)
{
	if (scale->x_mag < (1 << 16))
	{
		if (scale->hardware & ka_hardware_neon)
			return scaleup_line_444_neon;
		else if (scale->hardware & ka_hardware_ldrh)
			return scaleup_line_444_sh;
		else
			return scaleup_line_444;
	}
	else if (scale->x_mag == (1 << 16))
		return copy_line16;
	else
		return scaledown_line16;
}

void ka_scale_linear_12bpp(ka_scale_t* scale)
{
	ka_scale_linear_halfword(scale,  ka_select_scale_line_12bpp(scale));
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 5-6-5 bits per component(TRGB16, TBGR16).
 * Use linear interpolation (on horiz. & vert.) when zooming in, no interpolation when zooming out.
 * Precondition: source byte width is word aligned.
 */
static void ka_scale_bilinear_halfword(ka_scale_t* scale, FNmerge_lines fmerge_lines, FNscale_line fscale_line)
{
	uint16_t* pdst = (uint16_t*) scale->wdst;
	const int sx0 = scale->x_mag * scale->wdst_x0;
	int sy0 = scale->y_mag * scale->wdst_y0;

	if (scale->y_mag < (1 << 16))
	{
		const uint16_t* psrc = ((const uint16_t*) scale->src) + scale->src_pix_width * (sy0 >> 16);
		const int skipb = (((int) scale->wdst) >> 1) & 1;
		int width = skipb + scale->wdst_width;
		const int skipe = width & 1;
		pdst -= skipb;
		width += skipe;

		if (scale->wb_size < 4 * width)
		{
			void* ptr = ka_mem_alloc(4 * width);
			if (ptr)
			{
				scale->wb_ptr = ptr;
				scale->wb_size = 4 * width;
			}
			else
			{
				// fallback
				ka_scale_linear_halfword(scale, fscale_line);
				return;
			}
		}
		uint16_t* line0 = scale->wb_ptr;
		uint16_t* line1 = line0 + width;

		fscale_line(line0 + skipb, scale->wdst_width, psrc, sx0, scale->x_mag);
		psrc += scale->src_pix_width;
		memcpy(line1, line0, 2*width);
		sy0 &= 0xffff;

		for (int y = scale->wdst_height; y > 0; y--)
		{
			int x;

			for (x = 0; x < skipb; x++)
			{
				line0[x] = line1[x] = pdst[x];
			}
			for (x = width - skipe; x < width; x++)
			{
				line0[x] = line1[x] = pdst[x];
			}

			fmerge_lines((uint32_t*) pdst, (uint32_t*) line0, (uint32_t*) line1, sy0, width / 2);

			sy0 += scale->y_mag;
			if (sy0 >= (1<<16))
			{
				uint16_t* linex = line0;
				line0 = line1;
				line1 = linex;
				fscale_line(line1 + skipb, scale->wdst_width, psrc, sx0, scale->x_mag);
				psrc += scale->src_pix_width;
				sy0 &= 0xffff;
			}

			pdst += (scale->wdst_bpr / 2);
		}
	}
	else
	{
		for (int y = scale->wdst_height; y > 0; y--)
		{
			const uint16_t* psrc = ((const uint16_t*) scale->src) + scale->src_pix_width * (sy0 >> 16);
			fscale_line(pdst, scale->wdst_width, psrc, sx0, scale->x_mag);

			sy0 += scale->y_mag;
			pdst += (scale->wdst_bpr / 2);
		}
	}
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 5-6-5 bits per component(TRGB16, TBGR16).
 * Use linear interpolation (on horiz. & vert.) when zooming in, no interpolation when zooming out.
 * Precondition: source byte width is word aligned.
 */
static inline FNmerge_lines ka_select_merge_lines_16bpp(ka_scale_t* scale)
{
	if (scale->hardware & ka_hardware_neon)
		return merge_lines_565_neon;
	else if (scale->hardware & ka_hardware_ldrh)
		return merge_lines_565_sh;
	else
		return merge_lines_565;
}

void ka_scale_bilinear_16bpp(ka_scale_t* scale)
{
	FNscale_line  fscale_line = ka_select_scale_line_16bpp(scale);
	FNmerge_lines fmerge_lines = ka_select_merge_lines_16bpp(scale);

	ka_scale_bilinear_halfword(scale, fmerge_lines, fscale_line);
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 5-5-5 bits per component(TRGB15, TBGR15).
 * Use linear interpolation (on horiz. & vert.) when zooming in, no interpolation when zooming out.
 * Precondition: source byte width is word aligned.
 */
static inline FNmerge_lines ka_select_merge_lines_t15bpp(ka_scale_t* scale)
{
	if (scale->hardware & ka_hardware_neon)
		return merge_lines_t555_neon;
	else if (scale->hardware & ka_hardware_ldrh)
		return merge_lines_t555_sh;
	else
		return merge_lines_t555;
}

void ka_scale_bilinear_t15bpp(ka_scale_t* scale)
{
	FNscale_line  fscale_line = ka_select_scale_line_15bpp(scale);
	FNmerge_lines fmerge_lines = ka_select_merge_lines_t15bpp(scale);

	ka_scale_bilinear_halfword(scale, fmerge_lines, fscale_line);
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 5-5-5 bits per component(TRGB15, TBGR15).
 * Use linear interpolation (on horiz. & vert.) when zooming in, no interpolation when zooming out.
 * Precondition: source byte width is word aligned.
 */
static inline FNmerge_lines ka_select_merge_lines_a15bpp(ka_scale_t* scale)
{
	if (scale->hardware & ka_hardware_neon)
		return merge_lines_a555_neon;
	else if (scale->hardware & ka_hardware_ldrh)
		return merge_lines_a555_sh;
	else
		return merge_lines_a555;
}

void ka_scale_bilinear_a15bpp(ka_scale_t* scale)
{
	FNscale_line  fscale_line = ka_select_scale_line_15bpp(scale);
	FNmerge_lines fmerge_lines = ka_select_merge_lines_a15bpp(scale);

	ka_scale_bilinear_halfword(scale, fmerge_lines, fscale_line);
}

/*
 * Scale image with 16-bit pixels (Halfword aligned) with 4-4-4 bits per component(TRGB12, TBGR12).
 * Use linear interpolation (on horiz. & vert.) when zooming in, no interpolation when zooming out.
 * Precondition: source byte width is word aligned.
 */
static inline FNmerge_lines ka_select_merge_lines_12bpp(ka_scale_t* scale)
{
	if (scale->hardware & ka_hardware_neon)
		return merge_lines_444_neon;
	else if (scale->hardware & ka_hardware_ldrh)
		return merge_lines_444_sh;
	else
		return merge_lines_444;
}

void ka_scale_bilinear_12bpp(ka_scale_t* scale)
{
	FNscale_line  fscale_line = ka_select_scale_line_12bpp(scale);
	FNmerge_lines fmerge_lines = ka_select_merge_lines_12bpp(scale);

	ka_scale_bilinear_halfword(scale, fmerge_lines, fscale_line);
}
