/*
*****************************************************************************
* COPYRIGHT AND WARRANTY INFORMATION
*
* Copyright 2003, Advanced Audio Video Coding Standard, Part II
*
* DISCLAIMER OF WARRANTY
*
* The contents of this file are subject to the Mozilla Public License
* Version 1.1 (the "License"); you may not use this file except in
* compliance with the License. You may obtain a copy of the License at
* http://www.mozilla.org/MPL/
*
* Software distributed under the License is distributed on an "AS IS"
* basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
* License for the specific language governing rights and limitations under
* the License.
*                     
* THIS IS NOT A GRANT OF PATENT RIGHTS - SEE THE AVS PATENT POLICY.
* The AVS Working Group doesn't represent or warrant that the programs
* furnished here under are free of infringement of any third-party patents.
* Commercial implementations of AVS, including shareware, may be
* subject to royalty fees to patent holders. Information regarding
* the AVS patent policy for standardization procedure is available at 
* AVS Web site http://www.avs.org.cn. Patent Licensing is outside
* of AVS Working Group.
*
* The Original Code is Reference Software for China National Standard 
* GB/T 20090.2-2006 (short for AVS-P2 or AVS Video) at version RM52J.
*
* The Initial Developer of the Original Code is Video subgroup of AVS
* Workinggroup (Audio and Video coding Standard Working Group of China).
* Contributors:   Guoping Li,    Siwei Ma,    Jian Lou,    Qiang Wang , 
*   Jianwen Chen,Haiwu Zhao,  Xiaozhen Zheng, Junhao Zheng, Zhiming Wang
* 
******************************************************************************
*/



/*
*************************************************************************************
* File name: 
* Function: 
	 Fast integer pel motion estimation and fractional pel motion estimation
	 algorithms are described in this file.
	  1. get_mem_FME() and free_mem_FME() are functions for allocation and release
	     of memories about motion estimation
	  2. FME_BlockMotionSearch() is the function for fast integer pel motion 
	     estimation and fractional pel motion estimation
	  3. DefineThreshold() defined thresholds for early termination
*
*************************************************************************************
*/

#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <memory.h>
#include <assert.h>

#include "memalloc.h"
#include "fast_me.h"
#include "refbuf.h"

#ifdef TimerCal
#include <sys/timeb.h>
#include <time.h>
#include <windows.h>
#endif

#define Q_BITS          15

extern  int*   byte_abs;
extern  int*   mvbits;
extern  int*   spiral_search_x;
extern  int*   spiral_search_y;

static pel_t (*PelY_14) (pel_t**, int, int);
static const int quant_coef[6][4][4] = {
  {{13107, 8066,13107, 8066},{ 8066, 5243, 8066, 5243},{13107, 8066,13107, 8066},{ 8066, 5243, 8066, 5243}},
  {{11916, 7490,11916, 7490},{ 7490, 4660, 7490, 4660},{11916, 7490,11916, 7490},{ 7490, 4660, 7490, 4660}},
  {{10082, 6554,10082, 6554},{ 6554, 4194, 6554, 4194},{10082, 6554,10082, 6554},{ 6554, 4194, 6554, 4194}},
  {{ 9362, 5825, 9362, 5825},{ 5825, 3647, 5825, 3647},{ 9362, 5825, 9362, 5825},{ 5825, 3647, 5825, 3647}},
  {{ 8192, 5243, 8192, 5243},{ 5243, 3355, 5243, 3355},{ 8192, 5243, 8192, 5243},{ 5243, 3355, 5243, 3355}},
  {{ 7282, 4559, 7282, 4559},{ 4559, 2893, 4559, 2893},{ 7282, 4559, 7282, 4559},{ 4559, 2893, 4559, 2893}}
};

void DefineThreshold()
{
  static float ThresholdFac[8] = {0,8,4,4,2.5,1.5,1.5,1}; 
  static int ThreshUp[8] = {0, 1024,512,512,448,384,384,384};

  AlphaSec[1] = 0.01f;
  AlphaSec[2] = 0.01f;
  AlphaSec[3] = 0.01f;
  AlphaSec[4] = 0.02f;
  AlphaSec[5] = 0.03f;
  AlphaSec[6] = 0.03f;
  AlphaSec[7] = 0.04f;

  AlphaThird[1] = 0.06f;
  AlphaThird[2] = 0.07f;
  AlphaThird[3] = 0.07f;
  AlphaThird[4] = 0.08f;
  AlphaThird[5] = 0.12f;
  AlphaThird[6] = 0.11f;
  AlphaThird[7] = 0.15f;

  DefineThresholdMB();
  return;
}

