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

#define FN(a, b) FNC(a, b) // needed to forced subsitution of LDRH
#define FNC(a, b) a ## b

/*
 * Merge word aligned lines with 16-bit pixels RGB16, BGR16, TBGR15, TRGB15.
 * Decomposes each word in 2 elements with at least 5 bits set to 0 between colour components.
 * By limiting linear interpolation factor (frac 0:16) to 32, we can interpolate on each element
 * without interference between their colour componants.
 * Note: For 32k modes, T/A component is only 1 bit so would interfere with the colour component following it
 *       but we have the chance that T is always 0 (opaque) and can be ignored.
 */
static void FN(merge_lines_tsh5, LDRH)(uint32_t* pdst, const uint32_t* line0, const uint32_t* line1, uint32_t sc, int width, const uint32_t mask)
{
	uint32_t val0a, val0b, val1a, val1b, b1, b2, sc2;
	int x;
	sc >>= 11;
	sc2 = 32 - sc;

	for (x = width; x & 3; x--)
	{
		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;
	}

	for (; x > 0; x-= 4)
	{
		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;
	}
}

/*
 * Merge word aligned lines with 16-bit pixels with 5-6-5 bits per component (RGB16, BGR16)
 * given a given linear interpolation factor [0-31].
 */
void FN(merge_lines_565, LDRH)(uint32_t* pdst, const uint32_t* line0, const uint32_t* line1, uint32_t sc, int width)
{
	const uint32_t mask = 0x07e0f81f;

	FN(merge_lines_tsh5, LDRH)(pdst, line0, line1, sc, width, mask);
}

/*
 * Merge word aligned lines with 16-bit pixels with 5-5-5 bits per component (TRGB15, TBGR15)
 * given a given linear interpolation factor [0-31]. Works as long as T is always 0 (opaque).
 */
void FN(merge_lines_t555, LDRH)(uint32_t* pdst, const uint32_t* line0, const uint32_t* line1, uint32_t sc, int width)
{
	const uint32_t mask = 0x03e07c1f;

	FN(merge_lines_tsh5, LDRH)(pdst, line0, line1, sc, width, mask);
}

/*
 * Merge word aligned lines with 16-bit pixels with 5-5-5 bits per component (ARGB15, ABGR15)
 * given a given linear interpolation factor [0-31].
 * We must take into accout that opqaue means A is always 1.
 */
void FN(merge_lines_a555, LDRH)(uint32_t* pdst, const uint32_t* line0, const uint32_t* line1, uint32_t sc, int width)
{
	const uint32_t mask = 0x03e07c1f;
	const uint32_t amask = 0x80008000;
	uint32_t val0a, val0b, val1a, val1b, b1, b2, sc2;
	int x;
	sc >>= 11;
	sc2 = 32 - sc;

	for (x = width; x & 3; x--)
	{
		val1a = *line1++;
		val0a = *line0++;
		val1a &= ~amask;
		val0a &= ~amask;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		b1 |= amask;
		*pdst++ = b1;
	}

	for (; x > 0; x-= 4)
	{
		val1a = *line1++;
		val0a = *line0++;
		val1a &= ~amask;
		val0a &= ~amask;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		b1 |= amask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1a &= ~amask;
		val0a &= ~amask;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		b1 |= amask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1a &= ~amask;
		val0a &= ~amask;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		b1 |= amask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1a &= ~amask;
		val0a &= ~amask;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 5;
		val0a = (val0a & ~mask) >> 5;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 5) & mask;
		b1 |= amask;
		*pdst++ = b1;
	}
}

/*
 * Merge word aligned lines with 16-bit pixels with 4-4-4 bits per component (TRGB12, TBGR12, ARGB12, ABGR12)
 * given a given linear interpolation factor [0-15].
 */
void FN(merge_lines_444, LDRH)(uint32_t* pdst, const uint32_t* line0, const uint32_t* line1, uint32_t sc, int width)
{
	const uint32_t mask = 0x0f0f0f0f;
	uint32_t val0a, val0b, val1a, val1b, b1, b2, sc2;
	int x;
	sc >>= 12;
	sc2 = 16 - sc;

	for (x = width; x & 3; x--)
	{
		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 4;
		val0a = (val0a & ~mask) >> 4;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;
	}

	for (; x > 0; x-= 4)
	{
		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 4;
		val0a = (val0a & ~mask) >> 4;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 4;
		val0a = (val0a & ~mask) >> 4;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 4;
		val0a = (val0a & ~mask) >> 4;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		val1a = *line1++;
		val0a = *line0++;
		val1b = val1a & mask;
		val0b = val0a & mask;
		val1a = (val1a & ~mask) >> 4;
		val0a = (val0a & ~mask) >> 4;
		b1 = val1a * sc + val0a * sc2;
		b2 = val1b * sc + val0b * sc2;
		b1 = b1 & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;
	}
}

