#include "tinygl/zgl.h"

// image conversion

void gl_convertRGB_to_5R6G5B(unsigned short *pixmap, unsigned char *rgb,
                             int xsize, int ysize) {
	int i, n;
	unsigned char *p;

	p = rgb;
	n = xsize * ysize;
	for (i = 0; i < n; i++) {
		pixmap[i] = ((p[0] & 0xF8) << 8) | ((p[1] & 0xFC) << 3) | ((p[2] & 0xF8) >> 3); 
		p += 3;
	}
}

// linear interpolation with xf, yf normalized to 2^16

#define INTERP_NORM_BITS  16
#define INTERP_NORM       (1 << INTERP_NORM_BITS)

static inline int interpolate(int v00, int v01, int v10, int xf, int yf) {
	return v00 + (((v01 - v00) * xf + (v10 - v00) * yf) >> INTERP_NORM_BITS);
}

// TODO: more accurate resampling 

void gl_resizeImage(unsigned char *dest, int xsize_dest, int ysize_dest,
                    unsigned char *src, int xsize_src, int ysize_src) {
	unsigned char *pix, *pix_src;
	float x1, y1, x1inc, y1inc;
	int xi, yi, j, xf, yf, x, y;

	pix = dest;
	pix_src = src;
  
	x1inc = (float)(xsize_src - 1) / (float)(xsize_dest - 1);
	y1inc = (float)(ysize_src - 1) / (float)(ysize_dest - 1);

	y1 = 0;
	for (y = 0; y < ysize_dest; y++) {
		x1 = 0;
		for (x = 0; x < xsize_dest; x++) {
			xi = (int)x1;
			yi = (int)y1;
			xf = (int)((x1 - floor(x1)) * INTERP_NORM);
			yf = (int)((y1 - floor(y1)) * INTERP_NORM);

			if ((xf + yf) <= INTERP_NORM) {
				for (j = 0; j < 3; j++) {
					pix[j] = interpolate(pix_src[(yi * xsize_src + xi) * 3 + j],
								pix_src[(yi * xsize_src + xi + 1) * 3 + j],
								pix_src[((yi + 1) * xsize_src + xi) * 3 + j],
								xf, yf);
				}
			} else {
					xf = INTERP_NORM - xf;
					yf = INTERP_NORM - yf;
					for (j = 0; j < 3; j++) {
						pix[j] = interpolate(pix_src[((yi + 1) * xsize_src + xi + 1) * 3 + j],
						pix_src[((yi + 1) * xsize_src + xi) * 3 + j],
						pix_src[(yi * xsize_src + xi + 1) * 3 + j],
						xf, yf);
					}
			}
			pix += 3;
			x1 += x1inc;
		}
		y1 += y1inc;
	}
}

#define FRAC_BITS 16

// resizing with no interlating nor nearest pixel
void gl_resizeImageNoInterpolate(unsigned char *dest, int xsize_dest, int ysize_dest,
                                 unsigned char *src, int xsize_src, int ysize_src) {
	unsigned char *pix, *pix_src, *pix1;
	int x1, y1, x1inc, y1inc;
	int xi, yi, x, y;

	pix = dest;
	pix_src = src;

	x1inc = (int)((float)((xsize_src) << FRAC_BITS) / (float)(xsize_dest));
	y1inc = (int)((float)((ysize_src) << FRAC_BITS) / (float)(ysize_dest));

	y1 = 0;
	for (y = 0; y < ysize_dest; y++) {
		x1 = 0;
		for (x = 0; x < xsize_dest; x++) {
			xi = x1 >> FRAC_BITS;
			yi = y1 >> FRAC_BITS;
			pix1 = pix_src + (yi * xsize_src + xi) * 3;

			pix[0] = pix1[0];
			pix[1] = pix1[1];
			pix[2] = pix1[2];

			pix += 3;
			x1 += x1inc;
		}
		y1 += y1inc;
	}
}