/*!
 *************************************************************************************
 * \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];
extern const unsigned char cofuv_blk_y[3][8][4];
extern const unsigned char cofuv_blk_x[3][8][4];
/*!
 ************************************************************************
 * \brief
 *    block single list prediction
 ************************************************************************
 */
static 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];

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

/*!
 ************************************************************************
 * \brief
 *    block single list weighted prediction
 ************************************************************************
 */
static 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;
  imgpel *mpr;
  
  for(jj=0;jj<ver_block_size;jj++)
  {
    mpr = img->mpr[yuv][jj + joff];
    for(ii=0;ii<hor_block_size;ii++)
      mpr[ii+ioff] = iClip1(color_clip, 
      (rshift_rnd((wp_scale *  block[jj][ii]), weight_denom)  + wp_offset ));
  }
}


/*!
 ************************************************************************
 * \brief
 *    block biprediction
 ************************************************************************
 */
static 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;
  imgpel *mpr;

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

/*!
 ************************************************************************
 * \brief
 *    block weighted biprediction
 ************************************************************************
 */
static 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;
  imgpel *mpr;
  
  for(jj=0;jj<ver_block_size;jj++)
  {
    mpr = img->mpr[yuv][jj + joff];    
    for(ii=0;ii<hor_block_size;ii++)
      mpr[ii+ioff] = (int)iClip1(color_clip, 
      (rshift_rnd((wp_scale_l0 * block_l0[jj][ii] + wp_scale_l1 * block_l1[jj][ii]), 
      weight_denom) + wp_offset));
  }
}

/*!
 ************************************************************************
 * \brief
 *    Interpolation of 1/4 subpixel
 ************************************************************************
 */
