//c decoder

#include "main.h"
#include "WaveletC.h"

static BYTE y[MAXXRES*MAXYRES];
static BYTE u[MAXXRES*MAXYRES];
static BYTE v[MAXXRES*MAXYRES];

static FPU dataf[MAXXRES*MAXYRES];
static FPU dataf2[MAXXRES*MAXYRES];
static BYTE temp[MAXXRES*MAXYRES];

static float imatrix[3][3] =
{
	{ 1.0f, 0.f, 1.4022f},
    { 1.0f, -0.3456f, -0.7145f},
    { 1.0f, 1.7710f, 0.f}
};

// YUV -> RGB STUFF

void yuv2rgb (BYTE *y, BYTE *u, BYTE *v, BYTE *r, BYTE *g, BYTE *b)
{
    	float tr = *y * imatrix[0][0] + (*u-128.0f) * imatrix[0][1] + (*v-128.0f) * imatrix[0][2];
        float tg = *y * imatrix[1][0] + (*u-128.0f) * imatrix[1][1] + (*v-128.0f) * imatrix[1][2];
        float tb = *y * imatrix[2][0] + (*u-128.0f) * imatrix[2][1] + (*v-128.0f) * imatrix[2][2];
        if (tr<0) tr = 0; if (tr>255) tr=255;
        if (tg<0) tg = 0; if (tg>255) tg=255;
        if (tb<0) tb = 0; if (tb>255) tb=255;
        *r = (BYTE)tr;
        *g = (BYTE)tg;
        *b = (BYTE)tb;
}

void yuv2rgb_block (BYTE *y, BYTE *u, BYTE *v, BYTE *argb, int w, int h, int samplex, int sampley){
	BYTE *tu;
    BYTE *tv;
    int xi, yi;
	for (yi=0; yi<h; yi++){
    	tu = u;
        tv = v;
    	for (xi=0; xi<w; xi++) {
            //slow.. first optimize the single pixel functions
            yuv2rgb (y, u, v, &argb[2], &argb[1], &argb[0]);
            // yuv2rgb (y, u, v, &argb[0], &argb[1], &argb[2]);
            if ((xi%samplex==(samplex-1))) {
            	u++;
                v++;
            }
            y++;
        	argb+=4;
        }
	 	if (yi%sampley!=(sampley-1)){
        	u = tu;
            v = tv;
        }
    }
}

// RLE STUFF

BYTE inbit (int *pos, BYTE *src){
	int bitpos = (*pos)&0x7;
    int bytepos = (*pos)>>3;
    (*pos)++;
    return (src[bytepos]>>bitpos)&1;
}

unsigned int invalue (int *pos, BYTE *src, int bits){
	unsigned int r=0;
    int i;
	for (i=0; i<bits; i++) r |= inbit (pos, src) << i;
    return r;
}

int decode_rle1 (BYTE *data, BYTE *dest, BYTE zerovalue, int size){
	int outpos = 0;
    int inpos = 0;
    unsigned int l,i;
	while (outpos<size) {
    	if (inbit (&inpos, data)==1){
        	dest[outpos++] = invalue (&inpos, data, 8);
        } else {
        	l = invalue (&inpos, data, LENBITS);
            for (i=0; i<l; i++) dest[outpos++] = zerovalue;
        }
    }
    return (inpos+8)>>3;
}

// QUANTISATZER

BYTE quant_single (FPU value, QHEADER h) {
    float s = 0;
    if (h.max!=h.min) s = (float) 255.0 / (h.max-h.min);
	return (BYTE)((value-h.min)*s);
}

void dequant_fpu (BYTE *data, int size, FPU *dest, QHEADER h){
    float s = 0;
    int o;
    float i;
    if (h.max!=h.min) s = (h.max - h.min)/(float) 255.0;
    for (o=0; o<size; o++) {
    	i = ((float)data[o])*s + h.min;
    	dest[o] = i;
    }
}

// WAVELET STUFF

void extend (FPU_WAVELET *wave) {
	//extends l coefs into h, li, hi
    int i,j;
	for (i=0;i<wave->numc;i++){
	    j=wave->numc-(i+1);
	    wave->h[i]=(float) ((j&1)*2.0-1.0)*wave->l[j];
    	wave->li[i]=wave->l[j];
	    wave->hi[i]=(float) ((i&1)*2.0-1.0)*wave->l[i];
	}
}

FPU_WAVELET default_daubechies ()
{
    FPU_WAVELET wave;
  	wave.numc=6;
  	wave.l[0]=(float) 0.332670552950;
  	wave.l[1]=(float) 0.806891509311;
  	wave.l[2]=(float) 0.459877502118;
  	wave.l[3]=(float) -.135011020010;
  	wave.l[4]=(float) -.085441273882;
  	wave.l[5]=(float) 0.035226291882;
    extend (&wave);
    return wave;
}

