/*
    Token Parsing Routines				Rich Webb 4/11/94

Version 1: rww 11apr94
    Processes Tokens into blocks.
*/

#include <ultra64.h>
#include <string.h>
#include <stdio.h>
#include "tok.h"
#include "all.h"
#include "mpeg_proto.h"
#include "tok_proto.h"

#define DEBUG

FILE *tok_file;

int
tok_open_stream( char *name )
{
    int stream;
    int i;

    stream = tok_num_streams++;

    tok_file = fopen(name, "r");

    if (tok_file == NULL) {
        printf("TOK_OPEN_STREAM: Couldn't open <%s>", name);
	perror(":");
	return(-1);
    }

    tok_cur_context = &(tok_contexts[stream]);

    for(i=0; i<64; i++)
        tok_cur_context->iqmat[i] = tok_iqmat_default[i];

    for(i=0; i<64; i++)
        tok_cur_context->niqmat[i] = tok_niqmat_default[i];

    if( tok_parse_seq_hdr() < 0 ) {
	backup_page = 1;
/*	return(-1); */
    }

/*
    if( tok_cur_context->ref1 == NULL )
*/
    tok_alloc_pics();    

    return(stream);

}

void
tok_alloc_pics( void )
{
    tok_cur_context->xsize = XSIZE;
    tok_cur_context->ysize = YSIZE;

    tok_cur_context->cur        = tok_cur_context->ref1;
    tok_cur_context->ref_for    = tok_cur_context->ref2;
    tok_cur_context->ref_bak    = tok_cur_context->ref1;
    tok_cur_context->bidir_next = tok_cur_context->bidir1;
    
}

void
tok_close_stream( int stream )
{
}

void
tok_save_context( void )
{
    tok_cur_context->cur        = tok_cur_pic; 
    tok_cur_context->ref_for    = tok_ref_for_pic; 
    tok_cur_context->ref_bak    = tok_ref_bak_pic; 
    tok_cur_context->bidir_next = tok_bidir_next_pic; 
    tok_cur_context->tref_cur   = tok_tref_cur; 
    tok_cur_context->ptype_cur  = tok_ptype_cur;
    tok_cur_context->mbnum      = tok_mbnum;
    tok_cur_context->tokstream  = tok_stream_ptr;
    tok_cur_context->tokend     = tok_stream_end;
}

void
tok_get_context( void )
{
    tok_cur_raster     = tok_cur_context->raster;
    tok_cur_pic        = tok_cur_context->cur;
    tok_ref_for_pic    = tok_cur_context->ref_for;
    tok_ref_bak_pic    = tok_cur_context->ref_bak;
    tok_bidir_next_pic = tok_cur_context->bidir_next;
    tok_tref_cur       = tok_cur_context->tref_cur;
    tok_ptype_cur      = tok_cur_context->ptype_cur;

    tok_mbnum          = tok_cur_context->mbnum;
    tok_stream_ptr     = tok_cur_context->tokstream;
    tok_stream_end     = tok_cur_context->tokend;

    tok_niqmat = &(tok_cur_context->niqmat[0]);
    tok_iqmat  = &(tok_cur_context->iqmat[0]);

    tok_luma_size = tok_cur_context->xsize * tok_cur_context->ysize;
    tok_line_size = tok_cur_context->xsize;
}



int
tok_parse_seq_hdr( void )
{
    int status;
    int i;

    status = tok_prepare_next_page();
    if( status < 0 )
	return( status );
    
    if( cur_tok_buf[tok_n] != TOK_NEXT_SEQ )
	return( -1 );
    
    tok_n++;
    tok_cur_context->xsize      = cur_tok_buf[tok_n++];
    tok_cur_context->ysize      = cur_tok_buf[tok_n++];
    tok_cur_context->bitrate_h  = cur_tok_buf[tok_n++];
    tok_cur_context->bitrate_l  = cur_tok_buf[tok_n++];
    tok_cur_context->bitbuf_h   = cur_tok_buf[tok_n++];
    tok_cur_context->bitbuf_l   = cur_tok_buf[tok_n++];
    tok_cur_context->pic_rate   = cur_tok_buf[tok_n++];

    for(i=0; i<64; i++)
	tok_cur_context->iqmat[i] = cur_tok_buf[tok_n++];

    for(i=0; i<64; i++)
	tok_cur_context->niqmat[i] = cur_tok_buf[tok_n++];

}



