/*
	MPEG Decoding Functions				Rich Webb 4/27/94
    
    Version 1: rww 27apr94
	Added IQ function.
*/

#include <ultra64.h>
#include "tok.h"
#include "all.h"
#include "mpeg_proto.h"
#include "mpeg_lproto.h"


int
mpeg_iquant( short *in, short *out, short *qmat, int quant, int intra_flag )
{
    int x,i;
    static short *prev_qmat = NULL;
    static int prev_quant = 0;
    static short sqmat[64];

    if( (qmat != prev_qmat) || (prev_quant != quant) )
	msp_scale_qmat( qmat, quant, sqmat );
    
    prev_qmat  = qmat;
    prev_quant = quant;

    qmat = sqmat;

    if( intra_flag ) { 		/* INTRA */

	    *out++ = *in++ * 8;		/* DC */
	    qmat++;

	    for(i=1;i<64;i++) {
		x = *in++;
		x = (2*x*(*qmat++))/32; /* NOTE: MPEG-2 scale quant! */

		if( (x & 1) == 0 )	/* Should we make it odd? */
		    x = x - SIGN(x);
		
		if( x > 2047 )
		    x = 2047;
		else if( x < -2048 )
		    x = -2048;
		
		*out++ = x;
	    };

    } else {

	    *out++ = *in++ * 8;		/* DC */
	    qmat++;

	    for(i=1;i<64;i++) {
		x = *in++;
		x = ((2*x+SIGN(x))*(*qmat++))/32; /* NOTE: MPEG-2 scale quant! */

		if( (x & 1) == 0 )	/* Should we make it odd? */
		    x = x - SIGN(x);
		
		if( x > 2047 )
		    x = 2047;
		else if( x < -2048 )
		    x = -2048;
		
		*out++ = x;
	    };
    }
    return( 0 );
}


int
SIGN( int x )
{
    if( x < 0 )
	return( -1 );

    if( x > 0 )
	return( 1 );

    return( 0 );
}

void ref_idct( short * ); 

void IDct8x8_fastInt16(short *);


int
mpeg_idct( short *block )
{

   IDct8x8_fastInt16(block); 

   return( 0 );
}


#define MB_YTILE_SIZE	(16*16)
#define MB_UVTILE_SIZE	(16*8)

#define MB_YREF_SIZE	(2*16*17)
#define MB_UVREF_SIZE	(2*16*9)

unsigned char fyref[MB_YREF_SIZE];
unsigned char byref[MB_YREF_SIZE];

unsigned char fuvref[MB_UVREF_SIZE];
unsigned char buvref[MB_UVREF_SIZE];

unsigned char yref[MB_YTILE_SIZE];
unsigned char uvref[MB_UVTILE_SIZE];

unsigned char xtern_yref[MB_YTILE_SIZE];
unsigned char xtern_uvref[MB_UVTILE_SIZE];

#define TL_YTILE	(0)
#define TR_YTILE	(MB_YREF_SIZE/2)

#define TL_UVTILE	(0)
#define TR_UVTILE	(MB_UVREF_SIZE/2)

#include <string.h>


int save_mbx, save_mby;

