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

This software module was originally developed by 

	Ming-Chieh Lee (mingcl@microsoft.com), Microsoft Corporation
	Wei-ge Chen (wchen@microsoft.com), Microsoft Corporation
	Bruce Lin (blin@microsoft.com), Microsoft Corporation
	(date: March, 1996)
and edited by
        Wei Wu (weiwu@stallion.risc.rockwell.com) Rockwell Science Center
and edited by Xuemin Chen (xchen@gi.com) General Instrument Corp.)
and also edited by
    Mathias Wien (wien@ient.rwth-aachen.de) RWTH Aachen / Robert BOSCH GmbH

and also edited by
	Yoshinori Suzuki (Hitachi, Ltd.)

and also edited by
	Prabhudev Irappa Hosur (pn188359@ntu.edu.sg) NTU singapore

and also edited by
	Hideaki Kimata (NTT)

and also edited by
    Fujitsu Laboratories Ltd. (contact: Eishi Morimatsu)

in the course of development of the MPEG-4 Video (ISO/IEC 14496-2). 
This software module is an implementation of a part of one or more MPEG-4 Video tools 
as specified by the MPEG-4 Video. 
ISO/IEC gives users of the MPEG-4 Video free license to this software module or modifications 
thereof for use in hardware or software products claiming conformance to the MPEG-4 Video. 
Those intending to use this software module in hardware or software products are advised that its use may infringe existing patents. 
The original developer of this software module and his/her company, 
the subsequent editors and their companies, 
and ISO/IEC have no liability for use of this software module or modifications thereof in an implementation. 
Copyright is not released for non MPEG-4 Video conforming products. 
Microsoft retains full right to use the code for his/her own purpose, 
assign or donate the code to a third party and to inhibit third parties from using the code for non <MPEG standard> conforming products. 
This copyright notice must be included in all copies or derivative works. 

Copyright (c) 1996, 1997.


Module Name:

	motEst.cpp

Abstract:

	Motion estimation routines.

Revision History:
	Dec 20, 1997:	Interlaced tools added by NextLevel Systems
	Feb.16, 1999:     add Quarter Sample 
                            Mathias Wien (wien@ient.rwth-aachen.de) 
	Feb.24, 1999:	GMC added by Yoshinori Suzuki (Hitachi, Ltd.)
	May 9, 1999:	tm5 rate control by DemoGraFX, duhoff@mediaone.net (added by mwi)
	August 9, 1999:   Fast Motion Estimation added by
					Prabhudev Irappa Hosur (pn188359@ntu.edu.sg), NTU, S'pore
	Aug.24, 1999:   NEWPRED added by Hideaki Kimata (NTT) 
	Aug.31, 1999:   QP pred. for GMC/LMC decision in rate control cases
			added by Yoshinori Suzuki (Hitachi, Ltd.)
	Sep.06	1999 : RRV added by Eishi Morimatsu (Fujitsu Laboratories Ltd.) 
    March 30 , 2000 : Diamond Search removed by Prabhudev Irappa Hosur (NTU)
*************************************************************************/

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <iostream.h>

#include "typeapi.h"
#include "codehead.h"
#include "global.hpp"
#include "entropy/bitstrm.hpp"
#include "entropy/entropy.hpp"
#include "entropy/huffman.hpp"
#include "mode.hpp"
#include "vopses.hpp"
#include "vopseenc.hpp"

// NEWPRED
#include "newpred.hpp"
// ~NEWPRED

// RRV insertion
#include "rrv.hpp"
// ~RRV

#ifdef __MFC_
#ifdef _DEBUG
#undef THIS_FILE
static char BASED_CODE THIS_FILE[] = __FILE__;
#endif

#define new DEBUG_NEW				   
#endif // __MFC_

#define FAVORZERO		129
#define FAVOR_DIRECT	129
#define FAVOR_INTER		512
#define FAVOR_16x16		129
#define FAVOR_FIELD		 65			// P-VOP favor field over 8x8 due to fewer MVs
// RRV insertion
#define FAVORZERO_RRV		513
#define FAVOR_DIRECT_RRV	513
#define FAVOR_INTER_RRV		2048
#define FAVOR_32x32			513
#define FAVOR_FIELD_RRV		65	
// ~RRV 

