/******
*
*	<>
*
*	these routines are obsoleted by "quantim" and need to make calls
*	to the quantizeImage routines with passed-in qtypes and whatnot
*
*******/

#define NEVER_QUANT_LL	// seems to be a winner

#define FASTQUANT_SHIFT 8
#define FASTQUANT_ONE (1<<FASTQUANT_SHIFT)
#define FASTQUANT_HALF (1<<(FASTQUANT_SHIFT-1))

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <crblib/inc.h>

#include "image.h"
#include "codeimage.h"
#include "quantutil.h"

/*************/

#define quantizeBands	quantizeBandsDZ		// not very cool to do this
#define quantizeBandsU	quantizeBandsDZU	// not very cool to do this
	// quantizeBands() is used by the fiddle() and whatnot routines

#define VAL_SCALE		16
#define INV_VAL_SCALE	(1.0/VAL_SCALE)
#define MAX_VAL			128
#define MAX_VAL_SCALED	(MAX_VAL * VAL_SCALE)
#define TOP_VAL_SCALED	(MAX_VAL_SCALED - 1)
#define SCALE_Q_UP	1.125
#define SCALE_Q_DN	0.9375

typedef struct {
	int size;
	int num_of_val[MAX_VAL_SCALED];

	// used in fiddle:
	int dD_up,dD_dn,dR_up,dR_dn;
	double RoD_dn,RoD_up;
} bandInfo;

int distortion(int *num_of_val,double quantizer);
void infoBand(double *ptr,int w,int h,int fullw,bandInfo *bi);
ulong packedQuantizedImageSize(imageFloat *imf,coder *coder_template,int levels,float *quantizers);
ulong packedQuantizedImageStopErr(imageFloat *imf,coder *coder_template,int levels,double quantizer,int stopSize);

/*******

seekStoppedQ : scale the quantizers[] to get a desired rate.

	this routine is imperfect, in that we almost always miss the target by
		about 100 bytes.  This is *not* a failure of the routine, but
		actually a failure of the concept : sometimes you just don't get
		rate changes when you move the quantizers a teeny bit - the image
		is not floating-point.

********/

#define MIN_STEP	0.1
#define MAX_SEEK_Q	100