void DefineThresholdMB()
{
  int gb_qp_per    = (input->qpN-MIN_QP)/6;
  int gb_qp_rem    = (input->qpN-MIN_QP)%6;
  
  int gb_q_bits    = Q_BITS+gb_qp_per;
  int gb_qp_const,Thresh4x4;

  if (img->type == INTRA_IMG)
    gb_qp_const=(1<<gb_q_bits)/3;    // intra
  else
    gb_qp_const=(1<<gb_q_bits)/6;    // inter
  
  Thresh4x4 =   ((1<<gb_q_bits) - gb_qp_const)/quant_coef[gb_qp_rem][0][0];
  Quantize_step = Thresh4x4/(4*5.61f);
  Bsize[7]=(16*16)*Quantize_step;

  Bsize[6]=Bsize[7]*4;
  Bsize[5]=Bsize[7]*4;
  Bsize[4]=Bsize[5]*4;
  Bsize[3]=Bsize[4]*4;
  Bsize[2]=Bsize[4]*4;
  Bsize[1]=Bsize[2]*4;
}

/*
*************************************************************************
* Function:Dynamic memory allocation of all infomation needed for Fast ME
* Input:
* Output:
* Return: Number of allocated bytes
* Attention:
*************************************************************************
*/

int get_mem_mincost (int****** mv)
{
  int i, j, k, l;

  if(input->InterlaceCodingOption != FRAME_CODING)   
	  img->buf_cycle *= 2;	

  if ((*mv = (int*****)calloc(img->width/4,sizeof(int****))) == NULL)  //add by wuzhongmou 0610
    no_mem_exit ("get_mem_mv: mv");
  for (i=0; i<img->width/4; i++)
  {
    if (((*mv)[i] = (int****)calloc(img->height/4,sizeof(int***))) == NULL)  //add by wuzhongmou 0610
      no_mem_exit ("get_mem_mv: mv");
    for (j=0; j<img->height/4; j++)
    {
      if (((*mv)[i][j] = (int***)calloc(img->buf_cycle,sizeof(int**))) == NULL)
        no_mem_exit ("get_mem_mv: mv");
      for (k=0; k<img->buf_cycle; k++)
      {
        if (((*mv)[i][j][k] = (int**)calloc(9,sizeof(int*))) == NULL)
          no_mem_exit ("get_mem_mv: mv");
        for (l=0; l<9; l++)
          if (((*mv)[i][j][k][l] = (int*)calloc(3,sizeof(int))) == NULL)
            no_mem_exit ("get_mem_mv: mv");
      }
    }
  }
  if(input->InterlaceCodingOption != FRAME_CODING)   
	  img->buf_cycle /= 2;	

  return img->width/4*img->height/4*img->buf_cycle*9*3*sizeof(int); //add by wuzhongmou 0610
} 
/*
*************************************************************************
* Function:Dynamic memory allocation of all infomation needed for backward prediction
* Input:
* Output:
* Return: Number of allocated bytes
* Attention:
*************************************************************************
*/

int get_mem_bwmincost (int****** mv)
{
  int i, j, k, l;

  if(input->InterlaceCodingOption != FRAME_CODING)   img->buf_cycle *= 2;	

  if ((*mv = (int*****)calloc(img->width/4,sizeof(int****))) == NULL)  //add by wuzhongmou 0610
    no_mem_exit ("get_mem_mv: mv");
  for (i=0; i<img->width/4; i++) //add by wuzhongmou 0610
  {
    if (((*mv)[i] = (int****)calloc(img->height/4,sizeof(int***))) == NULL) //add by wuzhongmou 0610
      no_mem_exit ("get_mem_mv: mv");
    for (j=0; j<img->height/4; j++)
    {
      if (((*mv)[i][j] = (int***)calloc(img->buf_cycle,sizeof(int**))) == NULL)
        no_mem_exit ("get_mem_mv: mv");
      for (k=0; k<img->buf_cycle; k++)
      {
        if (((*mv)[i][j][k] = (int**)calloc(9,sizeof(int*))) == NULL)
          no_mem_exit ("get_mem_mv: mv");
        for (l=0; l<9; l++)
          if (((*mv)[i][j][k][l] = (int*)calloc(3,sizeof(int))) == NULL)
            no_mem_exit ("get_mem_mv: mv");
      }
    }
  }
  if(input->InterlaceCodingOption != FRAME_CODING)   img->buf_cycle /= 2;	

  return img->width/4*img->height/4*1*9*3*sizeof(int);  //add by wuzhongmou 0610
}

int get_mem_FME()
{
	int memory_size = 0;
	memory_size += get_mem2Dint(&McostState, 2*input->search_range+1, 2*input->search_range+1);
	memory_size += get_mem_mincost (&(all_mincost));
	memory_size += get_mem_bwmincost(&(all_bwmincost));
	memory_size += get_mem2D(&SearchState,7,7);
	
	return memory_size;
}

