/********************************************************************************
 *																				*
 * This code has been developed by Project Mayo. This software is an			*
 * implementation of a part of one or more MPEG-4 Video tools as				*
 * specified in ISO/IEC 14496-2 standard.  Those intending to use this			*
 * software module in hardware or software products are advised that its		*
 * use may infringe existing patents or copyrights, and any such use			*
 * would be at such party's own risk.  The original developer of this			*
 * software module and his/her company, and subsequent editors and their		*
 * companies (including Project Mayo), will have no liability for use of		*
 * this software or modifications or derivatives thereof.						*
 *																				*
 ********************************************************************************
 *																				*	
 * This program is free software; you can redistribute it and/or modify			*
 * it under the terms of the GNU General Public License as published by			*
 * the Free Software Foundation; either version 2 of the License, or			*
 * (at your option) any later version.											*
 *																				*
 * The GPL can be found at: http://www.gnu.org/copyleft/gpl.html				*
 *																				*
 * Authors:																		*
 *																				*
 *	Andrea Graziani	(Ag):														*
 *		- Original source code (Open Divx Decoder 0.4a).						*		
 *																				*
 *	Pedro Mateu		(Pm):														*
 *		- Heavily modified and optimized code + MIPS ASM						*
 *																				*
 ********************************************************************************/

// Motion Vector Management. 

#include "global.h"
#include "mp4_mv.h"

static int Get_MV_Data()
{
	unsigned short code;

	if (getbits1()) return 0; // hor_mv_data == 0
	
	code = showbits(12);

	if (code >= 512)
	{
		code = ((code >> 8) - 2)<<1;
		flushbits(MVtab0[code+1]);
		return MVtab0[code];
	}
	
	if (code >= 128)
	{
		code = ((code >> 2) - 32)<<1;
		flushbits(MVtab1[code+1]);
		return MVtab1[code];
	}

	code = (code-4)<<1; 

	flushbits(MVtab2[code+1]);
	return MVtab2[code];
}

static int Get_PMV(unsigned char Block_Num)
{
	int p1, p2, p3, temp;
	
	unsigned char x = mp4_hdr.mb_xpos;
	unsigned char y = mp4_hdr.mb_ypos;
		
	switch (Block_Num)
		{
			case 0:
				if (!(x|y)) return 0;  //We take into consideration the borders
				if (!y) return MV[x-1][1];
				if (!x) {
					p1= MV[x][2];
					p2= 0;
					p3= MV[x+1][2];
					break;
				}
				p1 = MV[x-1][1];
				p2 = MV[x][2];
				p3 = MV[x+1][2];
				break;
			case 1:
				if (!y)	return MV[x][0];
				p1 = MV[x][0];
				p2 = MV[x][3];
				p3 = MV[x+1][2];
				break;	
			case 2:
				if (!x) {
					p1 = 0;
					p2 = MV[x][0];
					p3 = MV[x][1];
					break;
				}
				p1 = MV[x-1][3];
				p2 = MV[x][0];
				p3 = MV[x][1];
				break;
			default: // case 3
				p1 = MV[x][2];
				p2 = MV[x][0];
				p3 = MV[x][1];
				break;
		}
	temp = mmin(mmax(p1, p2), mmin(mmax(p2, p3), mmax(p1, p3)));
	p1=(p1<<16)>>16;
	p2=(p2<<16)>>16;
	p3=(p3<<16)>>16;
	p1 = mmin(mmax(p1, p2), mmin(mmax(p2, p3), mmax(p1, p3)));

	return ((temp&0xFFFF0000)|(p1&0xFFFF));

}

void setMV(int Block_Num)
{
	int hor_mv_data, ver_mv_data, hor_mv_res, ver_mv_res;
	int scale_fac = 1 << (mp4_hdr.fcode_for - 1);
	int high = (scale_fac<<5) - 1;
	int low = -(scale_fac<<5);
	int range = (scale_fac<<6);
	int mvd_x, mvd_y, pmv_x, pmv_y, mv_x, mv_y;
	
	hor_mv_data = Get_MV_Data(); // mv data

	if ((scale_fac == 1) || (hor_mv_data == 0))
		mvd_x = hor_mv_data;
	else {
		hor_mv_res = getbits(mp4_hdr.fcode_for-1); // mv residual
		mvd_x = ((abs(hor_mv_data) - 1) * scale_fac) + hor_mv_res + 1;
		if (hor_mv_data < 0)
			mvd_x = - mvd_x;
	}
	
	ver_mv_data = Get_MV_Data(); 
	
	if ((scale_fac == 1) || (ver_mv_data == 0))
		mvd_y = ver_mv_data;
	else {
		ver_mv_res = getbits(mp4_hdr.fcode_for-1);
		mvd_y = ((abs(ver_mv_data) - 1) * scale_fac) + ver_mv_res + 1;
		if (ver_mv_data < 0)
			mvd_y = - mvd_y;
	}
	
	if (Block_Num<0){
	pmv_x = Get_PMV(0);
	}
	else {
	pmv_x = Get_PMV(Block_Num);
	}
	pmv_y = (pmv_x>>16);
	pmv_x = (pmv_x<<16)>>16;

	mv_x = pmv_x + mvd_x;
	
	if (mv_x < low)
		mv_x += range;
	
	if (mv_x > high)
		mv_x -= range;
		
	mv_y = pmv_y + mvd_y;
	
	if (mv_y < low)
		mv_y += range;
	
	if (mv_y > high)
		mv_y -= range;
	
	// put [mv_x, mv_y] in MV struct
	if (Block_Num == -1) {
		#ifdef MIPS_ASM
			__asm ("sll		$6,$6,16;"
				   "andi	$5,$5,0xFFFF;"
				   "or		$8,$6,$5;"
				   "sw		$8,0($4);"
				   "sw		$8,4($4);"
				   "sw		$8,8($4);"
				   "sw		$8,12($4);",&MV[mp4_hdr.mb_xpos][0],mv_x,mv_y);
		#else 
		int i;
		for (i = 0; i < 4; i++) {
			MV[mp4_hdr.mb_xpos][i] = (mv_y<<16)|(mv_x&0xFFFF);
		}
		#endif
	}
	else {
		#ifdef MIPS_ASM
			__asm ("sll		$6,$6,16;"
				   "andi	$5,$5,0xFFFF;"
				   "or		$8,$6,$5;"
				   "sw		$8,0($4);",&MV[mp4_hdr.mb_xpos][Block_Num],mv_x,mv_y);
		#else
		MV[mp4_hdr.mb_xpos][Block_Num] = (mv_y<<16)|(mv_x&0xFFFF);
		#endif
	}
}