int
tok_process_next_page( int stream, int max_work )
{
    int ntot = 0;
    int status, n;

    tok_cur_context = &(tok_contexts[stream]);

    tok_get_context();

    while( max_work > 0 ) {

	status = tok_prepare_next_page();
	if( status < 0 )
	    return( status );

	n = tok_process_page();

	if( n == 0 )
	    return( -1 );

	if( n < 0 ) {
	    n = -n;
	    ntot += n;
	    break;
	};

	ntot += n;
	max_work -= n;
    };

    tok_save_context();

    return( ntot );

}


/* Takes next page of tokens and splits them into two arrays,
   *tok_index (low 6 bits) and *tok_lev (high 9bits, signed).
*/

int
tok_prepare_next_page( void )
{
    short *tmp; 
    int len;

    if( backup_page == 0 ) {
	tmp          = cur_tok_buf;	/* Swap pages */
	cur_tok_buf  = prev_tok_buf;
	prev_tok_buf = tmp;
    
#ifdef DEBUG
	printf("DMA in from ROM\n");
#endif
/*
       dmaCallBack(tok_stream_ptr, TOK_BUF_SIZE*sizeof(short), cur_tok_buf, 1);
       tok_stream_ptr += TOK_BUF_SIZE;	
   
       if (tok_stream_ptr >= tok_stream_end)
           return( -1);
*/
	len = fread(cur_tok_buf, sizeof(short), TOK_BUF_SIZE, tok_file);
	if (len < TOK_BUF_SIZE)
	    return(-1);
    }

    vand( cur_tok_buf, 0x3f, tok_index, TOK_BUF_SIZE );

    vshift( cur_tok_buf, 6, tok_lev, TOK_BUF_SIZE ); /* 2*lev+sgn(lev) */

    tok_n = 0;

    backup_page = 0;
}


int
tok_process_page( void )
{

    if( (cur_tok_buf[tok_n] == TOK_NEXT_PIC) ) {
	tok_n++;
	tok_tref_cur  = cur_tok_buf[tok_n++];
	tok_ptype_cur = cur_tok_buf[tok_n++];

	printf("\nPic %d(type=%d): ", tok_tref_cur, tok_ptype_cur );

	printf("Begin Pic #%d (type = %d)\n", tok_tref_cur, tok_ptype_cur );

	tok_mbnum = 0;
	mb_x = 0;
	mb_y = 0;

	if( tok_ptype_cur != TOK_PTYPE_BIDIR ) {
	    PIC tmp;

	    tmp = tok_ref_for_pic;
	    tok_ref_for_pic = tok_ref_bak_pic;
	    tok_ref_bak_pic = tmp;

	    tok_cur_pic = tok_ref_bak_pic;
	} else {
	    tok_cur_pic = tok_bidir_next_pic;
	    if( tok_cur_pic == tok_cur_context->bidir1 )
		tok_bidir_next_pic = tok_cur_context->bidir2;
	    else
		tok_bidir_next_pic = tok_cur_context->bidir1;
	};

    };
	

    while( (cur_tok_buf[tok_n] == TOK_NEXT_MB) ) {
	tok_n++;
	tok_get_mb( &mb );
	tok_dec_mb( &mb );

/*
	status = mpeg_save_mb_tile( mblock_data, tok_cur_pic, mb_x, mb_y, 
			 tok_line_size, tok_luma_size, tok_luma_size + 1 );


#ifdef DEBUG
	if( status == 0 ) {
	    printf("\tquant = %d     mbtype = 0x%04x\n", quant, mbtype);
	    printf("\tmvf = (%d,%d)   mvb = (%d,%d)\n", mvfx,mvfy, mvbx,mvby);
	};
#endif
*/

	tok_mbnum++;

	mb_x++;
	if( mb_x*16 >= tok_line_size ) {
	    mb_x = 0;
	    mb_y++;
	};
    };

    if( cur_tok_buf[tok_n] != TOK_END_PAGE ) {
	if( cur_tok_buf[tok_n] != TOK_END_PIC ) {
	    printf(" (tok[%d]=0x%04hx) ", tok_n, cur_tok_buf[tok_n] );
	    printf("End Page for Strange Reason\n" );
	} else
	    printf("End Page and Pic\n" );
	return( -tok_n );
    };

    return( tok_n );
}


/* Unpack tokens into Macroblock structure */

