/*!
 *************************************************************************************
 * \file mc_prediction.c
 *
 * \brief
 *    Functions for motion compensated prediction
 *
 * \author
 *      Main contributors (see contributors.h for copyright, 
 *                         address and affiliation details)
 *      - Alexis Michael Tourapis  <alexismt@ieee.org>
 *
 *************************************************************************************
 */
#include <assert.h>
#include <string.h>

#include "global.h"
#include "block.h"
#include "mc_prediction.h"
#include "mbuffer.h"
#include "mb_access.h"

extern StorablePicture *no_reference_picture;
extern const unsigned char subblk_offset_y[3][8][4];
extern const unsigned char subblk_offset_x[3][8][4];
/*!
 ************************************************************************
 * \brief
 *    block single list prediction
 ************************************************************************
 */
static inline void mc_prediction(struct img_par *img, 
                    int yuv,
                    int ver_block_size, 
                    int hor_block_size,
                    int joff,
                    int ioff,
                    imgpel block[MB_BLOCK_SIZE][MB_BLOCK_SIZE])
{
  static int jj;
  imgpel (*mpr) [16] = &img->mpr[yuv][joff];

  for(jj = 0; jj < ver_block_size; jj++)
  {
    memcpy(&(mpr[jj][ioff]), &(block[jj][0]), hor_block_size * sizeof(imgpel));
  }
}

/*!
 ************************************************************************
 * \brief
 *    block single list weighted prediction
 ************************************************************************
 */
static inline void weighted_mc_prediction(struct img_par *img,
                            int yuv, 
                            int ver_block_size, 
                            int hor_block_size,
                            int joff,
                            int ioff,
                            imgpel block[MB_BLOCK_SIZE][MB_BLOCK_SIZE], 
                            int wp_scale,
                            int wp_offset,
                            int weight_denom,
                            int color_clip)
{
  static int ii, jj;
  static imgpel *mpr, *b0;
  
  for(jj=0;jj<ver_block_size;jj++)
  {
    mpr = &img->mpr[yuv][jj + joff][ioff];
    b0 = block[jj];
    for(ii=0;ii<hor_block_size;ii++)
      *(mpr++) = iClip1(color_clip, 
      (rshift_rnd((wp_scale *  *(b0++)), weight_denom)  + wp_offset ));
  }
}


/*!
 ************************************************************************
 * \brief
 *    block biprediction
 ************************************************************************
 */
static inline void bi_prediction(struct img_par *img, 
                    int yuv,
                    int ver_block_size, 
                    int hor_block_size,
                    int joff,
                    int ioff,
                    imgpel block_l0[MB_BLOCK_SIZE][MB_BLOCK_SIZE], 
                    imgpel block_l1[MB_BLOCK_SIZE][MB_BLOCK_SIZE])
{
  static int ii, jj;
  static imgpel *mpr, *b0, *b1;

  for(jj = 0;jj < ver_block_size;jj++)
  {
    mpr = &img->mpr[yuv][jj + joff][ioff];    
    b0 = block_l0[jj];
    b1 = block_l1[jj];
    for(ii = 0; ii < hor_block_size;ii++)
      *(mpr++) = rshift_rnd_sf(*(b0++) + *(b1++), 1);
  }
}

/*!
 ************************************************************************
 * \brief
 *    block weighted biprediction
 ************************************************************************
 */
static inline void weighted_bi_prediction(struct img_par *img, 
                            int yuv,
                            int ver_block_size, 
                            int hor_block_size,
                            int joff,
                            int ioff,
                            imgpel block_l0[MB_BLOCK_SIZE][MB_BLOCK_SIZE], 
                            imgpel block_l1[MB_BLOCK_SIZE][MB_BLOCK_SIZE],
                            int wp_scale_l0,
                            int wp_scale_l1,
                            int wp_offset,
                            int weight_denom,
                            int color_clip)
{
  static int ii, jj;
  static imgpel *mpr, *b0, *b1;
  
  for(jj=0;jj<ver_block_size;jj++)
  {
    mpr = &img->mpr[yuv][jj + joff][ioff];    
    b0 = block_l0[jj];
    b1 = block_l1[jj];
    for(ii=0;ii<hor_block_size;ii++)
      *(mpr++) = (int)iClip1(color_clip, 
      (rshift_rnd((wp_scale_l0 * *(b0++) + wp_scale_l1 * *(b1++)), weight_denom) + wp_offset));
  }
}

/*!
 ************************************************************************
 * \brief
 *    Interpolation of 1/4 subpixel
 ************************************************************************
 */ 
