--- motion_est.c	2002/12/13 11:56:31	1.44.2.34
+++ motion_est.c	2002/12/26 12:10:11	1.44.2.38
@@ -31,6 +31,7 @@
 #include <assert.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <string.h>	// memcpy
 
 #include "../encoder.h"
 #include "../utils/mbfunctions.h"
@@ -52,11 +53,13 @@
 (*CheckCandidate)((const int)(X),(const int)(Y), (D), &iDirection, data ); }
 
 static __inline int
-d_mv_bits(int x, int y, const uint32_t iFcode, const int qpel, const int rrv)
+d_mv_bits(int x, int y, const VECTOR pred, const uint32_t iFcode, const int qpel, const int rrv)
 {
 	int xb, yb;
 	if (qpel) { x *= 2; y *= 2;}
 	else if (rrv) { x = RRV_MV_SCALEDOWN(x); y = RRV_MV_SCALEDOWN(y); }
+	x = pred.x - x;
+	y = pred.y - y;
 	
 	if (x == 0) xb = 1;
 	else {
@@ -145,12 +148,11 @@
 	const int halfpel_y = y/2;
 	const uint8_t *ref1, *ref2, *ref3, *ref4;
 
-	ref1 = GetReference(halfpel_x, halfpel_y, dir, data); // this reference is used in all cases
+	ref1 = GetReference(halfpel_x, halfpel_y, dir, data);
 	ref1 += 8 * (block&1) + 8 * (block>>1) * iEdgedWidth;
 	switch( ((x&1)<<1) + (y&1) ) {
 	case 0: // pure halfpel position
-		Reference = (uint8_t *) GetReference(halfpel_x, halfpel_y, dir, data);
-		Reference += 8 * (block&1) + 8 * (block>>1) * iEdgedWidth;
+		return (uint8_t *) ref1;
 		break;
 
 	case 1: // x halfpel, y qpel - top or bottom during qpel refinement
@@ -190,10 +192,10 @@
 	const int halfpel_y = y/2;
 	const uint8_t *ref1, *ref2, *ref3, *ref4;
 
-	ref1 = GetReference(halfpel_x, halfpel_y, dir, data); // this reference is used in all cases
+	ref1 = GetReference(halfpel_x, halfpel_y, dir, data);
 	switch( ((x&1)<<1) + (y&1) ) {
 	case 0: // pure halfpel position
-		return (uint8_t *) GetReference(halfpel_x, halfpel_y, dir, data);
+		return (uint8_t *) ref1;
 	case 1: // x halfpel, y qpel - top or bottom during qpel refinement
 		ref2 = GetReference(halfpel_x, y - halfpel_y, dir, data);
 		interpolate8x8_avg2(Reference, ref1, ref2, iEdgedWidth, rounding, 8);
@@ -245,7 +247,7 @@
 		current = data->currentMV;
 		xc = x; yc = y;
 	}
-	t = d_mv_bits(x - data->predMV.x, y - data->predMV.y, data->iFcode, data->qpel && !data->qpel_precision, 0);
+	t = d_mv_bits(x, y, data->predMV, data->iFcode, data->qpel && !data->qpel_precision, 0);
 	
 	data->temp[0] = sad16v(data->Cur, Reference, data->iEdgedWidth, data->temp + 1);
 
@@ -281,7 +283,7 @@
 		|| ( y > data->max_dy) || (y < data->min_dy)) return;
 
 	Reference = GetReference(x, y, 0, data);
-	t = d_mv_bits(x - data->predMV.x, y - data->predMV.y, data->iFcode, 0, 1);
+	t = d_mv_bits(x, y, data->predMV, data->iFcode, 0, 1);
 
 	data->temp[0] = sad32v_c(data->Cur, Reference, data->iEdgedWidth, data->temp + 1);
 
@@ -324,7 +326,7 @@
 		Reference = GetReference(x, y, 0, data);
 		current = data->currentMV;
 	}
-	t = d_mv_bits(x - data->predMV.x, y - data->predMV.y, data->iFcode,
+	t = d_mv_bits(x, y, data->predMV, data->iFcode,
 					data->qpel && !data->qpel_precision && !data->rrv, data->rrv);
 	
 	sad = sad16(data->Cur, Reference, data->iEdgedWidth, 256*4096);
@@ -337,21 +339,28 @@
 }
 
 static void 
-CheckCandidate16no4vI(const int x, const int y, const int Direction, int * const dir, const SearchData * const data)
+CheckCandidate32I(const int x, const int y, const int Direction, int * const dir, const SearchData * const data)
 {
 // maximum speed - for P/B/I decision
-	int32_t sad;
 
 	if (( x > data->max_dx) || ( x < data->min_dx)
 		|| ( y > data->max_dy) || (y < data->min_dy)) return;
 
-	sad = sad16(data->Cur, data->Ref + x/2 + (y/2)*(data->iEdgedWidth),
-					data->iEdgedWidth, 256*4096);
-
-	if (sad < *(data->iMinSAD)) {
-		*(data->iMinSAD) = sad;
+	data->temp[0] = sad32v_c(data->Cur, data->Ref + x/2 + (y/2)*(data->iEdgedWidth),
+							data->iEdgedWidth, data->temp+1);
+	if (data->temp[0] < *(data->iMinSAD)) {
+		*(data->iMinSAD) = data->temp[0];
 		data->currentMV[0].x = x; data->currentMV[0].y = y;
 		*dir = Direction; }
+	if (data->temp[1] < data->iMinSAD[1]) {
+		data->iMinSAD[1] = data->temp[1]; data->currentMV[1].x = x; data->currentMV[1].y = y; }
+	if (data->temp[2] < data->iMinSAD[2]) {
+		data->iMinSAD[2] = data->temp[2]; data->currentMV[2].x = x; data->currentMV[2].y = y; }
+	if (data->temp[3] < data->iMinSAD[3]) {
+		data->iMinSAD[3] = data->temp[3]; data->currentMV[3].x = x; data->currentMV[3].y = y; }
+	if (data->temp[4] < data->iMinSAD[4]) {
+		data->iMinSAD[4] = data->temp[4]; data->currentMV[4].x = x; data->currentMV[4].y = y; }
+
 }
 
 
@@ -378,8 +387,8 @@
 		current = data->currentMV;
 	}
 
-	t = d_mv_bits(xf - data->predMV.x, yf - data->predMV.y, data->iFcode, data->qpel && !data->qpel_precision, 0)
-		 + d_mv_bits(xb - data->bpredMV.x, yb - data->bpredMV.y, data->iFcode, data->qpel && !data->qpel_precision, 0);
+	t = d_mv_bits(xf, yf, data->predMV, data->iFcode, data->qpel && !data->qpel_precision, 0)
+		 + d_mv_bits(xb, yb, data->bpredMV, data->iFcode, data->qpel && !data->qpel_precision, 0);
 	
 	sad = sad16bi(data->Cur, ReferenceF, ReferenceB, data->iEdgedWidth);
 	sad += (data->lambda16 * t * sad)/1000;
@@ -398,6 +407,7 @@
 	const uint8_t *ReferenceF;
 	const uint8_t *ReferenceB;
 	VECTOR mvs, b_mvs;
+	const VECTOR zeroMV={0,0};
 
 	if (( x > 31) || ( x < -32) || ( y > 31) || (y < -32)) return;
 
@@ -430,7 +440,7 @@
 		if (sad > *(data->iMinSAD)) return;
 	}
 