int
mpeg_fetch_ref_tile( int xsize, int luma_size, short mbx, short mby,
		     int mvfx, int mvfy, PIC for_pic,
		     int mvbx, int mvby, PIC bak_pic,
		     MacroBlock *pmb )
{
    int src, len;
    int olen, nlines;
    PIC for_chroma;
    PIC bak_chroma;
    int out_mvx;
    int out_mvy;

    int xoff;
    int yxoff;
    int i;

    short mb_type = pmb->header.mbtype;

    save_mbx = mbx;
    save_mby = mby;

    for_chroma = for_pic + luma_size + mby * 8 * xsize + mbx * 16 * 8;
    bak_chroma = bak_pic + luma_size + mby * 8 * xsize + mbx * 16 * 8;

    for_pic += mby * 16 * xsize + mbx * 16 * 16;
    bak_pic += mby * 16 * xsize + mbx * 16 * 16;

    if( mb_type & TOK_MBTYPE_FOR ) {
	src = mvfy>>5;				/* Vert. whole tiles */
	src *= xsize * 16;
	src += (mvfx>>5) * MB_YTILE_SIZE;	/* Hor. whole tiles */
	nlines = ((mvfy>>1) & 0xf) * 16;
	src += nlines;					/* Vert. lines in tile */
	len = MB_YTILE_SIZE - nlines;
	memcpy( &(pmb->fyref[TL_YTILE]), for_pic + src, len );

	src += MB_YTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->fyref[TR_YTILE]), for_pic + src, len );
	olen = len;

	src = (mvfy>>5) + 1;			/* Vert. whole tiles */
	src *= xsize * 16;
	src += (mvfx>>5) * MB_YTILE_SIZE;	/* Hor. whole tiles */
	len = MB_YTILE_SIZE - len + 16;
	memcpy( &(pmb->fyref[TL_YTILE+olen]), for_pic + src, len );

	src += MB_YTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->fyref[TR_YTILE+olen]), for_pic + src, len );

	yxoff = ((mvfx>>1) & 0xf);

	out_mvx = mvfx;
	out_mvy = mvfy;

	if( (mvfy & 1) && (mvfy < 0) )	/* Nudge fraction in the right dir. */
	    mvfy++;

	if( (mvfx & 1) && (mvfx < 0) )	/* Nudge fraction in the right dir. */
	    mvfx++;

	src = mvfy>>5;				/* Vert. whole tiles */
	src *= xsize * 8;
	src += (mvfx>>5) * MB_UVTILE_SIZE;	/* Hor. whole tiles */
	nlines = ((mvfy>>2) & 0x7) * 16;
	src += nlines;					/* Vert. lines in tile */
	len = MB_UVTILE_SIZE - nlines;
	memcpy( &(pmb->fuvref[TL_UVTILE]), for_chroma + src, len );

	src += MB_UVTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->fuvref[TR_UVTILE]), for_chroma + src, len );
	olen = len;

	src = (mvfy>>5) + 1;			/* Vert. whole tiles */
	src *= xsize * 8;
	src += (mvfx>>5) * MB_UVTILE_SIZE;	/* Hor. whole tiles */
	len = MB_UVTILE_SIZE - len + 16;
	memcpy( &(pmb->fuvref[TL_UVTILE+olen]), for_chroma + src, len );

	src += MB_UVTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->fuvref[TR_UVTILE+olen]), for_chroma + src, len );

	xoff = ((mvfx>>2) & 0x7);

#ifdef DEBUG
        printf("FMV Chroma=(%lg,%lg)\n", out_mvx/2.0, out_mvy/2.0 );
#endif

#ifdef C_CHECK
	memcpy( fyref,  pmb->fyref,  MB_YREF_SIZE);
	memcpy( fuvref, pmb->fuvref, MB_UVREF_SIZE);

	resamp_y(   fyref, yxoff, out_mvx & 1, out_mvy & 1 );
	resamp_uv( fuvref, xoff, (mvfx>>1) & 1, (mvfy>>1) & 1 );
#endif
    };

    if( mb_type & TOK_MBTYPE_BAK ) {
	src = mvby>>5;
	src *= xsize * 16;
	src += (mvbx>>5) * MB_YTILE_SIZE;	/* Hor. whole tiles */
	nlines = ((mvby>>1) & 0xf) * 16;
	src += nlines;					/* Vert. lines in tile */
	len = MB_YTILE_SIZE - nlines;
	memcpy( &(pmb->byref[TL_YTILE]), bak_pic + src, len );

	src += MB_YTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->byref[TR_YTILE]), bak_pic + src, len );
	olen = len;

	src = (mvby>>5) + 1;
	src *= xsize * 16;
	src += (mvbx>>5) * MB_YTILE_SIZE;	/* Hor. whole tiles */
	len = MB_YTILE_SIZE - len + 16;
	memcpy( &(pmb->byref[TL_YTILE+olen]), bak_pic + src, len );

	src += MB_YTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->byref[TR_YTILE+olen]), bak_pic + src, len );

	yxoff = ((mvbx>>1) & 0xf);

	out_mvx = mvbx;
	out_mvy = mvby;

	if( (mvby & 1) && (mvby < 0) )	/* Nudge fraction in the right dir. */
	    mvby++;

	if( (mvbx & 1) && (mvbx < 0) )	/* Nudge fraction in the right dir. */
	    mvbx++;

	src = mvby>>5;				/* Vert. whole tiles */
	src *= xsize * 8;
	src += (mvbx>>5) * MB_UVTILE_SIZE;	/* Hor. whole tiles */
	nlines = ((mvby>>2) & 0x7) * 16;
	src += nlines;					/* Vert. lines in tile */
	len = MB_UVTILE_SIZE - nlines;
	memcpy( &(pmb->buvref[TL_UVTILE]), bak_chroma + src, len );

	src += MB_UVTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->buvref[TR_UVTILE]), bak_chroma + src, len );
	olen = len;

	src = (mvby>>5) + 1;			/* Vert. whole tiles */
	src *= xsize * 8;
	src += (mvbx>>5) * MB_UVTILE_SIZE;	/* Hor. whole tiles */
	len = MB_UVTILE_SIZE - len + 16;
	memcpy( &(pmb->buvref[TL_UVTILE+olen]), bak_chroma + src, len );

	src += MB_UVTILE_SIZE;		  /* right adjacent tile */
	memcpy( &(pmb->buvref[TR_UVTILE+olen]), bak_chroma + src, len );

	xoff = ((mvbx>>2) & 0x7);