void seekStoppedQuantizers_Fast(imageFloat *imF,int levels,float * quantizers,int stopSize,
	coder *coder_template)
{
//int nextErr_up,nextErr_dn,curErr;
int i,nbands;
double L,R,Lold,Rold,Lold_old,Rold_old;
int L_y,R_y,Lold_y,Rold_y;
double Lslope,Rslope,last_guess,guess;

#if 0	/** generate a plot **/
	if ( 1 ) {
		double sizer = 1.0 / imF->tot_size;
		for(L=1.0;L<MAX_SEEK_Q;L += MIN_STEP) {
			L_y = packedQuantizedImageStopErr(imF,coder_template,levels,L,stopSize);
			printf("%2.2f %f\n",L,L_y*sizer);
		}
		return;
	}
#endif

/******************
****
*		this is an implementation of the tangent-intersection search.
*			it converges in log(N) time.
		Unfortunately it only works on data whose second derivative has
			the same sign everywhere, and if you run the plots above you
			see that's not us !
		
		this search is like a binary search; we start with a L and R
			which we hope are on either side of the minimum.  We find
			the tangent at each, then the intersection of the tangents.
		This intersection is the "guess".
		We then contract L & R to converge towards the guess.

		There is one subtlety: after contraction we may have done a
			bad step, in which case the two tangets will have the same
*			sign of slope (they should always have opposite sign,
*			indicating the trough or hill).  When this happens we try
*			to "reset" to resume the search.
**
**	->	This algorithm is useless for me, because the actually shape
**			of Error(Quantizer) is nearly chaotic.
***
 **************/

	nbands=3*levels+1;

	Lold = 1.0;
	Rold = MAX_SEEK_Q;

	Lold_y = packedQuantizedImageStopErr(imF,coder_template,levels,Lold,stopSize);
	Rold_y = packedQuantizedImageStopErr(imF,coder_template,levels,Rold,stopSize);

	last_guess = guess = (Lold + Rold) * 0.5;
	
	for(;;) {

		last_guess = guess;
		L = (Lold + guess)*0.5;
		R = (Rold + guess)*0.5;
	
	resetted:

		L_y = packedQuantizedImageStopErr(imF,coder_template,levels,L,stopSize);
		R_y = packedQuantizedImageStopErr(imF,coder_template,levels,R,stopSize);

		Lslope = (L_y - Lold_y)/(L - Lold);
		Rslope = (R_y - Rold_y)/(R - Rold);

		fprintf(stderr,"Lold=%f, L=%f, guess=%f, R=%f, Rold=%f\n",Lold,L,guess,R,Rold);
		fprintf(stderr,"Lold_y=%d, L_y=%d, R_y=%d, Rold_y=%d\n",Lold_y,L_y,R_y,Rold_y);

		if ( Lslope > 0 ) {	/** screwed up, must re-try **/
			errputs("warning : reset left!");
			R = Lold;
			Rold = L;
			L = (Lold + Lold_old)*0.5;
			Lold = Lold_old;
			if ( L == Lold || R == Rold ) {
				errputs("warning : step size is zero; exiting");
				break;
			}
			guess = (L + R) * 0.5;
			goto resetted;
		} else if ( Rslope < 0 ) {
			errputs("warning : reset right!");
			L = Rold;
			Lold = R;
			R = (Rold + Rold_old)*0.5;
			Rold = Rold_old;
			if ( L == Lold || R == Rold ) {
				errputs("warning : step size is zero; exiting");
				break;
			}
			guess = (L + R) * 0.5;
			goto resetted;
		}

		guess = (Lslope * L - Rslope * R + R_y - L_y)/(Lslope - Rslope);

		if ( fabs(last_guess - guess) < MIN_STEP ) {
			break;
		}

		Lold_old = Lold;	Lold = L;	Lold_y = L_y;
		Rold_old = Rold;	Rold = R;	Rold_y = R_y;	
	}

	fprintf(stderr,"chose : %f\n",guess);

	for(i=0;i<nbands;i++) quantizers[i] = (float) guess;
}

void seekStoppedQuantizers_Slow(imageFloat *imF,int levels,float * quantizers,int stopSize,
	coder *coder_template)
{
double Q,best_Q;
ulong err,best_err,last_err;
int i;
image *im_work;

	errputs("doing seek slow...");

	best_err = LONG_MAX; best_Q = 1.0;

#if 0
	for(Q=1.0;Q<MAX_SEEK_Q;Q += MIN_STEP) {
		err = packedQuantizedImageStopErr(imF,coder_template,levels,Q,stopSize);
		if ( err < best_err ) {
			best_err = err;
			best_Q = Q;
		}
	}
#else	/** with a local copy of StopErr **/

	if ( (im_work = newImageFromFloat(imF)) == NULL ) errexit("copyimage failed");

	last_err =0;
	for(Q=1.0;Q<MAX_SEEK_Q;Q += MIN_STEP) {
		int p,r,qv,v;
		double *dp,f,dv;
		double quantme;

		fprintf(stderr,"."); fflush(stderr);

		quantizeBandsU(imF,im_work,levels,Q);
		err = packedImageStopErr(im_work,coder_template,levels,stopSize);

		quantme = 1.0 / Q;

		v = (int)(LONG_MAX*quantme*quantme);
		if ( err >= (ulong)v ) err = LONG_MAX;
		else err = (ulong)(err*Q*Q);

		for(p=0;p<imF->planes;p++) {
			dp = imF->data[p][0];
			for(r= imF->plane_size;r--;) {
				f = *dp++; if ( f < 0 ) f = -f;
				qv = (int)(f * quantme);
				if ( qv == 0 ) dv = 0;
				else dv = ((double)qv + 0.5) * Q;
				err += (ulong)((dv - f)*(dv - f) + 0.5);
			}
		}

		if ( err < best_err ) {
			if ( err >= last_err ) { /* make sure it's stable */
				best_err = err;
				best_Q = Q;
			}
		}
		last_err = err;
	}

	printf("chose Q = %f , best err = %d\n",best_Q,best_err);

	freeImage(im_work);
#endif

	for(i=0;i<=3*levels;i++) quantizers[i] = (float)best_Q;
}