-	sad += (data->lambda16 * d_mv_bits(x, y, 1, 0, 0) * sad)/1000;
+	sad += (data->lambda16 * d_mv_bits(x, y, zeroMV, 1, 0, 0) * sad)/1000;
 
 	if (sad < *(data->iMinSAD)) {
 		*(data->iMinSAD) = sad;
@@ -445,6 +455,7 @@
 	const uint8_t *ReferenceF;
 	const uint8_t *ReferenceB;
 	VECTOR mvs, b_mvs;
+	const VECTOR zeroMV = {0,0};
 
 	if (( x > 31) || ( x < -32) || ( y > 31) || (y < -32)) return;
 
@@ -471,7 +482,7 @@
 	ReferenceB = Interpolate16x16qpel(b_mvs.x, b_mvs.y, 1, data);
 
 	sad = sad16bi(data->Cur, ReferenceF, ReferenceB, data->iEdgedWidth);
-	sad += (data->lambda16 * d_mv_bits(x, y, 1, 0, 0) * sad)/1000;
+	sad += (data->lambda16 * d_mv_bits(x, y, zeroMV, 1, 0, 0) * sad)/1000;
 
 	if (sad < *(data->iMinSAD)) {
 		*(data->iMinSAD) = sad;
@@ -492,7 +503,7 @@
 	else Reference =  GetReference(x, y, 0, data);
 
 	sad = sad8(data->Cur, Reference, data->iEdgedWidth);
-	t = d_mv_bits(x - data->predMV.x, y - data->predMV.y, data->iFcode, data->qpel && !data->qpel_precision, 0);
+	t = d_mv_bits(x, y, data->predMV, data->iFcode, data->qpel && !data->qpel_precision, 0);
 
 	sad += (data->lambda8 * t * (sad+NEIGH_8X8_BIAS))/100;
 
@@ -996,7 +1007,7 @@
 	if (pParam->m_quarterpel) Data->predMV = get_qpmv2(pMBs, pParam->mb_width, 0, x, y, 0);
 	else Data->predMV = pmv[0];
 
-	i = d_mv_bits(Data->predMV.x, Data->predMV.y, Data->iFcode, 0, 0);
+	i = d_mv_bits(0, 0, Data->predMV, Data->iFcode, 0, 0);
 	Data->iMinSAD[0] = pMB->sad16 + (Data->lambda16 * i * pMB->sad16)/1000;
 	Data->iMinSAD[1] = pMB->sad8[0] + (Data->lambda8 * i * (pMB->sad8[0]+NEIGH_8X8_BIAS))/100;
 	Data->iMinSAD[2] = pMB->sad8[1];
@@ -1126,6 +1137,7 @@
 			Data->currentMV[0].x = RRV_MV_SCALEDOWN(Data->currentMV[0].x);
 			Data->currentMV[0].y = RRV_MV_SCALEDOWN(Data->currentMV[0].y);
 	}
+
 	if (!(inter4v) ||
 		(Data->iMinSAD[0] < Data->iMinSAD[1] + Data->iMinSAD[2] + 
 			Data->iMinSAD[3] + Data->iMinSAD[4] + IMV16X16 * (int32_t)iQuant )) {
@@ -1171,14 +1183,14 @@
 
 	if(pParam->m_quarterpel) {
 		Data->predMV = get_qpmv2(pMBs, pParam->mb_width, 0, x/2, y/2, block);
-		if (block != 0)	i = d_mv_bits(	Data->currentQMV->x - Data->predMV.x, 
-										Data->currentQMV->y - Data->predMV.y, Data->iFcode, 0, 0);
+		if (block != 0)	i = d_mv_bits(	Data->currentQMV->x, Data->currentQMV->y,
+										Data->predMV, Data->iFcode, 0, 0);
 
 	} else {
 		Data->predMV = get_pmv2(pMBs, pParam->mb_width, 0, x/2, y/2, block);
 		if (block != 0) {
-			if (block != 0)	i = d_mv_bits(	Data->currentMV->x - Data->predMV.x, 
-											Data->currentMV->y - Data->predMV.y, Data->iFcode, 0, Data->rrv);
+			if (block != 0)	i = d_mv_bits(	Data->currentMV->x, Data->currentMV->y,
+											Data->predMV, Data->iFcode, 0, Data->rrv);
 		}
 	}
 
@@ -1326,7 +1338,7 @@
 
 	const int32_t iEdgedWidth = pParam->edged_width;
 	
-	int i, iDirection, mask;
+	int i, iDirection = 255, mask;
 	VECTOR pmv[7];
 	MainSearchFunc *MainSearchPtr;
 	*Data->iMinSAD = MV_MAX_ERROR;
@@ -1351,7 +1363,7 @@
 	CheckCandidate = CheckCandidate16no4v;
 
 // main loop. checking all predictions
-	for (i = 0; i < 8; i++) {
+	for (i = 0; i < 7; i++) {
 		if (!(mask = make_mask(pmv, i)) ) continue;
 		CheckCandidate16no4v(pmv[i].x, pmv[i].y, mask, &iDirection, Data);
 	}
@@ -1362,11 +1374,11 @@
 		MainSearchPtr = AdvDiamondSearch;
 		else MainSearchPtr = DiamondSearch;
 
-	(*MainSearchPtr)(Data->currentMV->x, Data->currentMV->y, Data, 255);
+	(*MainSearchPtr)(Data->currentMV->x, Data->currentMV->y, Data, iDirection);
 
 	SubpelRefine(Data);
 	
-	if (Data->qpel) {
+	if (Data->qpel && *Data->iMinSAD < *best_sad + 300) {
 		Data->currentQMV->x = 2*Data->currentMV->x;
 		Data->currentQMV->y = 2*Data->currentMV->y;
 		Data->qpel_precision = 1;
@@ -1376,7 +1388,7 @@
 	}
 
 // three bits are needed to code backward mode. four for forward
-// we treat the bits just like they were vector's
+
 	if (mode_current == MODE_FORWARD) *Data->iMinSAD +=  4 * Data->lambda16;
 	else *Data->iMinSAD +=  3 * Data->lambda16;
 
@@ -1394,13 +1406,12 @@
 			pMB->pmvs[0].x = Data->currentMV->x - predMV->x;
 			pMB->pmvs[0].y = Data->currentMV->y - predMV->y;
 		}
-		if (mode_current == MODE_FORWARD) 
-			pMB->mvs[0] = *(Data->currentMV+2) = *Data->currentMV;
-		else 
-			pMB->b_mvs[0] = *(Data->currentMV+1) = *Data->currentMV; //we store currmv for interpolate search
-
+		if (mode_current == MODE_FORWARD) pMB->mvs[0] = *Data->currentMV;
+		else pMB->b_mvs[0] = *Data->currentMV;
 	}
-	
+	if (mode_current == MODE_FORWARD)  *(Data->currentMV+2) = *Data->currentMV;
+	else *(Data->currentMV+1) = *Data->currentMV; //we store currmv for interpolate search
+
 }
 
 static void 
@@ -1534,7 +1545,6 @@
 		}
 	}
 