#ifdef DEBUG
        printf("BMV Chroma=(%lg,%lg)\n", out_mvx/2.0, out_mvy/2.0 );
#endif

#ifdef C_CHECK
        memcpy( byref,  pmb->byref,  MB_YREF_SIZE);
	memcpy( buvref, pmb->buvref, MB_UVREF_SIZE);

	resamp_y(  byref, yxoff, out_mvx & 1, out_mvy & 1 );
	resamp_uv( buvref, xoff, (mvbx>>1) & 1, (mvby>>1) & 1 );
#endif
    };

#ifdef C_CHECK
    if( mb_type & TOK_MBTYPE_FOR ) {
	if( mb_type & TOK_MBTYPE_BAK ) {
	    int i;

	    for( i=0; i<MB_YTILE_SIZE; i++)
		yref[i] = (fyref[i] + byref[i] + 1) >> 1;

	    for( i=0; i<MB_UVTILE_SIZE; i++)
		uvref[i] = (fuvref[i] + buvref[i] + 1) >> 1;

	} else {
	    memcpy( yref, fyref, MB_YTILE_SIZE );
	    memcpy( uvref, fuvref, MB_UVTILE_SIZE );
	};
    } else if ( mb_type & TOK_MBTYPE_BAK ) {
	    memcpy( yref, byref, MB_YTILE_SIZE );
	    memcpy( uvref, buvref, MB_UVTILE_SIZE );
    };
#endif
}


void
resamp_uv( unsigned char *uvin, int xoff, int halfx, int halfy )
{
    int x,y;
    int xt,xth;
    int yt,yth;

    xoff  <<= 1;
    halfx <<= 1;

/*	Leave halfy alone!!!!  */

    for(y=0; y<8; y++)
	for(x=0; x<16; x++) {

	    xt = x+xoff;
	    xth = xt + halfx;
	    
	    yt = y;
	    yth = yt + halfy;
	    
	    if( xt >= 16 )
		xt += TR_UVTILE - 16;
	    if( xth >= 16 )
		xth += TR_UVTILE - 16;
	    
	    uvin[16*y+x] = (uvin[16*yt + xt] +
			    uvin[16*yt + xth] +
			    uvin[16*yth + xt] +
			    uvin[16*yth + xth] + 2) >> 2;
	};
}


void
resamp_y( unsigned char *yin, int xoff, int halfx, int halfy )
{
    int x,y;
    int xt,xth;
    int yt,yth;

    for(y=0; y<16; y++)
	for(x=0; x<16; x++) {

	    xt = x+xoff;
	    xth = xt + halfx;
	    
	    yt = y;
	    yth = yt + halfy;
	    
	    if( xt >= 16 )
		xt += TR_YTILE - 16;
	    if( xth >= 16 )
		xth += TR_YTILE - 16;
	    
	    yin[16*y+x] = (yin[16*yt + xt] +
			    yin[16*yt + xth] +
			    yin[16*yth + xt] +
			    yin[16*yth + xth] + 2) >> 2;
	};
}