void
tok_get_mb( MacroBlock *mb_ptr)
{
    int prev_bi;
    int blk_indx;
    short cbp = 0;

    tok_get_mbh(mb_ptr);

    mpeg_fetch_ref_tile( tok_line_size, tok_luma_size, mb_x, mb_y,
			 mvfx, mvfy, tok_ref_for_pic,
			 mvbx, mvby, tok_ref_bak_pic,
			 mb_ptr );

    tok_zero_mb(mb_ptr);

    prev_bi = -1;

    while( (cur_tok_buf[tok_n] < tok_num_max_block) ) {


	blk_indx = tok_index[tok_n++];

	prev_bi = blk_indx;

	cbp |= (1<<blk_indx);

#ifdef PRE_ODD
	while( tok_lev[tok_n] != 1 )
#else
	while( tok_lev[tok_n] != 0 )
#endif
	{
	    mb_ptr->mbdata[blk_indx][ tok_index[tok_n] ] = tok_lev[tok_n];
	    tok_n++;
	}
    }
    mb_ptr->header.cbp = cbp;
    mb_ptr->header.block_num = tok_mbnum;
    
}   


void
xtern_mpeg(MacroBlock *pmb)
{
   FILE *src;
   FILE *dst;
   short dummy;

   src = fopen("in.pipe", "w");
   fwrite(tok_iqmat, sizeof(short), 64, src);
   fwrite(tok_niqmat, sizeof(short), 64, src);
   fclose(src);

   dst = fopen("out.pipe", "r");
   fread(&dummy, sizeof(short), 1, dst);
   fclose(dst);

   src = fopen("in.pipe", "w");
   fwrite(pmb, sizeof(MacroBlock), 1, src);
   fclose(src);

   dst = fopen("out.pipe", "r");
   fread(pmb, sizeof(MacroBlock), 1, dst);
   fclose(dst);
 }


/* Decodes the next Macroblock */


void
tok_dec_mb( MacroBlock *mb_ptr)
{
    int i,j;
    short block[MB_DATA_SIZE];
    short *pblock = block;
    unsigned char mb_chk[MB_DATA_SIZE];
    unsigned char *ybase, *uvbase;
    short cbp;
    int status, iflag;
    short *qmat;

#ifdef C_CHECK
    iflag = mbtype & TOK_MBTYPE_INTRA;

    if( iflag )
	qmat = tok_iqmat;
    else
	qmat = tok_niqmat;

    cbp = mb_ptr->header.cbp;
    memcpy( block, mb_ptr->mbdata[0], 6*64*sizeof(short) );

    while (cbp) {
        if (cbp & 1) {
      	    mpeg_iquant( pblock, pblock, qmat, quant, iflag );	
	    mpeg_idct( pblock );
	}
        pblock += 64;
	cbp = cbp >> 1;
    }

    mpeg_recon( block, mbtype );	/* Add reference and pack results */
    
    mpeg_save_mb_tile(block, mb_chk, 0, 0, 0, 256, 257);

#endif

/*    
    startVideoTask(vmpegMainDataStart+MBOFFSET, sizeof(MacroBlock)); 
*/

    xtern_mpeg(mb_ptr);

#ifdef C_CHECK

    ybase = mb_chk;

    for(i=0; i<256; i++) 
        if (ybase[i] != mb_ptr->fyref[i])
	    printf("Mismatch(Y=%d): real=%d, ucode=%d\n",
	         i, ybase[i], mb_ptr->fyref[i]);

    uvbase = ybase + 256;

    for(i=0; i<128; i++)
        if (uvbase[i] != mb_ptr->fuvref[i])
	    printf("Mismatch(UV=%d): real=%d, ucode=%d\n",
	         i, uvbase[i], mb_ptr->fuvref[i]);	   

#endif

    
    memcpy(tok_cur_pic+256*tok_mbnum, mb_ptr->fyref, 256);
    memcpy(tok_cur_pic+tok_luma_size+128*tok_mbnum, mb_ptr->fuvref, 128);

#ifdef DEBUG
    printf("Done macroblock, %d\n", tok_mbnum);
#endif

}