-	
 	if (b_mb->mode == MODE_INTER4V) CheckCandidate = CheckCandidateDirect;
 	else CheckCandidate = CheckCandidateDirectno4v;
 		
@@ -1561,8 +1571,7 @@
 
 	*best_sad = *Data->iMinSAD;
 
-	if (b_mb->mode == MODE_INTER4V) 
-		pMB->mode = MODE_DIRECT;
+	if (b_mb->mode == MODE_INTER4V) pMB->mode = MODE_DIRECT;
 	else pMB->mode = MODE_DIRECT_NO4V; //for faster compensation
 
 	pMB->pmvs[3] = *Data->currentMV;
@@ -1624,7 +1633,7 @@
 	fData->qpel_precision = 0;
 	memcpy(&bData, fData, sizeof(SearchData)); //quick copy of common data
 	*fData->iMinSAD = 4096*256;
-	bData.currentMV ++; bData.currentQMV ++;
+	bData.currentMV++; bData.currentQMV++;
 	fData->iFcode = bData.bFcode = fcode; fData->bFcode = bData.iFcode = bcode;
 
 	i = (x + y * fData->iEdgedWidth) * 16;
@@ -1679,6 +1688,7 @@
 	} while (!(iDirection));
 
 	if (fData->qpel) {
+		if (*fData->iMinSAD > *best_sad + 500) return;
 		CheckCandidate = CheckCandidateInt;
 		fData->qpel_precision = bData.qpel_precision = 1;
 		get_range(&fData->min_dx, &fData->max_dx, &fData->min_dy, &fData->max_dy, x, y, 16, pParam->width, pParam->height, fcode, 1, 0);
@@ -1688,11 +1698,12 @@
 		fData->currentQMV[1].x = 2 * fData->currentMV[1].x;
 		fData->currentQMV[1].y = 2 * fData->currentMV[1].y;
 		SubpelRefine(fData);
+		if (*fData->iMinSAD > *best_sad + 300) return;
 		fData->currentQMV[2] = fData->currentQMV[0];
 		SubpelRefine(&bData);
 	}
 	
