/************************************************************************
 *
 *  recon.c, motion compensation routines for tmndecode (H.263 decoder)
 *  
 ************************************************************************/

#include <stdio.h>
#include <stdlib.h>

#include "config.h"
#include "main.h"
#include "global.h"

/* private prototypes */
static void recon_comp _ANSI_ARGS_((unsigned char *src, unsigned char *dst,
  int lx, int lx2, int w, int h, int x, int y, int dx, int dy, int flag));
static void recon_comp_obmc _ANSI_ARGS_((unsigned char *src,
  unsigned char *dst,int lx,int lx2,int comp,int w,int h,int x,int y));
static void rec _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void recc _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void reco _ANSI_ARGS_((unsigned char *s, int *d, int lx, int lx2,
  int addflag,int c, int xa, int xb, int ya, int yb));
static void rech _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void rechc _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void recho _ANSI_ARGS_((unsigned char *s, int *d, int lx, int lx2,
  int addflag,int c, int xa, int xb, int ya, int yb));
static void recv _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void recvc _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void recvo _ANSI_ARGS_((unsigned char *s, int *d, int lx, int lx2,
  int addflag,int c, int xa, int xb, int ya, int yb));
static void rec4 _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void rec4c _ANSI_ARGS_((unsigned char *s, unsigned char *d, int lx,
  int lx2, int h));
static void rec4o _ANSI_ARGS_((unsigned char *s, int *d, int lx, int lx2,
  int addflag,int c, int xa, int xb, int ya, int yb));

void reconstruct(int bx,int by,int P,int bdx,int bdy)
{
  int w,h,lx,lx2,dx,dy,xp,yp,comp,sum;
  int x,y,mode,xvec,yvec;
  unsigned char *src[3];

  x = bx/16+1; y = by/16+1;
  lx = coded_picture_width;

  if (mv_outside_frame) {
    lx2 = coded_picture_width + 64;
    src[0] = edgeframe[0];
    src[1] = edgeframe[1];
    src[2] = edgeframe[2];
  }
  else {
    lx2 = coded_picture_width;
    src[0] = oldrefframe[0];
    src[1] = oldrefframe[1];
    src[2] = oldrefframe[2];
  }

  mode = modemap[y][x];

  if (P) {
    /* P prediction */
    if (adv_pred_mode) {
      w = 8; h = 8;
      /* Y*/
      for (comp = 0; comp < 4; comp++) {
        xp = bx + ((comp&1)<<3);
        yp = by + ((comp&2)<<2);
        recon_comp_obmc(src[0],newframe[0], lx,lx2,comp,w,h,xp,yp);
      }
      if (mode == MODE_INTER4V) {

        sum = MV[0][1][y][x]+MV[0][2][y][x]+ MV[0][3][y][x]+MV[0][4][y][x];
        dx = sign(sum)*(roundtab[abs(sum)%16] + (abs(sum)/16)*2);

        sum = MV[1][1][y][x]+MV[1][2][y][x]+ MV[1][3][y][x]+MV[1][4][y][x];
        dy = sign(sum)*(roundtab[abs(sum)%16] + (abs(sum)/16)*2);

      }
      else {
        dx = MV[0][0][y][x];
        dy = MV[1][0][y][x];
        /* chroma rounding */
        dx = ( dx % 4 == 0 ? dx >> 1 : (dx>>1)|1 );
        dy = ( dy % 4 == 0 ? dy >> 1 : (dy>>1)|1 );
      }
      lx>>=1;bx>>=1; lx2>>=1; 
      by>>=1;
      /* Chroma */
      recon_comp(src[1],newframe[1], lx,lx2,w,h,bx,by,dx,dy,1);
      recon_comp(src[2],newframe[2], lx,lx2,w,h,bx,by,dx,dy,2);
    }
    else { /* normal prediction mode */
      /* P prediction */
      w = 16; h = 16;
      dx = MV[0][0][y][x];
      dy = MV[1][0][y][x];
      
      /* Y */
      recon_comp(src[0],newframe[0], lx,lx2,w,h,bx,by,dx,dy,0);
      
      lx>>=1; w>>=1; bx>>=1; lx2>>=1; 
      h>>=1; by>>=1;  
      /* chroma rounding */
      dx = ( dx % 4 == 0 ? dx >> 1 : (dx>>1)|1 );
      dy = ( dy % 4 == 0 ? dy >> 1 : (dy>>1)|1 );

      /* Chroma */
      recon_comp(src[1],newframe[1], lx,lx2,w,h,bx,by,dx,dy,1);
      recon_comp(src[2],newframe[2], lx,lx2,w,h,bx,by,dx,dy,2);
    }
  }

  else {
    /* B forward prediction */
    if (adv_pred_mode) {
      if (mode == MODE_INTER4V) {
        w = 8; h = 8;
        /* Y*/
        xvec = yvec = 0;
        for (comp = 0; comp < 4; comp++) {
          xvec += dx = (trb)*MV[0][comp+1][y][x]/trd + bdx;
          yvec += dy = (trb)*MV[1][comp+1][y][x]/trd + bdy;
          xp = bx + ((comp&1)<<3);
          yp = by + ((comp&2)<<2);
          recon_comp(src[0],bframe[0], lx,lx2,w,h,xp,yp,dx,dy,0);
        }

        /* chroma rounding (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);

        lx>>=1;bx>>=1; lx2>>=1;
        by>>=1;
        /* Chroma */
        recon_comp(src[1],bframe[1], lx,lx2,w,h,bx,by,dx,dy,1);
        recon_comp(src[2],bframe[2], lx,lx2,w,h,bx,by,dx,dy,2);
      }
      else {  /* adv_pred_mode but 16x16 vector */
        w = 16; h = 16;

        dx = (trb)*MV[0][0][y][x]/trd + bdx;
        dy = (trb)*MV[1][0][y][x]/trd + bdy;
        /* Y */
        recon_comp(src[0],bframe[0], lx,lx2,w,h,bx,by,dx,dy,0);
        
        lx>>=1; w>>=1; bx>>=1; lx2>>=1;
        h>>=1; by>>=1;  

        xvec = 4*dx;
        yvec = 4*dy;

        /* chroma rounding (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);

        /* Chroma */
        recon_comp(src[1],bframe[1], lx,lx2,w,h,bx,by,dx,dy,1);
        recon_comp(src[2],bframe[2], lx,lx2,w,h,bx,by,dx,dy,2);
      }
    }
    else { /* normal B forward prediction */

      w = 16; h = 16;
      dx = (trb)*MV[0][0][y][x]/trd + bdx;
      dy = (trb)*MV[1][0][y][x]/trd + bdy;
      /* Y */
      recon_comp(src[0],bframe[0], lx,lx2,w,h,bx,by,dx,dy,0);

      lx>>=1; w>>=1; bx>>=1; lx2>>=1;
      h>>=1; by>>=1;  

      xvec = 4*dx;
      yvec = 4*dy;

      /* chroma rounding (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);

      /* Chroma */
      recon_comp(src[1],bframe[1], lx,lx2,w,h,bx,by,dx,dy,1);
      recon_comp(src[2],bframe[2], lx,lx2,w,h,bx,by,dx,dy,2);
    }
  }
}

