/************************************************************************
 *
 *  pred.c, part of tmn (TMN encoder)
 *  
 ************************************************************************/

#include"sim.h"

static int roundtab[] = {0,0,0,1,1,1,1,1,1,1,1,1,1,1,2,2};

/**********************************************************************
 *
 *      Name:           Predict_P
 *	Description:    Predicts P macroblock in advanced or normal
 *                      mode
 *	
 *      Input:          pointers to current and previous frames
 *                      and previous interpolated image,
 *                      position and motion vector array
 *	Returns:	pointer to MB_Structure of data to be coded
 *	Side effects:	allocates memory to MB_Structure
 *
 ***********************************************************************/

MB_Structure *Predict_P(PictImage *curr_image, PictImage *prev_image,
        unsigned char *prev_ipol, int x, int y, 
        MotionVector *MV[6][MBR+1][MBC+2], int PB)
{
  int m,n;
  int curr[16][16];
  int pred[16][16];
  MotionVector *fr0,*fr1,*fr2,*fr3,*fr4;
  int sum, dx, dy;
  int xmb, ymb;

  MB_Structure *pred_error = (MB_Structure *)malloc(sizeof(MB_Structure));
    
  xmb = x/MB_SIZE+1;
  ymb = y/MB_SIZE+1;

  fr0 = MV[0][ymb][xmb];
  fr1 = MV[1][ymb][xmb];
  fr2 = MV[2][ymb][xmb];
  fr3 = MV[3][ymb][xmb];
  fr4 = MV[4][ymb][xmb];

  /* Find MB in current image */
  FindMB(x, y, curr_image->lum, curr);

  /* Find prediction based on half pel MV */
  if (advanced) {
    FindPredOBMC(x, y, MV, prev_ipol, &pred[0][0], 0, PB);
    FindPredOBMC(x, y, MV, prev_ipol, &pred[0][8], 1, PB);
    FindPredOBMC(x, y, MV, prev_ipol, &pred[8][0], 2, PB);
    FindPredOBMC(x, y, MV, prev_ipol, &pred[8][8], 3, PB);
  }

  else 
    FindPred(x, y, fr0, prev_ipol, &pred[0][0], 16, 0);

  /* Do the actual prediction */
  if (fr0->Mode == MODE_INTER || fr0->Mode == MODE_INTER_Q) {
    for (n = 0; n < MB_SIZE; n++)
      for (m = 0; m < MB_SIZE; m++) 
        pred_error->lum[n][m] = (int)(curr[n][m] - pred[n][m]);

    dx = 2*fr0->x + fr0->x_half;
    dy = 2*fr0->y + fr0->y_half;
    dx = ( dx % 4 == 0 ? dx >> 1 : (dx>>1)|1 );
    dy = ( dy % 4 == 0 ? dy >> 1 : (dy>>1)|1 );

    DoPredChrom_P(x, y, dx, dy, curr_image, prev_image, pred_error);
  }

  else if (fr0->Mode == MODE_INTER4V) {
    for (n = 0; n < MB_SIZE; n++)
      for (m = 0; m < MB_SIZE; m++) 
        pred_error->lum[n][m] = (int)(curr[n][m] - pred[n][m]);

    sum = 2*fr1->x + fr1->x_half + 2*fr2->x + fr2->x_half +
      2*fr3->x + fr3->x_half + 2*fr4->x + fr4->x_half ; 
    dx = sign(sum)*(roundtab[abs(sum)%16] + (abs(sum)/16)*2);

    sum = 2*fr1->y + fr1->y_half + 2*fr2->y + fr2->y_half +
      2*fr3->y + fr3->y_half + 2*fr4->y + fr4->y_half;
    dy = sign(sum)*(roundtab[abs(sum)%16] + (abs(sum)/16)*2);

    DoPredChrom_P(x, y, dx, dy, curr_image, prev_image, pred_error);
  }

  else
    fprintf(stderr,"Illegal Mode in Predict_P (pred.c)\n");


  return pred_error;
}


/***********************************************************************
 *
 *      Name:           Predict_B
 *	Description:    Predicts the B macroblock in PB-frame prediction
 *	
 *	Input:	        pointers to current frame, previous recon. frame,
 *                      pos. in image, MV-data, reconstructed macroblock
 *                      from image ahead
 *	Returns:        pointer to differential MB data after prediction
 *	Side effects:   allocates memory to MB_structure
 *
 ***********************************************************************/