-	*fData->iMinSAD +=  (2+2) * fData->lambda16; // two bits are needed to code interpolate mode.
+	*fData->iMinSAD +=  (2+3) * fData->lambda16; // two bits are needed to code interpolate mode.
 
 	if (*fData->iMinSAD < *best_sad) {
 		*best_sad = *fData->iMinSAD;
@@ -1800,7 +1811,7 @@
 									&Data);
 
 			if (pMB->mode == MODE_DIRECT_NONE_MV) { n_count++; continue; }
-			
+
 			// forward search
 			SearchBF(f_ref->y, f_refH->y, f_refV->y, f_refHV->y,
 						&frame->image, i, j,
@@ -1818,7 +1829,6 @@
 						MODE_BACKWARD, &Data);
 
 			// interpolate search comes last, because it uses data from forward and backward as prediction
-
 			SearchInterpolate(f_ref->y, f_refH->y, f_refV->y, f_refHV->y,
 						b_ref->y, b_refH->y, b_refV->y, b_refHV->y,
 						&frame->image,
@@ -1867,66 +1877,67 @@
 	free(qimage);
 }
 
-static __inline int
+static __inline void
 MEanalyzeMB (	const uint8_t * const pRef,
 				const uint8_t * const pCur,
 				const int x,
 				const int y,
 				const MBParam * const pParam,
-				const MACROBLOCK * const pMBs,
-				MACROBLOCK * const pMB,
+				MACROBLOCK * const pMBs,
 				SearchData * const Data)
 {
 
-	int i = 255, mask;
+	int i, mask;
 	VECTOR pmv[3];
-	*(Data->iMinSAD) = MV_MAX_ERROR;
+	MACROBLOCK * pMB = &pMBs[x + y * pParam->mb_width];
+	
+	for (i = 0; i < 5; i++) Data->iMinSAD[i] = MV_MAX_ERROR;
 
 	//median is only used as prediction. it doesn't have to be real
 	if (x == 1 && y == 1) Data->predMV.x = Data->predMV.y = 0;
 	else
 		if (x == 1) //left macroblock does not have any vector now
 			Data->predMV = (pMB - pParam->mb_width)->mvs[0]; // top instead of median
-		else if (y == 1) // top macroblock don't have it's vector 
+		else if (y == 1) // top macroblock doesn't have it's vector 
 			Data->predMV = (pMB - 1)->mvs[0]; // left instead of median
 			else Data->predMV = get_pmv2(pMBs, pParam->mb_width, 0, x, y, 0); //else median
 
 	get_range(&Data->min_dx, &Data->max_dx, &Data->min_dy, &Data->max_dy, x, y, 16,
-				pParam->width, pParam->height, Data->iFcode - pParam->m_quarterpel, 0, 0);
+				pParam->width, pParam->height, Data->iFcode - pParam->m_quarterpel, 0, Data->rrv);
 
 	Data->Cur = pCur + (x + y * pParam->edged_width) * 16;
 	Data->Ref = pRef + (x + y * pParam->edged_width) * 16;