int
mpeg_recon( short *data, int mb_type )
{
    int x,y;
    unsigned char *p;

    
    if( (mb_type & TOK_MBTYPE_FOR) ||
	(mb_type & TOK_MBTYPE_BAK)   ) {

	p = yref;
        for(y=0; y<8; y++)
            for(x=0; x<8; x++)
                *data++ += (int) (p[16*y+x]);

        p += 8;
        for(y=0; y<8; y++)
            for(x=0; x<8; x++)
                *data++ += (int) (p[16*y+x]);

        p += 16*8 - 8;
        for(y=0; y<8; y++)
            for(x=0; x<8; x++)
                *data++ += (int) (p[16*y+x]);

        p += 8;
        for(y=0; y<8; y++)
            for(x=0; x<8; x++)
                *data++ += (int) (p[16*y+x]);

        p = uvref;
        for(y=0; y<8; y++)
            for(x=0; x<8; x++)
                *data++ += (int) (p[16*y+x*2]);

        p += 1;
        for(y=0; y<8; y++)
            for(x=0; x<8; x++)
                *data++ += (int) (p[16*y+x*2]);

    };
    
}


int
mpeg_save_mb_tile( short *res, PIC base, int ix, int iy, int width, int cr_offset, int cb_offset )
{
    unsigned char *cur;
    int x,y;
    int sum = 0;
    int i;

    cur = base + iy * 16 * width + ix * 16 * 16;

    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    sum += cur[y*16+x] = *res++;
    
    cur += 8;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    sum += cur[y*16+x] = *res++;
    
    cur += 8*16 - 8;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    sum += cur[y*16+x] = *res++;
    
    cur += 8;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    sum += cur[y*16+x] = *res++;
    
    cur = base + cr_offset + iy * 8 * width + ix * 8 * 16;

    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*16+x*2] = *res++;
    
    cur = base + cb_offset + iy * 8 * width + ix * 8 * 16;

    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*16+x*2] = *res++;
    
    if( sum == 0 )
	printf("Totally black block at (%d, %d)\n", ix, iy );


    return( sum );
    
}


int
mpeg_save_mb( short *res, PIC base, int ix, int iy, int width, int cr_offset, int cb_offset )
{
    unsigned char *cur;
    int x,y;

    cur = base + iy * 16 * width + ix * 16;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*width+x] = *res++;
    
    cur += 8;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*width+x] = *res++;
    
    cur = base + (iy * 16 + 8) * width + ix * 16;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*width+x] = *res++;
    
    cur += 8;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*width+x] = *res++;
    
    width >>= 1;
    cur = base + cr_offset + iy * 8 * width + ix * 8;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*width+x] = *res++;
    
    cur = base + cb_offset + iy * 8 * width + ix * 8;
    for(y=0;y<8;y++)
	for(x=0;x<8;x++)
	    cur[y*width+x] = *res++;
    
}

/*
int
mpeg_save_pic( int tref, PIC pic_addr, int len )
{
    FILE *out;
    char outname[1024];

    sprintf( outname, "mpg%d.y", tref );
    out = fopen( outname, "w" );
    fwrite( pic_addr, len, sizeof( pic_addr[0] ), out );
    fclose( out );
    pic_addr += len;

    len >>= 2;

    sprintf( outname, "mpg%d.u", tref );
    out = fopen( outname, "w" );
    fwrite( pic_addr, len, sizeof( pic_addr[0] ), out );
    fclose( out );
    pic_addr += len;

    sprintf( outname, "mpg%d.v", tref );
    out = fopen( outname, "w" );
    fwrite( pic_addr, len, sizeof( pic_addr[0] ), out );
    fclose( out );
}
*/

#define ZSHIFT	(0)

static short vconsts[8] = { 1<<ZSHIFT, 2<<ZSHIFT, ~1, -2047, 2048, 0, 0, 0 };

#define SCALE	(vconsts[0])
#define SCALE2	(vconsts[1])
#define MASK	(vconsts[2])
#define IQ_MIN	(vconsts[3])
#define IQ_MAX	(vconsts[4])
#define QUANT	(vconsts[5])

#define NEW_MULQ


void
msp_intra_iq( short *in, short *out, short *qmat )
{
    int i;
    short out_dc;
    short *out_orig = out;

    out_dc = in[0] << 3;

    for(i=0; i<8; i++) {
	vmuli(  in, SCALE2, out );	/* Scale */

#ifdef NEW_MULQ

	vmulq( out, qmat, out );

#else
	vmulz( out, qmat,   out );

	vnori( out, MASK, even );	/* Oddification */
	vabs( out, even, even );
	vsub( out, even, out  );

	vcmpgti( out, IQ_MIN, out );	/* Clamp */
	vcmplti( out, IQ_MAX, out );
#endif

	in += 8;
	out += 8;
	qmat += 8;
    };

    out_orig[0] = out_dc;
}