MB_Structure *Predict_B(PictImage *curr_image, PictImage *prev_image,
        unsigned char *prev_ipol,int x, int y,
        MotionVector *MV[5][MBR+1][MBC+2],
        MB_Structure *recon_P, int TRD,int TRB)
{
  int i,j,k;
  int dx, dy, sad, sad_min=INT_MAX, curr[16][16], bdx=0, bdy=0;
  MB_Structure *p_err = (MB_Structure *)malloc(sizeof(MB_Structure));
  MB_Structure *pred = (MB_Structure *)malloc(sizeof(MB_Structure));
  MotionVector *f[5];
  int xvec, yvec, mvx, mvy;

  for (k = 0; k <= 4; k++)
    f[k] = MV[k][y/MB_SIZE+1][x/MB_SIZE+1];

  /* Find MB in current image */
  FindMB(x, y, curr_image->lum, curr);

  if (f[0]->Mode == MODE_INTER4V) {  /* Mode INTER4V */
    /* Find forward prediction */

    /* Luma */
    for (j = -DEF_PBDELTA_WIN; j <= DEF_PBDELTA_WIN; j++) {
      for (i = -DEF_PBDELTA_WIN; i <= DEF_PBDELTA_WIN; i++) {

        FindForwLumPredPB(prev_ipol, x, y, f[1], &pred->lum[0][0], 
          TRD, TRB, i, j, 8, 0);
        FindForwLumPredPB(prev_ipol, x, y, f[2], &pred->lum[0][8], 
          TRD, TRB, i, j, 8, 1);
        FindForwLumPredPB(prev_ipol, x, y, f[3], &pred->lum[8][0], 
          TRD, TRB, i, j, 8, 2);
        FindForwLumPredPB(prev_ipol, x, y, f[4], &pred->lum[8][8], 
          TRD, TRB, i, j, 8, 3);

        sad = SAD_MB_integer(&curr[0][0],&pred->lum[0][0], 16,INT_MAX);
        if (i == 0 && j == 0)
          sad -= PREF_PBDELTA_NULL_VEC;
        if (sad < sad_min) {
          sad_min = sad;
          bdx = i;
          bdy = j;
        }
      }
    }

    FindForwLumPredPB(prev_ipol,x,y,f[1],&pred->lum[0][0],TRD,TRB,bdx,bdy,8,0);
    FindForwLumPredPB(prev_ipol,x,y,f[2],&pred->lum[0][8],TRD,TRB,bdx,bdy,8,1);
    FindForwLumPredPB(prev_ipol,x,y,f[3],&pred->lum[8][0],TRD,TRB,bdx,bdy,8,2);
    FindForwLumPredPB(prev_ipol,x,y,f[4],&pred->lum[8][8],TRD,TRB,bdx,bdy,8,3);

    /* chroma vectors are sum of B luma vectors divided and rounded */
    xvec = yvec = 0;
    for (k = 1; k <= 4; k++) {
      xvec += TRB*(2*f[k]->x + f[k]->x_half)/TRD + bdx;
      yvec += TRB*(2*f[k]->y + f[k]->y_half)/TRD + bdy;
    }

    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindChromBlock_P(x, y, dx, dy, prev_image, pred);

    /* Find bidirectional prediction */
    FindBiDirLumPredPB(&recon_P->lum[0][0], f[1], &pred->lum[0][0], 
               TRD, TRB, bdx, bdy, 0, 0);
    FindBiDirLumPredPB(&recon_P->lum[0][8], f[2], &pred->lum[0][8], 
               TRD, TRB, bdx, bdy, 1, 0);
    FindBiDirLumPredPB(&recon_P->lum[8][0], f[3], &pred->lum[8][0], 
               TRD, TRB, bdx, bdy, 0, 1);
    FindBiDirLumPredPB(&recon_P->lum[8][8], f[4], &pred->lum[8][8], 
               TRD, TRB, bdx, bdy, 1, 1);

    /* chroma vectors are sum of B luma vectors divided and rounded */
    xvec = yvec = 0;
    for (k = 1; k <= 4; k++) {
      mvx = 2*f[k]->x + f[k]->x_half;
      mvy = 2*f[k]->y + f[k]->y_half;
      xvec += bdx == 0 ? (TRB-TRD) *  mvx / TRD : TRB * mvx / TRD + bdx - mvx;
      yvec += bdy == 0 ? (TRB-TRD) *  mvy / TRD : TRB * mvy / TRD + bdy - mvy;
    }

    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindBiDirChrPredPB(recon_P, dx, dy, pred); 
  }

  else {  /* Mode INTER or INTER_Q */
    /* Find forward prediction */

    for (j = -DEF_PBDELTA_WIN; j <= DEF_PBDELTA_WIN; j++) {
      for (i = -DEF_PBDELTA_WIN; i <= DEF_PBDELTA_WIN; i++) {

        dx = i; dy = j;
        /* To keep things simple I turn off PB delta vectors at the edges */
        if (!mv_outside_frame) {
          if (x == 0) dx = 0;
          if (x == pels - MB_SIZE) dx = 0;
          if (y == 0) dy = 0;
          if (y == lines - MB_SIZE) dy = 0;
        }

        if (f[0]->Mode == MODE_INTRA || f[0]->Mode == MODE_INTRA_Q) {
          dx = dy = 0;
        }

        if (f[0]->x == 0 && f[0]->y == 0 && 
            f[0]->x_half == 0 && f[0]->y_half == 0) {
          dx = dy = 0;
        }

        FindForwLumPredPB(prev_ipol, x, y, f[0], &pred->lum[0][0], 
          TRD, TRB, dx, dy, 16, 0);

        sad = SAD_MB_integer(&curr[0][0],&pred->lum[0][0], 16, INT_MAX);
        if (i == 0 && j == 0)
          sad -= PREF_PBDELTA_NULL_VEC;
        if (sad < sad_min) {
          sad_min = sad;
          bdx = dx;
          bdy = dy;
        }
      }
    }
    FindForwLumPredPB(prev_ipol,x,y,f[0],&pred->lum[0][0],TRD,TRB,
              bdx,bdy,16,0);

    xvec = 4 * (TRB*(2*f[0]->x + f[0]->x_half) / TRD + bdx);
    yvec = 4 * (TRB*(2*f[0]->y + f[0]->y_half) / TRD + bdy);
    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindChromBlock_P(x, y, dx, dy, prev_image, pred);

    /* Find bidirectional prediction */
    FindBiDirLumPredPB(&recon_P->lum[0][0], f[0], &pred->lum[0][0], 
               TRD, TRB, bdx, bdy, 0, 0);
    FindBiDirLumPredPB(&recon_P->lum[0][8], f[0], &pred->lum[0][8], 
               TRD, TRB, bdx, bdy, 1, 0);
    FindBiDirLumPredPB(&recon_P->lum[8][0], f[0], &pred->lum[8][0], 
               TRD, TRB, bdx, bdy, 0, 1);
    FindBiDirLumPredPB(&recon_P->lum[8][8], f[0], &pred->lum[8][8], 
               TRD, TRB, bdx, bdy, 1, 1);

    /* chroma vectors */
    mvx = 2*f[0]->x + f[0]->x_half;
    xvec = bdx == 0 ? (TRB-TRD) * mvx / TRD : TRB * mvx / TRD + bdx - mvx;
    xvec *= 4;

    mvy = 2*f[0]->y + f[0]->y_half;
    yvec = bdy == 0 ? (TRB-TRD) * mvy / TRD : TRB * mvy / TRD + bdy - mvy;
    yvec *= 4;
      
    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindBiDirChrPredPB(recon_P, dx, dy, pred); 
  }

  /* store PB-deltas */
  MV[5][y/MB_SIZE+1][x/MB_SIZE+1]->x = bdx; /* is in half pel format */
  MV[5][y/MB_SIZE+1][x/MB_SIZE+1]->y = bdy;
  MV[5][y/MB_SIZE+1][x/MB_SIZE+1]->x_half = 0;
  MV[5][y/MB_SIZE+1][x/MB_SIZE+1]->y_half = 0;


  /* Do the actual prediction */
  for (j = 0; j < MB_SIZE; j++) 
    for (i = 0; i < MB_SIZE; i++) 
      p_err->lum[j][i] = 
        *(curr_image->lum+x+i + (y+j)*pels) - pred->lum[j][i];

  y >>= 1;
  x >>= 1;
  for (j = 0; j < MB_SIZE>>1; j++) 
    for (i = 0; i < MB_SIZE>>1; i++) {
      p_err->Cr[j][i] = *(curr_image->Cr+x+i + (y+j)*cpels) - pred->Cr[j][i];
      p_err->Cb[j][i] = *(curr_image->Cb+x+i + (y+j)*cpels) - pred->Cb[j][i];
    }

  free(pred);
  return p_err;
}