static void recon_comp(src,dst,lx,lx2,w,h,x,y,dx,dy,chroma)
unsigned char *src;
unsigned char *dst;
int lx,lx2;
int w,h;
int x,y;
int dx,dy;
int chroma;
{
  int xint, xh, yint, yh;
  unsigned char *s, *d;

  xint = dx>>1;
  xh = dx & 1;
  yint = dy>>1;
  yh = dy & 1;

  /* origins */
  s = src + lx2*(y+yint) + x + xint;
  d = dst + lx*y + x;

  if (!xh && !yh)
    if (w!=8)
      rec(s,d,lx,lx2,h);
    else
      recc(s,d,lx,lx2,h);
  else if (!xh && yh)
    if (w!=8)
      recv(s,d,lx,lx2,h);
    else 
      recvc(s,d,lx,lx2,h);
  else if (xh && !yh)
    if (w!=8)
      rech(s,d,lx,lx2,h);
    else
      rechc(s,d,lx,lx2,h);
  else /* if (xh && yh) */
    if (w!=8)
      rec4(s,d,lx,lx2,h);
    else
      rec4c(s,d,lx,lx2,h);
}

static void rec(s,d,lx,lx2,h)
unsigned char *s, *d;
int lx,lx2,h;
{
  int j;

  for (j=0; j<h; j++)
  {
    d[0] = s[0];
    d[1] = s[1];
    d[2] = s[2];
    d[3] = s[3];
    d[4] = s[4];
    d[5] = s[5];
    d[6] = s[6];
    d[7] = s[7];
    d[8] = s[8];
    d[9] = s[9];
    d[10] = s[10];
    d[11] = s[11];
    d[12] = s[12];
    d[13] = s[13];
    d[14] = s[14];
    d[15] = s[15];
    s+= lx2;
    d+= lx;
  }
}

static void recc(s,d,lx,lx2,h)
unsigned char *s, *d;
int lx,lx2,h;
{
  int j;

  for (j=0; j<h; j++)
  {
    d[0] = s[0];
    d[1] = s[1];
    d[2] = s[2];
    d[3] = s[3];
    d[4] = s[4];
    d[5] = s[5];
    d[6] = s[6];
    d[7] = s[7];
    s+= lx2;
    d+= lx;
  }
}