inline Int minimum (Int a, Int b, Int c, Int d)
{
	if (a <= b && a <= c && a <= d)
		return a;
	else if (b <= a && b <= c && b <= d)
		return b;
	else if (c <= a && c <= b && c <= d)
		return c;
	else
		return d;
}

Void CVideoObjectEncoder::motionEstPVOP ()
{
	m_iMAD = 0;
	CoordI y = 0; 
	CMBMode* pmbmd = m_rgmbmd;
	CMotionVector* pmv = m_rgmv;
	const PixelC* ppxlcOrigY = m_pvopcOrig->pixelsBoundY ();
	const PixelC* ppxlcRefY = (m_volmd.bOriginalForME ? m_pvopcRefOrig0 : m_pvopcRefQ0)->pixelsY () + m_iStartInRefToCurrRctY;
	Int iMBX, iMBY;

// GMC_V2
        if ((m_uiSprite == 2) && (m_vopmd.vopPredType == SPRITE)) {
                CMBMode* pmbmdRef = m_rgmbmdRef;
                Int iqp_count=0, iqp_total=0;
                for (iMBY = 0; iMBY < m_iNumMBYRef; iMBY++) {
                        for (iMBX = 0; iMBX < m_iNumMBXRef; iMBX++)        {
                                iqp_total += pmbmdRef -> m_stepSize;
                                iqp_count++;
                             pmbmdRef++;
                        }
                }
                m_uiGMCQP = (iqp_total + iqp_count/2)/iqp_count;
		printf("QP for GMC/LMC selection: %d\n",m_uiGMCQP);
        }
// ~GMC_V2

	if (m_iMVFileUsage == 1)
		readPVOPMVs();

// NEWPRED
	const PixelC* RefbufY = (m_volmd.bOriginalForME ? m_pvopcRefOrig0 : m_pvopcRefQ0)->pixelsY ();
	if(m_volmd.bNewpredEnable) {
		g_pNewPredEnc->CopyRefYtoBufY(RefbufY, m_rctRefFrameY);
	}
// ~NEWPRED

// RRV modification
	for(iMBY = 0; iMBY < m_iNumMBY;	iMBY++, y += (MB_SIZE *m_iRRVScale))
	{
//	for (iMBY = 0; iMBY < m_iNumMBY; iMBY++, y += MB_SIZE) {
// ~RRV
		const PixelC* ppxlcOrigMBY = ppxlcOrigY;
		const PixelC* ppxlcRefMBY = ppxlcRefY;
		CoordI x = 0;
// RRV modification
		for(iMBX = 0; iMBX < m_iNumMBX;	iMBX++, x += (MB_SIZE *m_iRRVScale))
		{
//		for (iMBX = 0; iMBX < m_iNumMBX; iMBX++, x += MB_SIZE)	{
// ~RRV

// NEWPRED
			if(m_volmd.bNewpredEnable) {
// RRV modification
				if(g_pNewPredEnc->CheckSlice((iMBX *m_iRRVScale), (iMBY *m_iRRVScale))){
//				if(g_pNewPredEnc->CheckSlice(iMBX, iMBY)){
// ~RRV

// RRV modification
					PixelC* RefpointY = (PixelC*) m_pvopcRefQ0->pixelsY () + m_iStartInRefToCurrRctY + iMBY * (MB_SIZE *m_iRRVScale) * m_rctRefFrameY.width;
					g_pNewPredEnc->ChangeRefOfSliceYUV((const PixelC* )RefpointY, RefbufY, (iMBX *m_iRRVScale),(iMBY *m_iRRVScale),m_rctRefFrameY,'Y');   
//					PixelC* RefpointY = (PixelC*) m_pvopcRefQ0->pixelsY () + m_iStartInRefToCurrRctY + iMBY * MB_SIZE * m_rctRefFrameY.width;
//					g_pNewPredEnc->ChangeRefOfSliceYUV((const PixelC* )RefpointY, RefbufY, iMBX,iMBY,m_rctRefFrameY,'Y');   
// ~RRV
					m_rctRefVOPZoom0 = m_rctRefVOPY0.upSampleBy2 ();
					biInterpolateY (m_pvopcRefQ0, m_rctRefVOPY0, m_puciRefQZoom0, m_rctRefVOPZoom0, m_vopmd.iRoundingControl);
				}
			}		
// ~NEWPRED

#ifdef __TRACE_AND_STATS_
			m_pbitstrmOut->trace (CSite (iMBX, iMBY), "MB_X_Y");
#endif // __TRACE_AND_STATS_
			if (m_volmd.volType == ENHN_LAYER && m_vopmd.iRefSelectCode == 3)	{
				motionEstMB_PVOP (x, y, pmv, pmbmd);
			}
			else {
				copyToCurrBuffY (ppxlcOrigMBY);
				pmbmd->m_bFieldDCT=0;
				m_iMAD += motionEstMB_PVOP (x, y, pmv, pmbmd, ppxlcRefMBY);
			}

#ifdef __TRACE_AND_STATS_
			m_pbitstrmOut->trace (CSite (iMBX, iMBY), "MB_X_Y");
			m_pbitstrmOut->trace (pmv [0], "MV16");
			m_pbitstrmOut->trace (pmv [1], "MV8");
			m_pbitstrmOut->trace (pmv [2], "MV8");
			m_pbitstrmOut->trace (pmv [3], "MV8");
			m_pbitstrmOut->trace (pmv [4], "MV8");
// INTERLACE
			if(pmbmd->m_bForwardTop)
				m_pbitstrmOut->trace (pmv [6], "MV16x8");
			else
				m_pbitstrmOut->trace (pmv [5], "MV16x8");
			if(pmbmd->m_bForwardBottom)
				m_pbitstrmOut->trace (pmv [8], "MV16x8");
			else
				m_pbitstrmOut->trace (pmv [7], "MV16x8");
// ~INTERLACE
#endif // __TRACE_AND_STATS_
			pmbmd++;
			pmv += PVOP_MV_PER_REF_PER_MB;
// RRV modification
			ppxlcOrigMBY += (MB_SIZE *m_iRRVScale);
			ppxlcRefMBY += (MB_SIZE *m_iRRVScale);
//			ppxlcOrigMBY += MB_SIZE;
//			ppxlcRefMBY += MB_SIZE;
// ~RRV
		}
		ppxlcOrigY += m_iFrameWidthYxMBSize;
		ppxlcRefY += m_iFrameWidthYxMBSize;
	}
// RRV insertion
	pmv	= m_rgmv;
	if(m_vopmd.RRVmode.iRRVOnOff == 1)
	{
		MotionVectorScalingDown (pmv, m_iNumMBX *m_iNumMBY,
								 PVOP_MV_PER_REF_PER_MB);
	}
// ~RRV

	if (m_iMVFileUsage == 2)
		writePVOPMVs();

// NEWPRED
	if(m_volmd.bNewpredEnable)
		g_pNewPredEnc->CopyBufYtoRefY(RefbufY, m_rctRefFrameY);
// ~NEWPRED

}