/***********************************************************************
 *
 *      Name:           MB_Recon_B
 *	Description:    Reconstructs the B macroblock in PB-frame 
 *                      prediction
 *	
 *	Input:	        pointers previous recon. frame, pred. diff.,
 *                      pos. in image, MV-data, reconstructed macroblock
 *                      from image ahead
 *	Returns:        pointer to reconstructed MB data 
 *	Side effects:   allocates memory to MB_structure
 *
 ***********************************************************************/

MB_Structure *MB_Recon_B(PictImage *prev_image, MB_Structure *diff,
         unsigned char *prev_ipol,int x, int y,
         MotionVector *MV[5][MBR+1][MBC+2],
         MB_Structure *recon_P,int TRD, int TRB)
{
  int i,j,k;
  int dx, dy, bdx, bdy, mvx, mvy, xvec, yvec;
  MB_Structure *recon_B = (MB_Structure *)malloc(sizeof(MB_Structure));
  MB_Structure *pred = (MB_Structure *)malloc(sizeof(MB_Structure));
  MotionVector *f[5];

  for (k = 0; k <= 4; k++)
    f[k] = MV[k][y/MB_SIZE+1][x/MB_SIZE+1];

  bdx = MV[5][y/MB_SIZE+1][x/MB_SIZE+1]->x;
  bdy = MV[5][y/MB_SIZE+1][x/MB_SIZE+1]->y;

  if (f[0]->Mode == MODE_INTER4V) {  /* Mode INTER4V */
    /* Find forward prediction */

    /* Luma */
    FindForwLumPredPB(prev_ipol,x,y,f[1],&pred->lum[0][0],TRD,TRB,bdx,bdy,8,0);
    FindForwLumPredPB(prev_ipol,x,y,f[2],&pred->lum[0][8],TRD,TRB,bdx,bdy,8,1);
    FindForwLumPredPB(prev_ipol,x,y,f[3],&pred->lum[8][0],TRD,TRB,bdx,bdy,8,2);
    FindForwLumPredPB(prev_ipol,x,y,f[4],&pred->lum[8][8],TRD,TRB,bdx,bdy,8,3);

    /* chroma vectors are sum of B luma vectors divided and rounded */
    xvec = yvec = 0;
    for (k = 1; k <= 4; k++) {
      xvec += TRB*(2*f[k]->x + f[k]->x_half)/TRD + bdx;
      yvec += TRB*(2*f[k]->y + f[k]->y_half)/TRD + bdy;
    }

    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindChromBlock_P(x, y, dx, dy, prev_image, pred);

    /* Find bidirectional prediction */
    FindBiDirLumPredPB(&recon_P->lum[0][0], f[1], &pred->lum[0][0], 
               TRD, TRB, bdx, bdy, 0, 0);
    FindBiDirLumPredPB(&recon_P->lum[0][8], f[2], &pred->lum[0][8], 
               TRD, TRB, bdx, bdy, 1, 0);
    FindBiDirLumPredPB(&recon_P->lum[8][0], f[3], &pred->lum[8][0], 
               TRD, TRB, bdx, bdy, 0, 1);
    FindBiDirLumPredPB(&recon_P->lum[8][8], f[4], &pred->lum[8][8], 
               TRD, TRB, bdx, bdy, 1, 1);

    /* chroma vectors are sum of B luma vectors divided and rounded */
    xvec = yvec = 0;
    for (k = 1; k <= 4; k++) {
      mvx = 2*f[k]->x + f[k]->x_half;
      mvy = 2*f[k]->y + f[k]->y_half;
      xvec += bdx == 0 ? (TRB-TRD) *  mvx / TRD : TRB * mvx / TRD + bdx - mvx;
      yvec += bdy == 0 ? (TRB-TRD) *  mvy / TRD : TRB * mvy / TRD + bdy - mvy;
    }

    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindBiDirChrPredPB(recon_P, dx, dy, pred); 

  }
  else {  /* Mode INTER or INTER_Q */
    /* Find forward prediction */
    
    FindForwLumPredPB(prev_ipol,x,y,f[0],&pred->lum[0][0],TRD,TRB,
              bdx,bdy,16,0);

    xvec = 4 * (TRB*(2*f[0]->x + f[0]->x_half) / TRD + bdx);
    yvec = 4 * (TRB*(2*f[0]->y + f[0]->y_half) / TRD + bdy);
    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindChromBlock_P(x, y, dx, dy, prev_image, pred);

    /* Find bidirectional prediction */
    FindBiDirLumPredPB(&recon_P->lum[0][0], f[0], &pred->lum[0][0], 
               TRD, TRB, bdx, bdy, 0, 0);
    FindBiDirLumPredPB(&recon_P->lum[0][8], f[0], &pred->lum[0][8], 
               TRD, TRB, bdx, bdy, 1, 0);
    FindBiDirLumPredPB(&recon_P->lum[8][0], f[0], &pred->lum[8][0], 
               TRD, TRB, bdx, bdy, 0, 1);
    FindBiDirLumPredPB(&recon_P->lum[8][8], f[0], &pred->lum[8][8], 
               TRD, TRB, bdx, bdy, 1, 1);

    /* chroma vectors */
    mvx = 2*f[0]->x + f[0]->x_half;
    xvec = bdx == 0 ? (TRB-TRD) * mvx / TRD : TRB * mvx / TRD + bdx - mvx;
    xvec *= 4;

    mvy = 2*f[0]->y + f[0]->y_half;
    yvec = bdy == 0 ? (TRB-TRD) * mvy / TRD : TRB * mvy / TRD + bdy - mvy;
    yvec *= 4;
      
    /* round values according to TABLE 16/H.263 */
    dx = sign(xvec)*(roundtab[abs(xvec)%16] + (abs(xvec)/16)*2);
    dy = sign(yvec)*(roundtab[abs(yvec)%16] + (abs(yvec)/16)*2);

    FindBiDirChrPredPB(recon_P, dx, dy, pred); 

  }

  /* Reconstruction */
  for (j = 0; j < MB_SIZE; j++) 
    for (i = 0; i < MB_SIZE; i++) 
      recon_B->lum[j][i] = pred->lum[j][i] + diff->lum[j][i];
        
  for (j = 0; j < MB_SIZE>>1; j++) 
    for (i = 0; i < MB_SIZE>>1; i++) {
      recon_B->Cr[j][i] = pred->Cr[j][i] + diff->Cr[j][i];
      recon_B->Cb[j][i] = pred->Cb[j][i] + diff->Cb[j][i];
    }
  
  free(pred);
  return recon_B;
}