static void reco(s,d,lx,lx2,addflag,c,xa,xb,ya,yb)
unsigned char *s;
int *d;
int lx,lx2,addflag,c,xa,xb,ya,yb;
{
  int i,j;
  int *om;

  om = &OM[c][ya][0];

  if (!addflag) {
    for (j = ya; j < yb; j++) {
      for (i = xa; i < xb; i++) {
        d[i] = s[i] * om[i];
      }
      s+= lx2;
      d+= lx;
      om+= 8;
    }
  }
  else {
    for (j = ya; j < yb; j++) {
      for (i = xa; i < xb; i++) {
        d[i] += s[i] * om[i];
      }
      s+= lx2;
      d+= lx;
      om+= 8;
    }
  }
}

static void rech(s,d,lx,lx2,h)
unsigned char *s, *d;
int lx,lx2,h;
{
  unsigned char *dp,*sp;
  int j;
  unsigned int s1,s2;

  sp = s;
  dp = d;
  for (j=0; j<h; j++)
  {
    s1=sp[0];
    dp[0] = (unsigned int)(s1+(s2=sp[1])+1)>>1;
    dp[1] = (unsigned int)(s2+(s1=sp[2])+1)>>1;
    dp[2] = (unsigned int)(s1+(s2=sp[3])+1)>>1;
    dp[3] = (unsigned int)(s2+(s1=sp[4])+1)>>1;
    dp[4] = (unsigned int)(s1+(s2=sp[5])+1)>>1;
    dp[5] = (unsigned int)(s2+(s1=sp[6])+1)>>1;
    dp[6] = (unsigned int)(s1+(s2=sp[7])+1)>>1;
    dp[7] = (unsigned int)(s2+(s1=sp[8])+1)>>1;
    dp[8] = (unsigned int)(s1+(s2=sp[9])+1)>>1;
    dp[9] = (unsigned int)(s2+(s1=sp[10])+1)>>1;
    dp[10] = (unsigned int)(s1+(s2=sp[11])+1)>>1;
    dp[11] = (unsigned int)(s2+(s1=sp[12])+1)>>1;
    dp[12] = (unsigned int)(s1+(s2=sp[13])+1)>>1;
    dp[13] = (unsigned int)(s2+(s1=sp[14])+1)>>1;
    dp[14] = (unsigned int)(s1+(s2=sp[15])+1)>>1;
    dp[15] = (unsigned int)(s2+sp[16]+1)>>1;
    sp+= lx2;
    dp+= lx;
  }
}

static void rechc(s,d,lx,lx2,h)
unsigned char *s, *d;
int lx,lx2,h;
{
  unsigned char *dp,*sp;
  int j;
  unsigned int s1,s2;

  sp = s;
  dp = d;
  for (j=0; j<h; j++)
  {
    s1=sp[0];
    dp[0] = (unsigned int)(s1+(s2=sp[1])+1)>>1;
    dp[1] = (unsigned int)(s2+(s1=sp[2])+1)>>1;
    dp[2] = (unsigned int)(s1+(s2=sp[3])+1)>>1;
    dp[3] = (unsigned int)(s2+(s1=sp[4])+1)>>1;
    dp[4] = (unsigned int)(s1+(s2=sp[5])+1)>>1;
    dp[5] = (unsigned int)(s2+(s1=sp[6])+1)>>1;
    dp[6] = (unsigned int)(s1+(s2=sp[7])+1)>>1;
    dp[7] = (unsigned int)(s2+sp[8]+1)>>1;
    sp+= lx2;
    dp+= lx;
  }
}

static void recho(s,d,lx,lx2,addflag,c,xa,xb,ya,yb)
unsigned char *s;
int *d;
int lx,lx2,addflag,c,xa,xb,ya,yb;
{
  int *dp,*om;
  unsigned char *sp;
  int i,j;

  sp = s;
  dp = d;
  om = &OM[c][ya][0];

  if (!addflag) {
    for (j = ya; j < yb; j++) {
      for (i = xa; i < xb; i++) {
        dp[i] = (((unsigned int)(sp[i] + sp[i+1]+1))>>1)*om[i];
      }
      sp+= lx2;
      dp+= lx;
      om+= 8;
    }
  }
  else {
    for (j = ya; j < yb; j++) {
      for (i = xa; i < xb; i++) {
        dp[i] += (((unsigned int)(sp[i] + sp[i+1]+1))>>1)*OM[c][j][i];
      }
      sp+= lx2;
      dp+= lx;
      om+= 8;
    }
  }