void get_block_luma(ColorPlane pl, int ref_frame, StorablePicture **list, int x_pos, int y_pos, int hor_block_size, int ver_block_size, struct img_par *img, imgpel block[MB_BLOCK_SIZE][MB_BLOCK_SIZE])
{
  int dx = (x_pos & 3), dy = (y_pos & 3);
  int i, j, jj;
  int shift_x  = dec_picture->size_x;
  int maxold_x = dec_picture->size_x_m1;
  int maxold_y = (dec_picture->mb_field[img->current_mb_nr]) ? (dec_picture->size_y >> 1) - 1 : dec_picture->size_y_m1;
  int result;
  int pres_x;

  static int tmp_res[21][21];
  static int *tmp_line;
  static imgpel *p0, *p1, *p2, *p3, *p4, *p5;
  static int    *x0, *x1, *x2, *x3, *x4, *x5;
  static const int COEF[6] = { 1, -5, 20, 20, -5, 1 };
  StorablePicture *curr_ref = list[ref_frame];
  static imgpel **cur_imgY, *cur_lineY;
  int tmp_pos;
  static int ipos_m2, ipos_m1, ipos, ipos_p1, ipos_p2, ipos_p3;
  static imgpel *orig_line;

  if (curr_ref == no_reference_picture && img->framepoc < img->recovery_poc)
  {
    printf("list[ref_frame] is equal to 'no reference picture' before RAP\n");

    /* fill the block with sample value 128 */
    for (j = 0; j < ver_block_size; j++)
      for (i = 0; i < hor_block_size; i++)
        block[j][i] = 128;
    return;
  }

  if( IS_INDEPENDENT(img) )
  {
    switch( img->colour_plane_id )
    {
    case    0:
      cur_imgY = curr_ref->imgY;
      break;
    case    1:
      cur_imgY = curr_ref->imgUV[0];
      break;
    case    2:
      cur_imgY = curr_ref->imgUV[1];
      break;
    }
  }
  else if (pl==PLANE_Y)
  {
    cur_imgY = curr_ref->imgY;
  }
  else
  {
    cur_imgY = curr_ref->imgUV[pl-1]; 
  }

  x_pos = x_pos >> 2;
  y_pos = y_pos >> 2;

  if ( (y_pos > 1) && (y_pos < maxold_y - 2 - ver_block_size) && (x_pos > 1) && (x_pos < maxold_x - 2 - hor_block_size))
  {
    if (dx == 0 && dy == 0)
    {  /* fullpel position */
      for (j = 0; j < ver_block_size; j++)
      {        
        memcpy(&(block[j][0]), &(cur_imgY[ y_pos + j ][x_pos]), hor_block_size * sizeof(imgpel));
      }
    }
    else
    { /* other positions */

      if (dy == 0)
      { /* No vertical interpolation */
        for (j = 0; j < ver_block_size; j++)
        {
          p0 = &cur_imgY[y_pos + j][x_pos - 2];
          p1 = p0 + 1;
          p2 = p1 + 1;
          p3 = p2 + 1;
          p4 = p3 + 1;
          p5 = p4 + 1;
          orig_line = block[j];

          for (i = 0; i < hor_block_size; i++)
          {        
            result  = (*(p0++) + *(p5++)) * COEF[0]
                    + (*(p1++) + *(p4++)) * COEF[1]
                    + (*(p2++) + *(p3++)) * COEF[2];

            *orig_line++ = iClip1(img->max_imgpel_value, ((result + 16)>>5));
          }
        }

        if ((dx&1) == 1)
        {          
          jj = y_pos;
          for (j = 0; j < ver_block_size; j++)
          {
            cur_lineY = &(cur_imgY[ jj++ ][x_pos + (dx >> 1)]);
            orig_line = block[j];
            for (i = 0; i < hor_block_size; i++)
            {
              *orig_line = (*orig_line + *(cur_lineY++) + 1 )>>1;
              orig_line++;
            }
          }
        }
      }
      else if (dx == 0)
      {  /* No horizontal interpolation */        
        p0 = &(cur_imgY[y_pos - 2][x_pos]);
        for (j = 0; j < ver_block_size; j++)
        {                  
          p1 = p0 + shift_x;          
          p2 = p1 + shift_x;
          p3 = p2 + shift_x;
          p4 = p3 + shift_x;
          p5 = p4 + shift_x;
          orig_line = block[j];

          for (i = 0; i < hor_block_size; i++)
          {
            result  = (*(p0++) + *(p5++)) * COEF[0]
                    + (*(p1++) + *(p4++)) * COEF[1]
                    + (*(p2++) + *(p3++)) * COEF[2];

            *orig_line++ = iClip1(img->max_imgpel_value, ((result + 16)>>5));
          }
          p0 = p1 - hor_block_size;
        }

        if ((dy&1) == 1)
        {
          jj = y_pos + (dy >> 1);
          for (j = 0; j < ver_block_size; j++)
          {
            cur_lineY = &(cur_imgY[jj++][x_pos]);
            orig_line = block[j];
            for (i = 0; i < hor_block_size; i++)
            {
              *orig_line = (*orig_line + *(cur_lineY++) + 1 )>>1;
              orig_line++;
            }
          }
        }
      }
      else if (dx == 2)
      {  /* Vertical & horizontal interpolation */
        jj = y_pos - 2;
        for (j = 0; j < ver_block_size + 5; j++)
        {
          p0 = &cur_imgY[jj++][x_pos - 2];
          p1 = p0 + 1;
          p2 = p1 + 1;
          p3 = p2 + 1;
          p4 = p3 + 1;
          p5 = p4 + 1;
          orig_line = block[j];
          tmp_line  = tmp_res[j];

          for (i = 0; i < hor_block_size; i++)
          {        
            *(tmp_line++) = (*(p0++) + *(p5++)) * COEF[0]
                          + (*(p1++) + *(p4++)) * COEF[1]
                          + (*(p2++) + *(p3++)) * COEF[2];
          }
        }

        for (j = 0; j < ver_block_size; j++)
        {
          x0 = tmp_res[j    ];
          x1 = tmp_res[j + 1];
          x2 = tmp_res[j + 2];
          x3 = tmp_res[j + 3];
          x4 = tmp_res[j + 4];
          x5 = tmp_res[j + 5];
          orig_line = block[j];

          for (i = 0; i < hor_block_size; i++)
          {
            result  = (*x0++ + *x5++) * COEF[0]
                    + (*x1++ + *x4++) * COEF[1]
                    + (*x2++ + *x3++) * COEF[2];

            *(orig_line++) = iClip1(img->max_imgpel_value, ((result+512)>>10));
          }
        }

        if ((dy&1) == 1)
        {
          jj = 2 + (dy>>1);
          for (j = 0; j < ver_block_size; j++)
          {            
            tmp_line  = tmp_res[jj++];
            orig_line = block[j];
            for (i = 0; i < hor_block_size; i++)
            {
              *orig_line = (*orig_line + iClip1(img->max_imgpel_value, ((*(tmp_line++) + 16) >> 5)) + 1 )>>1;
              orig_line++;
            }
          }
        }
      }
      else if (dy == 2)
      {  /* Horizontal & vertical interpolation */
        p0 = &(cur_imgY[y_pos - 2][x_pos - 2]);
        for (j = 0; j < ver_block_size; j++)
        {                    
          p1 = p0 + shift_x;
          p2 = p1 + shift_x;
          p3 = p2 + shift_x;
          p4 = p3 + shift_x;
          p5 = p4 + shift_x;
          tmp_line  = tmp_res[j];

          for (i = 0; i < hor_block_size + 5; i++)
          {
            *(tmp_line++)  = (*(p0++) + *(p5++)) * COEF[0]
                           + (*(p1++) + *(p4++)) * COEF[1]
                           + (*(p2++) + *(p3++)) * COEF[2];
          }
          p0 = p1 - (hor_block_size + 5);
        }

        for (j = 0; j < ver_block_size; j++)
        {
          orig_line = block[j];
          x0 = tmp_res[j];
          x1 = x0 + 1;
          x2 = x1 + 1;
          x3 = x2 + 1;
          x4 = x3 + 1;
          x5 = x4 + 1;

          for (i = 0; i < hor_block_size; i++)
          {
            result  = (*(x0++) + *(x5++)) * COEF[0]
                    + (*(x1++) + *(x4++)) * COEF[1]
                    + (*(x2++) + *(x3++)) * COEF[2];

            *(orig_line++) = iClip1(img->max_imgpel_value, ((result + 512)>>10));
          }
        }

        if ((dx&1) == 1)
        {
          for (j = 0; j < ver_block_size; j++)
          {
            tmp_line  = &tmp_res[j][2 + (dx>>1)];
            orig_line = block[j];
            for (i = 0; i < hor_block_size; i++)
            {
              *orig_line = (*orig_line + iClip1(img->max_imgpel_value, ((*(tmp_line++) + 16)>>5))+1)>>1;
              orig_line ++;
            }
          }
        }
      }
      else
      {  /* Diagonal interpolation */
        jj = (dy == 1 ? y_pos : y_pos + 1);

        for (j = 0; j < ver_block_size; j++)
        {
          p0 = &cur_imgY[jj++][x_pos - 2];
          p1 = p0 + 1;
          p2 = p1 + 1;
          p3 = p2 + 1;
          p4 = p3 + 1;
          p5 = p4 + 1;

          orig_line = block[j];