-	
+
 	pmv[1].x = EVEN(pMB->mvs[0].x);
 	pmv[1].y = EVEN(pMB->mvs[0].y);
 	pmv[2].x = EVEN(Data->predMV.x);
 	pmv[2].y = EVEN(Data->predMV.y);
 	pmv[0].x = pmv[0].y = 0;
 
-	CheckCandidate16no4vI(0, 0, 255, &i, Data);
+	CheckCandidate32I(0, 0, 255, &i, Data);
 
-//early skip for 0,0
-	if (*Data->iMinSAD < MAX_SAD00_FOR_SKIP * 4) {
-		pMB->mvs[0] = pMB->mvs[1] = pMB->mvs[2] = pMB->mvs[3] = Data->currentMV[0];
-		pMB->mode = MODE_NOT_CODED;
-		return 0;
-	}
+	if (*Data->iMinSAD > 4 * MAX_SAD00_FOR_SKIP * 4) {
 
-	if (!(mask = make_mask(pmv, 1)))
-		CheckCandidate16no4vI(pmv[1].x, pmv[1].y, mask, &i, Data);
-	if (!(mask = make_mask(pmv, 2)))
-		CheckCandidate16no4vI(pmv[2].x, pmv[2].y, mask, &i, Data);
+		if (!(mask = make_mask(pmv, 1)))
+			CheckCandidate32I(pmv[1].x, pmv[1].y, mask, &i, Data);
+		if (!(mask = make_mask(pmv, 2)))
+			CheckCandidate32I(pmv[2].x, pmv[2].y, mask, &i, Data);
 
-	if (*Data->iMinSAD > MAX_SAD00_FOR_SKIP * 6) // diamond only if needed
-		DiamondSearch(Data->currentMV->x, Data->currentMV->y, Data, i);
+		if (*Data->iMinSAD > 4 * MAX_SAD00_FOR_SKIP * 4) // diamond only if needed
+			DiamondSearch(Data->currentMV->x, Data->currentMV->y, Data, i);
 
-	pMB->mvs[0] = pMB->mvs[1] = pMB->mvs[2] = pMB->mvs[3] = Data->currentMV[0];
-	pMB->mode = MODE_INTER;
-	return *(Data->iMinSAD);
+		for (i = 0; i < 4; i++) {
+			MACROBLOCK * MB = &pMBs[x + (i&1) + (y+(i>>1) * pParam->mb_width)];
+			MB->mvs[0] = MB->mvs[1] = MB->mvs[2] = MB->mvs[3] = Data->currentMV[i];
+			MB->mode = MODE_INTER;
+			MB->sad16 = Data->iMinSAD[i+1];
+		}
+	}
 }
 