/*
*************************************************************************
* Function:free the memory allocated for of all infomation needed for Fast ME
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void free_mem_mincost (int***** mv)
{
  int i, j, k, l;
  if(input->InterlaceCodingOption != FRAME_CODING)   img->buf_cycle *= 2;	

  for (i=0; i<img->width/4; i++)
  {
    for (j=0; j<img->height/4; j++)
    {
      for (k=0; k<img->buf_cycle; k++)
      {
        for (l=0; l<9; l++)
          free (mv[i][j][k][l]);
        free (mv[i][j][k]);
      }
      free (mv[i][j]);
    }
    free (mv[i]);
  }
  free (mv);
  if(input->InterlaceCodingOption != FRAME_CODING)   img->buf_cycle /= 2;	
}

/*
*************************************************************************
* Function:free the memory allocated for of all infomation needed for backward prediction
* Input:
* Output:
* Return: 
* Attention:
*************************************************************************
*/

void free_mem_bwmincost (int***** mv)
{
  int i, j, k, l;

  for (i=0; i<img->width/4; i++)  //add by wuzhongmou 0610
  {
    for (j=0; j<img->height/4; j++)  //add by wuzhongmou 0610
    {
      for (k=0; k<1; k++)
      {
        for (l=0; l<9; l++)
          free (mv[i][j][k][l]);
        free (mv[i][j][k]);
      }
      free (mv[i][j]);
    }
    free (mv[i]);
  }
  free (mv);
}

void free_mem_FME()
{
  free_mem2Dint(McostState);
  free_mem_mincost (all_mincost);
  free_mem_bwmincost(all_bwmincost);

  free_mem2D(SearchState);
}

void
FME_SetMotionVectorPredictor (int  pmv[2],
                          int  **refFrArr,
                          int  ***tmp_mv,
                          int  ref_frame,
                          int  mb_x,
                          int  mb_y,
                          int  blockshape_x,
                          int  blockshape_y,
						  int  blocktype,
						  int  ref)
{
  int pic_block_x          = (img->block_x>>1) + (mb_x>>3);
  int pic_block_y          = (img->block_y>>1) + (mb_y>>3);
  int mb_nr                = img->current_mb_nr;
  int mb_width             = img->width/16;
  int mb_available_up      = (img->mb_y == 0          ) ? 0 : (img->mb_data[mb_nr].slice_nr == img->mb_data[mb_nr-mb_width  ].slice_nr);  // jlzheng 6.23
  int mb_available_left    = (img->mb_x == 0          ) ? 0 : (img->mb_data[mb_nr].slice_nr == img->mb_data[mb_nr-1         ].slice_nr);  // jlzheng 6.23
  int mb_available_upleft  = (img->mb_x == 0 ||
                              img->mb_y == 0          ) ? 0 : (img->mb_data[mb_nr].slice_nr == img->mb_data[mb_nr-mb_width-1].slice_nr);  // jlzheng 6.23
  int mb_available_upright = (img->mb_x >= mb_width-1 ||
                              img->mb_y == 0          ) ? 0 : (img->mb_data[mb_nr].slice_nr == img->mb_data[mb_nr-mb_width+1].slice_nr);  // jlzheng 6.23
  int block_available_up, block_available_left, block_available_upright, block_available_upleft;
  int mv_a, mv_b, mv_c, mv_d, pred_vec=0;
  int mvPredType, rFrameL, rFrameU, rFrameUR;
  int hv;

//FAST MOTION ESTIMATION. ZHIBO CHEN 2003.3
  int SAD_a, SAD_b, SAD_c, SAD_d;
  int temp_pred_SAD[2];
	/*lgp*/
  int y_up = 1,y_upright=1,y_upleft=1,off_y=0;
  int mva[3] , mvb[3],mvc[3];
  /*Lou 1016 Start*/
  
  int rFrameUL;
  Macroblock*     currMB = &img->mb_data[img->current_mb_nr];
  int smbtypecurr, smbtypeL, smbtypeU, smbtypeUL, smbtypeUR;
  
	smbtypecurr = -2;
	smbtypeL = -2;
	smbtypeU = -2;
	smbtypeUL = -2;
	smbtypeUR = -2;

  /*Lou 1016 End*/

	pred_SAD_space = 0;
	
  /* D B C */
  /* A X   */
  
  /* 1 A, B, D are set to 0 if unavailable       */
  /* 2 If C is not available it is replaced by D */

    block_available_up   = mb_available_up   || (mb_y > 0);
    block_available_left = mb_available_left || (mb_x > 0);