int
tok_get_mbh( MacroBlock *mb_ptr )
{
    int mv_low;

    mbtype = cur_tok_buf[tok_n++];

    if( mbtype & TOK_MBTYPE_INTRA ) 
	quant = cur_tok_buf[tok_n++];


    if( mbtype & TOK_MBTYPE_ERRS )
	quant = cur_tok_buf[tok_n++];


    if( mbtype & TOK_MBTYPE_FOR ) {
	mv_low = cur_tok_buf[tok_n++];


	mvfx = (mbtype>>TOK_FMVX_HIGH_SHIFT) & TOK_MVX_HIGH_MASK;
	mvfx <<= (32 - TOK_MVX_HIGH_BITS);
	mvfx >>= (32 - TOK_MVX_HIGH_BITS);
	mvfx = (mvfx << TOK_MVX_LOW_BITS) + ((mv_low >> TOK_MVX_LOW_SHIFT) & TOK_MVX_LOW_MASK);

	mvfy = (mbtype>>TOK_FMVY_HIGH_SHIFT) & TOK_MVY_HIGH_MASK;
	mvfy <<= (32 - TOK_MVY_HIGH_BITS);
	mvfy >>= (32 - TOK_MVY_HIGH_BITS);
	mvfy = (mvfy << TOK_MVY_LOW_BITS) + ((mv_low >> TOK_MVY_LOW_SHIFT) & TOK_MVY_LOW_MASK);

#ifdef DEBUG
	if( (mvfx > 100) || (mvfy > 100) || (mvfx < -100) || (mvfy < -100) )
	    printf("MVF at (%d,%d) = ( %d , %d ) (mvlow=0x%04x, mbt=0x%04x)\n",
		mb_x, mb_y, mvfx, mvfy, mv_low, mbtype );
#endif
    };

    if( mbtype & TOK_MBTYPE_BAK ) {
	mv_low = cur_tok_buf[tok_n++];

	mvbx = (mbtype>>TOK_BMVX_HIGH_SHIFT) & TOK_MVX_HIGH_MASK;
	mvbx <<= (32 - TOK_MVX_HIGH_BITS);
	mvbx >>= (32 - TOK_MVX_HIGH_BITS);
	mvbx = (mvbx << TOK_MVX_LOW_BITS) + ((mv_low >> TOK_MVX_LOW_SHIFT) & TOK_MVX_LOW_MASK);

	mvby = (mbtype>>TOK_BMVY_HIGH_SHIFT) & TOK_MVY_HIGH_MASK;
	mvby <<= (32 - TOK_MVY_HIGH_BITS);
	mvby >>= (32 - TOK_MVY_HIGH_BITS);
	mvby = (mvby << TOK_MVY_LOW_BITS) + ((mv_low >> TOK_MVY_LOW_SHIFT) & TOK_MVY_LOW_MASK);

#ifdef DEBUG
	if( (mvbx > 100) || (mvby > 100) || (mvbx < -100) || (mvby < -100) )
	    printf("MVB at (%d,%d) = ( %d , %d ) (mvlow=0x%04x, mbt=0x%04x)\n",
		    mb_x, mb_y, mvbx, mvby, mv_low, mbtype );
#endif
    };
    

    mb_ptr->header.mbtype = mbtype;
    mb_ptr->header.quant = quant;
    mb_ptr->header.mvfx = mvfx;
    mb_ptr->header.mvfy = mvfy;
    mb_ptr->header.mvbx = mvbx;
    mb_ptr->header.mvby = mvby;

    return( 0 );
}


/* tok_zero_mb - Zeros all of the macroblock data. */


void 
tok_zero_mb( MacroBlock *mb_ptr )
{
    int i;
    short *mbdata = mb_ptr->mbdata[0];

    i = tok_num_max_block << 6;

    while( i ) {
	i -= 8;
	vstore( mbdata, i, vzero ); 
    };
}

void
vstore( short *dmem_addr, int index, short *vdata)
{
    int i;

    for(i=0; i<8; i++)
	dmem_addr[index+i] = vdata[i];
}


void
vand( short *dm_in, int mask, short *dm_out, int len )
{
    while( len-- )
	*dm_out++ = *dm_in++ & mask;
}

void
vshift( short *dm_in, int shift, short *dm_out, int len )
{
#ifdef PRE_ODD
    int x, s;
#endif

    while( len-- ) {
#ifdef PRE_ODD
	x = *dm_in++;
	s = x >> 15;
	s = s+s+1;			/* 0 => 1 which is a minor problem */
	*dm_out++ = ((x >> shift) << 1) + s;
#else
	*dm_out++ = *dm_in++ >> shift;
#endif
    };
}