-#define INTRA_THRESH	1350
-#define INTER_THRESH	1200
+#define INTRA_BIAS		2500
+#define INTRA_THRESH	1500
+#define INTER_THRESH	1400
 
 
 int
@@ -1942,15 +1953,18 @@
 	MACROBLOCK * const pMBs = Current->mbs;
 	const IMAGE * const pCurrent = &Current->image;
 	int IntraThresh = INTRA_THRESH, InterThresh = INTER_THRESH;
+	const VECTOR zeroMV = {0,0};
 
-	VECTOR currentMV;
-	int32_t iMinSAD;
+	int32_t iMinSAD[5], temp[5];
+	VECTOR currentMV[5];
 	SearchData Data;
 	Data.iEdgedWidth = pParam->edged_width;
-	Data.currentMV = &currentMV;
-	Data.iMinSAD = &iMinSAD;
+	Data.currentMV = currentMV;
+	Data.iMinSAD = iMinSAD;
 	Data.iFcode = Current->fcode;
-	CheckCandidate = CheckCandidate16no4vI;
+	Data.rrv = Current->global_flags & XVID_REDUCED;
+	Data.temp = temp;
+	CheckCandidate = CheckCandidate32I;
 
 	if (intraCount < 10) // we're right after an I frame
 		IntraThresh += 4 * (intraCount - 10) * (intraCount - 10);
@@ -1958,32 +1972,36 @@
 		if ( 5*(maxIntra - intraCount) < maxIntra) // we're close to maximum. 2 sec when max is 10 sec
 			IntraThresh -= (IntraThresh * (maxIntra - 5*(maxIntra - intraCount)))/maxIntra;
 
-
 	InterThresh += 400 * (1 - bCount);
-	if (InterThresh < 200) InterThresh = 200;
+	if (InterThresh < 300) InterThresh = 300;
 
 	if (sadInit) (*sadInit) ();
 
-	for (y = 1; y < pParam->mb_height-1; y++) {
-		for (x = 1; x < pParam->mb_width-1; x++) {
-			int sad, dev;
-			MACROBLOCK *pMB = &pMBs[x + y * pParam->mb_width];
+	for (y = 1; y < pParam->mb_height-1; y+=2) {
+		for (x = 1; x < pParam->mb_width-1; x+=2) {
+			int i;
+
+			if (bCount == 0) pMBs[x + y * pParam->mb_width].mvs[0] = zeroMV;
 
-			sad = MEanalyzeMB(pRef->y, pCurrent->y, x, y,
-								pParam, pMBs, pMB, &Data);
+			MEanalyzeMB(pRef->y, pCurrent->y, x, y, pParam, pMBs, &Data);
 			
-			if (sad > IntraThresh) {
-				dev = dev16(pCurrent->y + (x + y * pParam->edged_width) * 16,
-							  pParam->edged_width);
-				if (dev + IntraThresh < sad) {
-					pMB->mode = MODE_INTRA;
-					if (++intra > (pParam->mb_height-2)*(pParam->mb_width-2)/2) return I_VOP;
+			for (i = 0; i < 4; i++) {
+				int dev;
+				MACROBLOCK *pMB = &pMBs[x+(i&1) + y+(i>>1) * pParam->mb_width];
+				if (pMB->sad16 > IntraThresh) {
+					dev = dev16(pCurrent->y + (x + (i&1) + (y + (i>>1))* pParam->edged_width) * 16,
+									  pParam->edged_width);
+					if (dev + IntraThresh < pMB->sad16) {
+						pMB->mode = MODE_INTRA;
+						if (++intra > (pParam->mb_height-2)*(pParam->mb_width-2)/2) return I_VOP;
+					}
 				}
+				sSAD += pMB->sad16;
 			}
-			sSAD += sad;
 		}
 	}
 	sSAD /= (pParam->mb_height-2)*(pParam->mb_width-2);
+	if (sSAD > IntraThresh + INTRA_BIAS ) return I_VOP;
 	if (sSAD > InterThresh ) return P_VOP;
 	emms();
 	return B_VOP;
@@ -2015,7 +2033,7 @@
 }
 
 
-static __inline VECTOR
+static VECTOR
 GlobalMotionEst(const MACROBLOCK * const pMBs, const MBParam * const pParam, const uint32_t iFcode)
 {