Void CVideoObjectEncoder::motionEstPVOP_WithShape ()
{
	m_iMAD = 0;
	CoordI y = m_rctCurrVOPY.top; 
	CMBMode* pmbmd = m_rgmbmd;
	CMotionVector* pmv = m_rgmv;
	const PixelC* ppxlcOrigY = m_pvopcOrig->pixelsBoundY ();
	const PixelC* ppxlcOrigBY = m_pvopcOrig->pixelsBoundBY ();
	const PixelC* ppxlcRefY = (m_volmd.bOriginalForME ? m_pvopcRefOrig0 : m_pvopcRefQ0)->pixelsY () + m_iStartInRefToCurrRctY;
	Int iMBX, iMBY;

// GMC_V2
        if ((m_uiSprite == 2) && (m_vopmd.vopPredType == SPRITE)) {
                CMBMode* pmbmdRef = m_rgmbmdRef;
                Int iqp_count=0, iqp_total=0;
                for (iMBY = 0; iMBY < m_iNumMBYRef; iMBY++) {
                        for (iMBX = 0; iMBX < m_iNumMBXRef; iMBX++)     {
                          if (pmbmdRef->m_rgTranspStatus [0] != ALL){
                                iqp_total += pmbmdRef -> m_stepSize;
                                iqp_count++;
                          }
                             pmbmdRef++;
                        }
                }
                m_uiGMCQP = (iqp_total + iqp_count/2)/iqp_count;
		printf("QP for GMC/LMC selection: %d\n",m_uiGMCQP);
        }
// ~GMC_V2

	if (m_iMVFileUsage == 1)
		readPVOPMVs();

	for (iMBY = 0; iMBY < m_iNumMBY; iMBY++, y += MB_SIZE) {
		const PixelC* ppxlcOrigMBY = ppxlcOrigY;
		const PixelC* ppxlcOrigMBBY = ppxlcOrigBY;
		const PixelC* ppxlcRefMBY = ppxlcRefY;
		CoordI x = m_rctCurrVOPY.left;
		for (iMBX = 0; iMBX < m_iNumMBX; iMBX++, x += MB_SIZE)	{
#ifdef __TRACE_AND_STATS_
			if(m_volmd.bShapeOnly==FALSE)
				m_pbitstrmOut->trace (CSite (iMBX, iMBY), "MB_X_Y");
#endif // __TRACE_AND_STATS_
			copyToCurrBuffWithShapeY (ppxlcOrigMBY, ppxlcOrigMBBY);
			decideTransparencyStatus (pmbmd, m_ppxlcCurrMBBY);
			if(m_volmd.bShapeOnly==FALSE)
			{
//OBSS_SAIT_991015
				if (m_volmd.volType == ENHN_LAYER && m_vopmd.iRefSelectCode == 3)
					motionEstMB_PVOP (x, y, pmv, pmbmd);
				else {
					if (pmbmd->m_rgTranspStatus [0] == NONE)
					{
	// new changes X. Chen
						pmbmd->m_bFieldDCT=0;
						m_iMAD += motionEstMB_PVOP (x, y, pmv, pmbmd, ppxlcRefMBY);
					}
					else if (pmbmd->m_rgTranspStatus [0] == PARTIAL)
					{
	// new changes X. Chen
						pmbmd->m_bFieldDCT=0;
						m_iMAD += motionEstMB_PVOP_WithShape (x, y, pmv, pmbmd, ppxlcRefMBY);
					}
				}
//~OBSS_SAIT_991015

#ifdef __TRACE_AND_STATS_
				m_pbitstrmOut->trace (CSite (iMBX, iMBY), "MB_X_Y");
				m_pbitstrmOut->trace (pmv [0], "MV16");
				m_pbitstrmOut->trace (pmv [1], "MV8");
				m_pbitstrmOut->trace (pmv [2], "MV8");
				m_pbitstrmOut->trace (pmv [3], "MV8");
				m_pbitstrmOut->trace (pmv [4], "MV8");
// INTERLACE
	// new changes
				if(pmbmd->m_bFieldDCT) {
					if(pmbmd->m_bForwardTop)
						m_pbitstrmOut->trace (pmv [6], "MV16x8");
					else
						m_pbitstrmOut->trace (pmv [5], "MV16x8");
					if(pmbmd->m_bForwardBottom)
						m_pbitstrmOut->trace (pmv [8], "MV16x8");
					else
						m_pbitstrmOut->trace (pmv [7], "MV16x8");
				}
	// end of new changes
// ~INTERLACE
#endif // __TRACE_AND_STATS_
			}
			pmbmd++;
			pmv += PVOP_MV_PER_REF_PER_MB;
			ppxlcOrigMBY += MB_SIZE;
			ppxlcOrigMBBY += MB_SIZE;
			ppxlcRefMBY += MB_SIZE;
		}
		ppxlcOrigY += m_iFrameWidthYxMBSize;
		ppxlcOrigBY += m_iFrameWidthYxMBSize;
		ppxlcRefY += m_iFrameWidthYxMBSize;
	}
	if (m_iMVFileUsage == 2)
		writePVOPMVs();
}

Void CVideoObjectEncoder::motionEstBVOP ()
{
	if (m_iMVFileUsage == 1)
		readBVOPMVs();
	m_iMAD = 0;
	CoordI y = 0; // frame-based, always start from 0
	CMBMode* pmbmd = m_rgmbmd;
	const CMBMode* pmbmdRef = NULL;
	const CMotionVector* pmvRef = NULL;		//mv in ref frame (for direct mode)

	if(m_bCodedFutureRef!=FALSE)
	{	
		pmbmdRef = m_rgmbmdRef;