/*
 * Scale line of halfword aligned 16-bit pixels RGB16, BGR16, TRGB15, TBGR15, ARGB15, ABGR15.
 * Use linear interpolation (32 steps) when zooming in, no interpolation when zooming out.
 */
static void FN(scaleup_line_sh5, LDRH)(uint16_t* pdst, uint32_t width, const uint16_t* psrc, int x0, int x_mag, const uint32_t mask)
{
	uint32_t val0a, val0b, val1a, val1b, sc, b1, b2;
	int x;

	psrc += x0 >> 16;
	x0 &= 0xffff;
	val0a = *psrc++;
	val0b = val0a & mask;
	val0a = val0a & ~mask;
	val1b = val0b;
	val1a = val0a;

	for (x = width; x & 3; x--)
	{
		sc = x0 >> 11;
		b1 = (val0a << 5) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (32 - sc);
		b1 = (b1 >> 5) & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}
	}

	for (; x > 0; x-=4)
	{
		sc = x0 >> 11;
		b1 = (val0a << 5) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (32 - sc);
		b1 = (b1 >> 5) & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}

		sc = x0 >> 11;
		b1 = (val0a << 5) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (32 - sc);
		b1 = (b1 >> 5) & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}

		sc = x0 >> 11;
		b1 = (val0a << 5) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (32 - sc);
		b1 = (b1 >> 5) & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}

		sc = x0 >> 11;
		b1 = (val0a << 5) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (32 - sc);
		b1 = (b1 >> 5) & ~mask;
		b1 |= (b2 >> 5) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}
	}
}

/*
 * Scale line of halfword aligned 16-bit pixels RGB16, BGR16.
 * Use linear interpolation when zooming in, no interpolation when zooming out.
 */
void FN(scaleup_line_565, LDRH)(uint16_t* pdst, uint32_t width, const uint16_t* psrc, int x0, int x_mag)
{
	const uint32_t mask = 0xf81f;

	FN(scaleup_line_sh5, LDRH)(pdst, width, psrc, x0, x_mag, mask);
}

/*
 * Scale line of halfword aligned 16-bit pixels TRGB15, TBGR15, ARGB15, ABGR15.
 * Use linear interpolation when zooming in, no interpolation when zooming out.
 */
void FN(scaleup_line_555, LDRH)(uint16_t* pdst, uint32_t width, const uint16_t* psrc, int x0, int x_mag)
{
	const uint32_t mask = 0x7c1f;

	FN(scaleup_line_sh5, LDRH)(pdst, width, psrc, x0, x_mag, mask);
}

/*
 * Scale line of halfword aligned 16-bit pixels TRGB12, TBGR12, ARGB12, ABGR12.
 * Use linear interpolation when zooming in, no interpolation when zooming out.
 */
void FN(scaleup_line_444, LDRH)(uint16_t* pdst, uint32_t width, const uint16_t* psrc, int x0, int x_mag)
{
	const uint32_t mask = 0x0f0f;

	uint32_t val0a, val0b, val1a, val1b, sc, b1, b2;
	int x;

	psrc += x0 >> 16;
	x0 &= 0xffff;
	val0a = *psrc++;
	val0b = val0a & mask;
	val0a = val0a & ~mask;
	val1b = val0b;
	val1a = val0a;

	for (x = width; x & 3; x--)
	{
		sc = x0 >> 12;
		b1 = (val0a << 4) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (16 - sc);
		b1 = (b1 >> 4) & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}
	}

	for (; x > 0; x-=4)
	{
		sc = x0 >> 12;
		b1 = (val0a << 4) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (16 - sc);
		b1 = (b1 >> 4) & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}

		sc = x0 >> 12;
		b1 = (val0a << 4) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (16 - sc);
		b1 = (b1 >> 4) & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}

		sc = x0 >> 12;
		b1 = (val0a << 4) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (16 - sc);
		b1 = (b1 >> 4) & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}

		sc = x0 >> 12;
		b1 = (val0a << 4) + (val1a - val0a) * sc;
		b2 = val1b * sc + val0b * (16 - sc);
		b1 = (b1 >> 4) & ~mask;
		b1 |= (b2 >> 4) & mask;
		*pdst++ = b1;

		x0 += x_mag;
		if (x0 >= (1<<16))
		{
			x0 &= 0xffff;
			val0a = val1a;
			val1a = *psrc++;
			val0b = val1b;
			val1b = val1a & mask;
			val1a = val1a & ~mask;
		}
	}
}