void get_block_luma(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;
  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, pres_y;

  static int tmp_res[21][21];
  static const int COEF[6] = { 1, -5, 20, 20, -5, 1  };
  StorablePicture *curr_ref = list[ref_frame];
  static imgpel **cur_imgY, *cur_lineY;
  static imgpel *cur_lineY_m2, *cur_lineY_m1, *cur_lineY_p1, *cur_lineY_p2, *cur_lineY_p3;
  int tmp_pos;
  static int jpos_m2, jpos_m1, jpos, jpos_p1, jpos_p2, jpos_p3;
  static int ipos_m2, ipos_m1, ipos, ipos_p1, ipos_p2, ipos_p3;

  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
  {
    cur_imgY = curr_ref->imgY;
  }

  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 */
        tmp_pos = x_pos - 2;
        for (i = 0; i < hor_block_size; i++)
        {        
          ipos_m2 = tmp_pos++;
          ipos_m1 = tmp_pos++;
          ipos    = tmp_pos++;
          ipos_p1 = tmp_pos++;
          ipos_p2 = tmp_pos++;
          ipos_p3 = tmp_pos  ;
          tmp_pos -= 4;

          for (j = 0; j < ver_block_size; j++)
          {
            cur_lineY = cur_imgY[y_pos + j];

            result  = (cur_lineY[ipos_m2] + cur_lineY[ipos_p3]) * COEF[0];
            result += (cur_lineY[ipos_m1] + cur_lineY[ipos_p2]) * COEF[1];
            result += (cur_lineY[ipos   ] + cur_lineY[ipos_p1]) * COEF[2];

            block[j][i] = iClip1(img->max_imgpel_value, ((result + 16)>>5));
          }
        }

        if ((dx&1) == 1)
        {
          for (j = 0; j < ver_block_size; j++)
          {
            cur_lineY = &(cur_imgY[ y_pos + j ][x_pos + (dx >> 1)]);
            for (i = 0; i < hor_block_size; i++)
            {
              block[j][i] = (block[j][i] + *(cur_lineY++) + 1 )>>1;
            }
          }
        }
      }
      else if (dx == 0)
      {  /* No horizontal interpolation */        
        cur_lineY_m2 = &(cur_imgY[y_pos - 2][x_pos]);
        for (j = 0; j < ver_block_size; j++)
        {                  
          cur_lineY_m1 = cur_lineY_m2 + shift_x;          
          cur_lineY    = cur_lineY_m1 + shift_x;
          cur_lineY_p1 = cur_lineY    + shift_x;
          cur_lineY_p2 = cur_lineY_p1 + shift_x;
          cur_lineY_p3 = cur_lineY_p2 + shift_x;

          for (i = 0; i < hor_block_size; i++)
          {
            result  = (*(cur_lineY_m2++) + *(cur_lineY_p3++)) * COEF[0];
            result += (*(cur_lineY_m1++) + *(cur_lineY_p2++)) * COEF[1];
            result += (*(cur_lineY   ++) + *(cur_lineY_p1++)) * COEF[2];

            block[j][i] = iClip1(img->max_imgpel_value, ((result+16)>>5));
          }
          cur_lineY_m2 = cur_lineY_m1 - hor_block_size;
        }

        if ((dy&1) == 1)
        {
          for (j = 0; j < ver_block_size; j++)
          {
            cur_lineY = &(cur_imgY[ y_pos + j + (dy >> 1)][x_pos]);
            for (i = 0; i < hor_block_size; i++)
            {
              block[j][i] = (block[j][i] + *(cur_lineY++) + 1 )>>1;
            }
          }
        }
      }
      else if (dx == 2)
      {  /* Vertical & horizontal interpolation */
        tmp_pos = x_pos - 2;
        for (i = 0; i < hor_block_size; i++)
        {        
          ipos_m2 = tmp_pos++;
          ipos_m1 = tmp_pos++;
          ipos    = tmp_pos++;
          ipos_p1 = tmp_pos++;
          ipos_p2 = tmp_pos++;
          ipos_p3 = tmp_pos  ;
          tmp_pos -= 4;

          for (j = 0; j < ver_block_size + 5; j++)
          {
            cur_lineY = cur_imgY[y_pos + j - 2];

            tmp_res[j][i]  = (cur_lineY[ipos_m2] + cur_lineY[ipos_p3]) * COEF[0];
            tmp_res[j][i] += (cur_lineY[ipos_m1] + cur_lineY[ipos_p2]) * COEF[1];
            tmp_res[j][i] += (cur_lineY[ipos   ] + cur_lineY[ipos_p1]) * COEF[2];
          }
        }

        for (j = 0; j < ver_block_size; j++)
        {
          jpos_m2 = j    ;
          jpos_m1 = j + 1;
          jpos    = j + 2;
          jpos_p1 = j + 3;
          jpos_p2 = j + 4;
          jpos_p3 = j + 5;

          for (i = 0; i < hor_block_size; i++)
          {
            result  = (tmp_res[jpos_m2][i] + tmp_res[jpos_p3][i]) * COEF[0];
            result += (tmp_res[jpos_m1][i] + tmp_res[jpos_p2][i]) * COEF[1];
            result += (tmp_res[jpos   ][i] + tmp_res[jpos_p1][i]) * COEF[2];

            block[j][i] = iClip1(img->max_imgpel_value, ((result+512)>>10));
          }
        }

        if ((dy&1) == 1)
        {
          for (j = 0; j < ver_block_size; j++)
          {
            pres_y = j+2+(dy>>1);
            for (i = 0; i < hor_block_size; i++)
            {
              block[j][i] = (block[j][i] + iClip1(img->max_imgpel_value, ((tmp_res[pres_y][i] + 16) >> 5)) + 1 )>>1;
            }
          }
        }
      }
      else if (dy == 2)
      {  /* Horizontal & vertical interpolation */
        cur_lineY_m2 = &(cur_imgY[y_pos - 2][x_pos - 2]);
        for (j = 0; j < ver_block_size; j++)
        {                    
          cur_lineY_m1 = cur_lineY_m2 + shift_x;
          cur_lineY    = cur_lineY_m1 + shift_x;
          cur_lineY_p1 = cur_lineY    + shift_x;
          cur_lineY_p2 = cur_lineY_p1 + shift_x;
          cur_lineY_p3 = cur_lineY_p2 + shift_x;

          for (i = 0; i < hor_block_size + 5; i++)
          {
            tmp_res[j][i]  = (*(cur_lineY_m2++) + *(cur_lineY_p3++)) * COEF[0];
            tmp_res[j][i] += (*(cur_lineY_m1++) + *(cur_lineY_p2++)) * COEF[1];
            tmp_res[j][i] += (*(cur_lineY   ++) + *(cur_lineY_p1++)) * COEF[2];
          }
          cur_lineY_m2 = cur_lineY_m1 - (hor_block_size + 5);
        }

        for (j = 0; j < ver_block_size; j++)
        {
          for (i = 0; i < hor_block_size; i++)
          {
            result  = (tmp_res[j][i    ] + tmp_res[j][i + 5]) * COEF[0];
            result += (tmp_res[j][i + 1] + tmp_res[j][i + 4]) * COEF[1];
            result += (tmp_res[j][i + 2] + tmp_res[j][i + 3]) * COEF[2];
            block[j][i] = iClip1(img->max_imgpel_value, ((result+512)>>10));
          }
        }

        if ((dx&1) == 1)
        {
          for (j = 0; j < ver_block_size; j++)
          {
            for (i = 0; i < hor_block_size; i++)
            {
              block[j][i] = (block[j][i] + iClip1(img->max_imgpel_value, ((tmp_res[j][i + 2 + (dx>>1)] + 16)>>5))+1)>>1;
            }
          }
        }
      }
      else
      {  /* Diagonal interpolation */
        tmp_pos = x_pos - 2;
        for (i = 0; i < hor_block_size; i++)
        {
          ipos_m2 = tmp_pos++;
          ipos_m1 = tmp_pos++;
          ipos    = tmp_pos++;
          ipos_p1 = tmp_pos++;
          ipos_p2 = tmp_pos++;
          ipos_p3 = tmp_pos  ;
          tmp_pos -= 4;

          for (j = 0; j < ver_block_size; j++)
          {
            cur_lineY = cur_imgY[(dy == 1 ? y_pos + j : y_pos + j + 1)];

            result  = (cur_lineY[ipos_m2] + cur_lineY[ipos_p3]) * COEF[0];
            result += (cur_lineY[ipos_m1] + cur_lineY[ipos_p2]) * COEF[1];
            result += (cur_lineY[ipos   ] + cur_lineY[ipos_p1]) * COEF[2];

            block[j][i] = iClip1(img->max_imgpel_value, ((result+16)>>5));
          }
        }

        cur_lineY_m2 = &(cur_imgY[y_pos - 2][(dx == 1 ? x_pos : x_pos + 1)]);
        for (j = 0; j < ver_block_size; j++)
        {