/*!
 ************************************************************************
 *
 * \file fast_me.c
 *
 * \brief
 *   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
 * \author 
 *    Main contributors: (see contributors.h for copyright, address and affiliation details)
 *    - Zhibo Chen         <chenzhibo@tsinghua.org.cn>
 *    - JianFeng Xu        <fenax@video.mdc.tsinghua.edu.cn>  
 * \date    
 *    2003.8
 ************************************************************************
 */

#include <stdlib.h>
#include <string.h>

#include "global.h"

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

#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, 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()
{
  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 == I_SLICE)
    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;
}

/*!
 ************************************************************************
 * \brief
 *    Dynamic memory allocation of all infomation needed for Fast ME
 * \par Input:
 * \return Number of allocated bytes
 * \date 
 *    2003/3
 ************************************************************************
 */

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

  if ((*mv = (int*****)calloc(input->img_width/4,sizeof(int****))) == NULL)
    no_mem_exit ("get_mem_mv: mv");
  for (i=0; i<input->img_width/4; i++)
  {
    if (((*mv)[i] = (int****)calloc(input->img_height/4,sizeof(int***))) == NULL)
      no_mem_exit ("get_mem_mv: mv");
    for (j=0; j<input->img_height/4; j++)
    {
      if (((*mv)[i][j] = (int***)calloc(img->max_num_references, sizeof(int**))) == NULL)
        no_mem_exit ("get_mem_mv: mv");
      for (k=0; k<img->max_num_references; 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");
      }
    }
  }

  return input->img_width/4*input->img_height/4*img->max_num_references*9*3*sizeof(int);
}
/*!
 *******************************************************************************
 * \brief
 *    Dynamic memory allocation of all infomation needed for backward prediction
 * \par Input:
 * \return Number of allocated bytes
 * \date 
 *    2003/3
 *******************************************************************************
 */
int get_mem_bwmincost (int****** mv)
{
  int i, j, k, l;


  if ((*mv = (int*****)calloc(input->img_width/4,sizeof(int****))) == NULL)
    no_mem_exit ("get_mem_mv: mv");
  for (i=0; i<input->img_width/4; i++)
  {
    if (((*mv)[i] = (int****)calloc(input->img_height/4,sizeof(int***))) == NULL)
      no_mem_exit ("get_mem_mv: mv");
    for (j=0; j<input->img_height/4; j++)
    {
      if (((*mv)[i][j] = (int***)calloc(img->max_num_references,sizeof(int**))) == NULL)
        no_mem_exit ("get_mem_mv: mv");
      for (k=0; k<img->max_num_references; 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");
      }
    }
  }

  return input->img_width/4*input->img_height/4*img->max_num_references*9*3*sizeof(int);
}

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;
}
/*!
 ************************************************************************
 * \brief
 *    free the memory allocated for of all infomation needed for Fast ME
 * \par Input:
 * \date 
 *    2003/3
 ************************************************************************
 */
void free_mem_mincost (int***** mv)
{
  int i, j, k, l;

  for (i=0; i<input->img_width/4; i++)
  {
    for (j=0; j<input->img_height/4; j++)
    {
      for (k=0; k<img->max_num_references; 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);
}

/*!
 ***********************************************************************************
 * \brief
 *    free the memory allocated for of all infomation needed for backward prediction
 * \date 
 *    2003/3
 ***********************************************************************************
 */
void free_mem_bwmincost (int***** mv)
{
  int i, j, k, l;

  for (i=0; i<input->img_width/4; i++)
  {
    for (j=0; j<input->img_height/4; j++)
    {
      for (k=0; k<img->max_num_references; 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);
}


int PartCalMad(pel_t *ref_pic,pel_t** orig_pic,pel_t *(*get_ref_line)(int, pel_t*, int, int, int, int), int blocksize_y,int blocksize_x, int blocksize_x4,int mcost,int min_mcost,int cand_x,int cand_y)
{
  int y,x4;
  int height=((img->MbaffFrameFlag)&&(img->mb_data[img->current_mb_nr].mb_field))?img->height/2:img->height;
  pel_t *orig_line, *ref_line;
  for (y=0; y<blocksize_y; y++)
    {
    ref_line  = get_ref_line (blocksize_x, ref_pic, cand_y+y, cand_x, /*img->*/height, img->width);//2004.3.3
    orig_line = orig_pic [y];
    
    for (x4=0; x4<blocksize_x4; x4++)
    {
      mcost += byte_abs[ *orig_line++ - *ref_line++ ];
      mcost += byte_abs[ *orig_line++ - *ref_line++ ];
      mcost += byte_abs[ *orig_line++ - *ref_line++ ];
      mcost += byte_abs[ *orig_line++ - *ref_line++ ];
    }
    if (mcost >= min_mcost)
    {
      break;
    }
    }
    return mcost;
}

/*!
 ************************************************************************
 * \brief
 *    FastIntegerPelBlockMotionSearch: fast pixel block motion search 
 *    this algrithm is called UMHexagonS(see JVT-D016),which includes 
 *    four steps with different kinds of search patterns
 * \par Input:
 * pel_t**   orig_pic,     // <--  original picture
 * int       ref,          // <--  reference frame (0... or -1 (backward))
 * int       pic_pix_x,    // <--  absolute x-coordinate of regarded AxB block
 * int       pic_pix_y,    // <--  absolute y-coordinate of regarded AxB block
 * int       blocktype,    // <--  block type (1-16x16 ... 7-4x4)
 * int       pred_mv_x,    // <--  motion vector predictor (x) in sub-pel units
 * int       pred_mv_y,    // <--  motion vector predictor (y) in sub-pel units
 * int*      mv_x,         //  --> motion vector (x) - in pel units
 * int*      mv_y,         //  --> motion vector (y) - in pel units
 * int       search_range, // <--  1-d search range in pel units                         
 * int       min_mcost,    // <--  minimum motion cost (cost for center or huge value)
 * double    lambda        // <--  lagrangian parameter for determining motion cost
 * \par