void
msp_nonintra_iq( short *in, short *out, short *qmat )
{
    int i;
    short sign[8];
    short out_dc;
    short *out_orig = out;

    out_dc = in[0] << 3;

    for(i=0; i<8; i++) {
	vabsi( in,  SCALE,  sign );	/* Scale */
	vmuli( in,  SCALE2, out );
	vadd(  out, sign,   out );
#ifdef NEW_MULQ

	vmulq( out, qmat, out );

#else
	vmulz( out, qmat,   out );

	vnori( out, MASK, even );	/* Oddification */
	vabs(  out, even, even );
	vsub(  out, even, out  );

	vcmpgti( out, IQ_MIN, out );	/* Clamp */
	vcmplti( out, IQ_MAX, out );
#endif

	in += 8;
	out += 8;
	qmat += 8;
    };

    out_orig[0] = out_dc;
}


void
msp_scale_qmat( short *in, int scale, short *out )
{
    int i;

    QUANT = scale;

    for(i=0; i<8; i++) {
	vmuli(  in, QUANT, out );	/* Scale */

	in += 8;
	out += 8;
    };
}


void
vabs( short *in, short *in2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	if( in[i] < 0 )
	    out[i] = -in2[i];
	else if( in[i] > 0 )
	    out[i] = in2[i];
	else
	    out[i] = in[i];	/* Implicitly in[i] == 0 */
}

void
vabsi( short *in, int cin2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	if( in[i] < 0 )
	    out[i] = -cin2;
	else if( in[i] > 0 )
	    out[i] = cin2;
	else
	    out[i] = in[i];	/* Implicitly in[i] == 0 */
}

void
vadd( short *in, short *in2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	out[i] = in[i] + in2[i];
}


void
vsub( short *in, short *in2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	out[i] = in[i] - in2[i];
}


void
vmul( short *in, short *in2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	out[i] = in[i] * in2[i];
}


void
vmuli( short *in, int cin2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	out[i] = in[i] * cin2;
}

#define QSHIFT	(19-ZSHIFT)

void
vmulq( short *in, short *in2, short *out )
{
    int i;
    long long int res;
    int round;
    int h,m,l, s,y,x,z;

    for(i=0; i<8; i++) {
	res = ((int) in[i]) * ((int) in2[i]);
	res <<= QSHIFT-5;

	h = (-1<<(QSHIFT+2));
	m = 1<<(QSHIFT+1);
	l = 1<<QSHIFT;

#define Z	8
#define S	4
#define Y	2
#define X	1

	z = (res==0)?Z:0;
	s = (res<0)?S:0;
	y = (res & l)?Y:0;
	x = (res & (l-1))?X:0;

	switch (z+s+y+x) {
	    case       0: round = h + m + l; break;
	    case       X: round = h + m + l; break;
	    case     Y  : round =         0; break;
	    case     Y+X: round =         0; break;

	    case   S    : round =         l; break;
	    case   S  +X: round =         l; break;
	    case   S+Y  : round =         0; break;
	    case   S+Y+X: round =     m    ; break;
	    case Z      : round =         0; break;
	};

	res += round;

	if( res >= (1<<30) )
	    round = (1<<30) - 1;
	else if( res < (-1<<30) )
	    round = (-1<<30);
	else
	    round = res;

	out[i] = round >>(QSHIFT);
    };
}


void
vmulz( short *in, short *in2, short *out )
{
    int i;
    int res;

    for(i=0; i<8; i++) {
	res = ((int) in[i]) * ((int) in2[i]);

	if( in[i] < 0 )
	    res += (1<<(ZSHIFT+5)) - 1;	/* Round as if this were a div */

	out[i] = res >>(ZSHIFT + 5);
    };
}

void
vnori( short *in, int cin2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	out[i] = ~( in[i] | cin2 );
}

void
vcmpgti( short *in, int cin2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	if( in[i] > cin2 )
	    out[i] = in[i];
	else
	    out[i] = cin2;
}

void
vcmplti( short *in, int cin2, short *out )
{
    int i;

    for(i=0; i<8; i++)
	if( in[i] < cin2 )
	    out[i] = in[i];
	else
	    out[i] = cin2;
}