/**********************************************************************
 *
 *	Name:	       FindForwLumPredPB
 *	Description:   Finds the forward luma  prediction in PB-frame 
 *                     pred.
 *	
 *	Input:	       pointer to prev. recon. frame, current positon,
 *                     MV structure and pred. structure to fill
 *
 ***********************************************************************/

void FindForwLumPredPB(unsigned char *prev_ipol, int x_curr, int y_curr, 
               MotionVector *fr, int *pred, int TRD, int TRB, 
               int bdx, int bdy, int bs, int comp)
{
  int i,j;
  int xvec,yvec,lx;

  lx = (mv_outside_frame ? pels + (long_vectors?64:32) : pels);

  /* Luma */
  xvec = (TRB)*(2*fr->x + fr->x_half)/TRD + bdx;
  yvec = (TRB)*(2*fr->y + fr->y_half)/TRD + bdy;

  x_curr += ((comp&1)<<3);
  y_curr += ((comp&2)<<2);

  for (j = 0; j < bs; j++) {
    for (i = 0; i < bs; i++) {
      *(pred+i+j*16) = *(prev_ipol + (i+x_curr)*2 + xvec +
         ((j+y_curr)*2 + yvec)*lx*2);
    }
  }

  return;
}


/**********************************************************************
 *
 *	Name:	       FindBiDirLumPredPB
 *	Description:   Finds the bi-dir. luma prediction in PB-frame 
 *                     prediction
 *	
 *	Input:	       pointer to future recon. data, current positon,
 *                     MV structure and pred. structure to fill
 *
 ***********************************************************************/

void FindBiDirLumPredPB(int *recon_P, MotionVector *fr, int *pred, int TRD, 
        int TRB, int bdx, int bdy, int nh, int nv)
{
  int xstart,xstop,ystart,ystop;
  int xvec,yvec, mvx, mvy;

  mvx = 2*fr->x + fr->x_half;
  mvy = 2*fr->y + fr->y_half;

  xvec = (bdx == 0 ? (TRB-TRD) *  mvx / TRD : TRB * mvx / TRD + bdx - mvx);