void fiddleInfoBand(bandInfo *bic,double quantizer,float *quant_work,float *quant_spot,int R_cur,
	imageFloat *imF,coder *coder_template,int levels);

void fiddleQuantizers(imageFloat *imF,int levels,float * quantizers,
	coder *coder_template)
{
int nbands,b,p,l,best_up,best_dn,dR,dD,best_dD;
int R_cur,R_new,R_orig;
double **rows;
bool didFiddle;
double best_RoD;
bandInfo *bic;
bandInfo * bi = NULL; //alloc'ed
float *quant_work;

	nbands = 3*levels + 1;

/**********
	<> there's still some screw-up in the "distortion" measure
		where we think the distortion is going down, but it
		actually goes *UP* .  In fact, the distortion almost
		always goes up as a result of this fiddling, even
		though we supposedly gaurantee it goes down. :(

	A classic example :

		barb.256.raw 256 256 -tw -qu20 -s1
		barb.256.raw      :   65536 ->    8192 =  1.000 bpb =  8.000 to 1
		error: av= 5.03,max= 45,mse= 45.528,rmse= 6.75,psnr= 31.58

			got option: fiddle quantizers
		fiddle : band 1 up, 7 down
		fiddle : got dR = 22, dD = -17365
		fiddle : band 1 up, 8 down
		fiddle : got dR = -10, dD = -11373
		barb.256.raw      :   65536 ->    8202 =  1.001 bpb =  7.990 to 1
		error: av= 5.05,max= 46,mse= 46.232,rmse= 6.80,psnr= 31.52

	the "dR" estimate is good, but it claims we made a huge distortion
	improvement, while in reality we made the distortion WORSE !

**********/

	if ( (bi = newarray(bandInfo,nbands)) == NULL )
		errexit("malloc failed");

	if ( (quant_work = newarray(float,nbands)) == NULL )
		errexit("malloc failed");

	/** fiddle needs the infoBands to compute the distortions quickly **/

	for(p=0;p<imF->planes;p++) {
		rows = imF->data[p]; b=0;
		for (l = levels; l > 0; l--) {
			int sizeX,sizeY;

			sizeX = imF->width >> l;
			sizeY = imF->height >> l;

			if (l == levels)
				infoBand(rows[0], 		sizeX, sizeY, imF->width, &bi[b++]);

			infoBand(rows[0] + sizeX, 	sizeX, sizeY, imF->width, &bi[b++]);
			infoBand(rows[sizeY], 		sizeX, sizeY, imF->width, &bi[b++]);
			infoBand(rows[sizeY]+sizeX,	sizeX, sizeY, imF->width, &bi[b++]);
		}
	}

	errputs("fiddle : starting");

	R_orig = R_cur = packedQuantizedImageSize(imF,coder_template,levels,quantizers);

#if 0
	maxErr = (imF->tot_size) >> 11;	if ( maxErr < 5 ) maxErr = 5;
	R_orig += maxErr;
#endif

	/** compute current distortions **/
	for(b=0;b<nbands;b++) {
		bic = &bi[b];
		memcpy(quant_work,quantizers,sizeof(float)*nbands);
		fiddleInfoBand(&bi[b],quantizers[b],quant_work,&quant_work[b],
			R_cur,imF,coder_template,levels);
	}

	do {
		didFiddle = false;

		/** find the best RoD for increasing a quantizer **/
		best_RoD = 0; best_up = 0;
#ifdef NEVER_QUANT_LL
		for(b=1;b<nbands;b++) {
#else
		for(b=0;b<nbands;b++) {
#endif
			bic = &bi[b];
			if ( bic->RoD_up > best_RoD ) {
				best_RoD = bic->RoD_up; best_up = b;
			}
		}

		/** now find a way to decrease a quantizer so that there is
			no net rate change, but an improved distortion **/

		best_dD = 0; best_dn = -1;
#ifdef NEVER_QUANT_LL
		for(b=1;b<nbands;b++) {
#else
		for(b=0;b<nbands;b++) {
#endif
			if ( b == best_up ) continue; /** can't do up & down on same band **/
			bic = &bi[b];
			dR = bi[best_up].dR_up + bic->dR_dn;
			dD = bi[best_up].dD_up + bic->dD_dn;
			R_new = R_cur + dR;
			if ( R_new < R_orig && dD < best_dD ) {
				best_dn = b; best_dD = dD;
			}
		}

		if ( best_dD < 0 ) {
			didFiddle = true;
			dR = bi[best_up].dR_up + bi[best_dn].dR_dn;
			dD = bi[best_up].dD_up + bi[best_dn].dD_dn;
			R_cur += dR;

			printf("fiddle : band %d up, %d down\n",best_up,best_dn);
			printf("fiddle : got dR = %d, dD = %d\n",dR,dD);

			/** must now update the changed entries **/

			quantizers[best_up] *= SCALE_Q_UP;

			memcpy(quant_work,quantizers,sizeof(float)*nbands);
			fiddleInfoBand(&bi[best_up],quantizers[best_up],quant_work,&quant_work[best_up],
				R_cur,imF,coder_template,levels);

			if ( quantizers[best_dn] > 1 ) {
				quantizers[best_dn] *= SCALE_Q_DN;

				memcpy(quant_work,quantizers,sizeof(float)*nbands);
				fiddleInfoBand(&bi[best_dn],quantizers[best_dn],quant_work,&quant_work[best_dn],
					R_cur,imF,coder_template,levels);
			}
		}

	} while ( didFiddle );

	errputs("fiddle : done");

	free(quant_work);
	free(bi);

}

void fiddleInfoBand(bandInfo *bic,double quantizer,float *quant_work,float *quant_spot,int R_cur,
	imageFloat *imF,coder *coder_template,int levels)
{
int D_cur;

	D_cur		= distortion(bic->num_of_val,quantizer);
	bic->dD_up	= distortion(bic->num_of_val,quantizer*SCALE_Q_UP) - D_cur;
	if ( quantizer <= 1 ) bic->dD_dn = 0;
	else bic->dD_dn	= distortion(bic->num_of_val,quantizer*SCALE_Q_DN) - D_cur;

	*quant_spot = (float)(quantizer * SCALE_Q_UP);

	bic->dR_up = packedQuantizedImageSize(imF,coder_template,levels,quant_work) - R_cur;

	if ( quantizer <= 1 ) {
		bic->dR_dn = 0;
	} else {
		*quant_spot = (float)(quantizer * SCALE_Q_DN);

		bic->dR_dn = packedQuantizedImageSize(imF,coder_template,levels,quant_work) - R_cur;
	}

	/** dD_up > 0 and dR_up < 0 **/
	/** dD_dn < 0 and dR_dn > 0 **/

	if ( bic->dD_up <= 0 ) bic->RoD_up = - (bic->dR_up<<16);
	else	bic->RoD_up = - (bic->dR_up)/(double)(bic->dD_up);

	if ( bic->dD_dn >= 0 ) bic->RoD_dn = - (bic->dR_dn<<16);
	else	bic->RoD_dn = - (bic->dR_dn)/(double)(bic->dD_dn);
}

void infoBand(double *ptr,int w,int h,int fullw,bandInfo *bi)
{
int x,y,rowpad = fullw-w;
int v;
double val;

	bi->size = w*h;

	for(y=0;y<h;y++) {
		for(x=0;x<w;x++) {
			val = *ptr++;
			if ( val < 0 ) val = - val;
			if ( val >= MAX_VAL ) bi->num_of_val[TOP_VAL_SCALED] ++;
			else { 
				v = (int)(val * VAL_SCALE + 0.5);
				bi->num_of_val[v] ++;
			}
		}
		ptr += rowpad;
	}
}

ulong packedQuantizedImageSize(imageFloat *imf,coder *coder_template,int levels,float *quantizers)
{
image *im_work;
ulong size;

	if ( (im_work = newImageFromFloat(imf)) == NULL ) errexit("copyimage failed");
	quantizeBands(imf,im_work,levels,quantizers);
	size = packedImageSize(im_work,coder_template,levels);
	freeImage(im_work);

return size;
}

ulong packedQuantizedImageStopErr(imageFloat *imf,coder *coder_template,int levels,double quantizer,int stopSize)
{
image *im_work;
ulong err,maxerr;
int p,r,qv,nbands,i;
double *dp,f,dv;
double quantme;
float *quant_work; //alloced

	nbands = 3*levels+1;

	if ( (quant_work = newarray(float,nbands)) == NULL )
		errexit("malloc failed");

	for(i=0;i<nbands;i++) quant_work[i] = (float) quantizer;

	if ( (im_work = newImageFromFloat(imf)) == NULL ) errexit("copyimage failed");
	quantizeBands(imf,im_work,levels,quant_work);
	err = packedImageStopErr(im_work,coder_template,levels,stopSize);
	freeImage(im_work);

	/** this doesn't count the error from the quantizing !!! **/

	quantme = 1.0 / quantizer;

	maxerr = (ulong) ((double)ULONG_MAX * quantme * quantme);
	if ( err > maxerr ) err = LONG_MAX;
	else err = (ulong)(err*quantizer*quantizer);

	for(p=0;p<imf->planes;p++) {
		dp = imf->data[p][0];
		for(r= imf->plane_size;r--;) {
			f = *dp++; if ( f < 0 ) f = -f;
			qv = (int)(f * quantme);
			if ( qv == 0 ) dv = 0;
			else dv = ((double)qv + 0.5) * quantizer;
			err += (ulong)((dv - f)*(dv - f) + 0.5);
		}
	}

	free(quant_work);

return err;
}

int distortion(int *num_of_val,double quantizer)
{
double invquant,t,val,D;
int qv,n,v;

	invquant = 1.0 / quantizer;

	D = 0;
	for(v=1;v<TOP_VAL_SCALED;v++) {
		n = num_of_val[v];
		if ( n == 0 ) continue;
		val = (double)v*INV_VAL_SCALE;
#if 0
	/** this is JPEG-standard (ESZZ) quantizer ,
	**/
		qv = (int)(val * invquant + 0.5); // quantize
		t = val - (qv * quantizer);	// dequantize & get difference
#else
	/** this is the wavelet-common DeadZone quantizer
	**/
		qv = (int)(val * invquant);
		if ( qv == 0 ) t = val;
		else t = val - ((qv + 0.5)*quantizer);
#endif
		D += n*t*t;
	}
	D += (num_of_val[TOP_VAL_SCALED] * quantizer * quantizer)/4;	// maximum error on these

return (int)D;
}

/***************************************************************** $$

"dead zone quantizers" :

	quantized = val * (1.0/quantizer)

	val = sign * floor( quantizer * ( abs(val) + 0.5 ) );

	(btw, Intel rounds negative floats towards zero)

-----------------

	mussing with these in any way introduces a lot of error
		(for example, adding 0.5 to the quantized value to round it
		in the quantization step)

see quantizers.txt

**********************/

void dequantizeBandDZ(int *fmptr,double *toptr,int w,int h,int fullw,double factor)
{
int x,y,rowpad = fullw-w;
int sign,v;

	for(y=0;y<h;y++) {
		for(x=0;x<w;x++) {
			v = *fmptr++;
			if ( v == 0 ) { 
				*toptr++ = 0; 
			} else {
				if ( v < 0 ) { sign = -1; v = -v; }
				else sign = 1;
				*toptr++ = sign * factor * ((double)v + 0.5);
			}
		}
		fmptr += rowpad;
		toptr += rowpad;
	}
}

void quantizeBandDZ(double *fmptr,int *toptr,int w,int h,int fullw,double factor)
{
int x,y,rowpad = fullw-w;