void wave_backward_vline (FPU *dest, FPU *datal, FPU *datah, int we, int h, FPU_WAVELET *wave){
	FPU sum1, sum2;
    int l = h>>1;
    int y, j;
    for (y=0; y<h; y+=2) {
    	sum1 = 0;
        sum2 = 0;
	    for (j=0; j<wave->numc; j+=2){
        	 sum1 += datal [((((y+j)>>1))%l)*we] * wave->li[j] + datah[((((y+j)>>1))%l)*we] * wave->hi[j];
             sum2 += datal [((((y+j)>>1))%l)*we] * wave->li[j+1] + datah[((((y+j)>>1))%l)*we] * wave->hi[j+1];
        }
    	dest[y*we] = sum2;
        dest[(y+1)*we] = sum1;
    }
}

void wave_backward_hline (FPU *dest, FPU *datal, FPU *datah, int w, FPU_WAVELET *wave){
	FPU sum1, sum2;
    int l = w>>1;
    int x, j;
    for (x=0; x<w; x+=2) {
    	sum1 = 0;
        sum2 = 0;
	    for (j=0; j<wave->numc; j+=2){
        	 sum1 += datal [(((x+j)>>1))%l] * wave->li[j] + datah[(((x+j)>>1))%l] * wave->hi[j];
             sum2 += datal [(((x+j)>>1))%l] * wave->li[j+1] + datah[(((x+j)>>1))%l] * wave->hi[j+1];
        }
    	dest[x] = sum2;
        dest[x+1] = sum1;
    }
}

//DECODER

void decode (BYTE *data, BYTE *dest, int w, int h, int levels){
	QHEADER qh;
    FPU_WAVELET wave = default_daubechies ();
    int srco = 0;
    int i, c, hpsize, x, y, rlesize;

    w>>=levels;
    h>>=levels;
	//step one: dequant lowpass (=step three)
    memcpy (&qh, data, sizeof (QHEADER));
    dequant_fpu (&data[sizeof(QHEADER)], w*h, dataf, qh);
    srco += w*h+sizeof (QHEADER);
    //step two: decomp & dequant highpass coefs
    for (i=0; i<levels; i++){
       	hpsize = (w<<1)*(h<<1) - (w*h);
        //compression
        memcpy (&qh, &data[srco], sizeof (QHEADER));
        rlesize = decode_rle1 (&data[srco+sizeof(QHEADER)], temp, quant_single (0.0, qh), hpsize);
        dequant_fpu (temp, hpsize, &dataf[w*h], qh);
        srco += sizeof (QHEADER) + rlesize;
    	w<<=1;
        h<<=1;
    }
    //step three: wavelet backwards
    w>>=levels;
    h>>=levels;
    for (i=0; i<levels; i++) {
        w<<=1;
        h<<=1;
    	for (x=0; x<(w>>1); x++) //vertical
			wave_backward_vline (&dataf2[x], &dataf[x], &dataf[x+((w*h)>>2)] , w>>1, h, &wave);
		memcpy (&dataf2[((w*h)>>1)],&dataf[((w*h)>>1)],(w*h*sizeof(FPU))>>1);
    	for (y=0; y<h; y++) //horizontal
    		wave_backward_hline (&dataf[y*w],&dataf2[y*(w>>1)], &dataf2[y*(w>>1)+((w*h)>>1)], w, &wave);
    }
    //step four: back from fpu
    for (i=0; i<w*h; i++) {
		c = (int) dataf[i];
        if (c<0) c=0;
        if (c>255) c=255;
    	dest[i] = (BYTE)c;
    }

}


//RGB WRAPPER

void load_kwi(BYTE *src, BYTE *dest)
{
    WAVEHEADER *wh = (WAVEHEADER*)src;
    int ysize, usize;
	
    memcpy (&ysize, &src[sizeof (WAVEHEADER)], 4);
    memcpy (&usize, &src[sizeof (WAVEHEADER)+4+ysize], 4);

    decode (&src[sizeof (WAVEHEADER)+4], y, wh->xres, wh->yres, wh->levels);
    decode (&src[sizeof (WAVEHEADER)+8+ysize], u, wh->xres/wh->subs, wh->yres/wh->subs, wh->levels/wh->ldiv);
    decode (&src[sizeof (WAVEHEADER)+12+usize+ysize], v, wh->xres/wh->subs, wh->yres/wh->subs, wh->levels/wh->ldiv);
    yuv2rgb_block (y, u, v, dest, wh->xres, wh->yres, wh->subs, wh->subs);
}



