/*
 * Copyright 1993 by the University of Pennsylvania
 *
 * Permission to use, copy, and distribute for non-commercial purposes,
 * is hereby granted without fee, providing that the above copyright
 * notice appear in all copies and that both the copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * The software may be modified for your own purposes, but modified versions
 * may not be distributed.
 *
 * This software is provided "as is" without any expressed or implied warranty.
 */

/*****************************************************************************
 **  construct_mv.c - Calculates motion vectors for a sequence of images    **
 **                                                                         **
 **  Uses full search to calculate motion vectors for macroblocks           **
 **  (16x16 luminance pixels). Number of operations is significantly        **
 **   lower than a half pixel search. The search space  used is a 30x30     **
 **   pixel area => motion vectors are [0,7] (+/-). The motion vectors are  **
 **  then stored in the following format x y E for each macroblock, where   **
 **  x & y are the x and y displacements of the macroblock and E is the     **
 **  absolute error. Macroblocks are defined in sequence starting from the  **
 **  top left.                                                              **
 *****************************************************************************/
#include <stdio.h>
#include <malloc.h>
#include <sys/file.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#ifdef _AIX
#include <sys/tape.h>
#include <sys/devinfo.h>
#else
#include <sys/mtio.h>
#endif
#include <errno.h>
#include "global.h"
#include "construct_mv.h"

int 
  construct_mv(Yframe1,Yframe0,mvx,mvy,width,height)
byte *Yframe0; /* target frame */
byte *Yframe1; /* cur frame */
int  *mvx;
int  *mvy;
int   width,height;
{
  MV *mvector;
  int i,j,nhmblock,nvmblock;
/*---- define global values ----*/
#ifdef DEBUG
  fprintf(stderr,"constructing MV's ");
#endif
  ncols = width;
  nlines = height;
  nhmblock = width / MBSIZE; 
  nvmblock = height / MBSIZE; 
/*---- get MV's ----*/
  for(i=0;i<nvmblock;i++) {
#ifdef DEBUG
    fprintf(stderr,".");
#endif
    if (Xon)
      if(working_update(1, NULL, 0)) return -1;

    for(j=0;j<nhmblock;j++) {
      mvector = get_mv(Yframe0,Yframe1,i,j,width,height);
      mvx[i*nhmblock+j] = mvector->xdisp;
      mvy[i*nhmblock+j] = mvector->ydisp;
#ifdef DDEBUG
  fprintf(stdout,"(%3d,%3d) (%3d,%3d) error = %5d\n",
     i,j,mvx[i*nhmblock+j],mvy[i*nhmblock+j],mvector->error);
#endif
    free(mvector);
    }
  }
#ifdef DEBUG
  fprintf(stderr," Complete.\n");
#endif
  return 0;
}
/*******************Calculate motion vectors ********************************/
MV*
  get_mv(Y_frame0, Y_frame1, vno, hno, width, height)
byte *Y_frame0;
byte *Y_frame1;
int vno, hno;
int width,height;
{
  MV *result_mv;
  byte *cur_mblock;
  int vcoord,hcoord;
  
  /* calculate coord of current macroblock*/
  vcoord = vno*16;
  hcoord = hno*16;
  cur_mblock = get_mblock(Y_frame1,vcoord,hcoord);
#ifdef FAST_MV
  result_mv = fast(Y_frame0,vcoord,hcoord,cur_mblock,width,height);
#else 
  result_mv = exhaustive(Y_frame0,vcoord,hcoord,cur_mblock,width,height);
#endif
  return(result_mv);
}
/******************* get current macroblock  *********************************/
byte * 
  get_mblock(frame, vno, hno)
byte *frame;
int vno, hno;
{
  int i,j,start;
  byte *m_block;
  
  m_block = (byte *)malloc(MBSIZE_2*sizeof(byte));
  start = (vno*ncols + hno);
  for(i=0;i<MBSIZE;i++)
    for(j=0;j<MBSIZE;j++)
      m_block[i*MBSIZE + j] = frame[start+i*ncols+j];
  return m_block;
}
/*************** get vector corresponding to minimum error ******************/
/*             This is the most time intensive routine.                     */
/****************************************************************************/
MV* 
  exhaustive(frame, vcoord, hcoord, ref_block, width, height)
byte *frame;
int vcoord, hcoord;
byte *ref_block;
int width,height;
{
  MV *result;
  byte *disp_block;
  register int i,j,k,error,x,y,minerr;
  int is,ie,js,je; /* start and end value define */
  result=(MV*)malloc(sizeof(MV));
  minerr = 256*256; /* initialize minerr to max error */
  x = -999;
  y = -999;
  /* starting point in pixels of search area */
  is = - DISPLACE; ie = -is; js = is; je = ie;
  /* search are limit check */
  if( vcoord < DISPLACE) 
    is = - vcoord;
  if( height - ( vcoord + MBSIZE ) < DISPLACE )
    ie = height - ( vcoord + MBSIZE );
  if( hcoord < DISPLACE)  
    js = - hcoord;
  if( width - ( hcoord + MBSIZE ) < DISPLACE )
    je = width - ( hcoord + MBSIZE );
  for(i=is;i<=ie;i++)
    for(j=js;j<=je;j++){
	disp_block = get_mblock(frame,vcoord+i,hcoord+j);
        error = 0;
        for(k=0;k<MBSIZE_2;k++) 
          error += abs(ref_block[k] - disp_block[k]);
	if ( error < minerr) {
	  x = j;
	  y = i;
	  minerr = error;
	}
	free(disp_block);
      }
  result->error = minerr; /* initialize result->error to max error */
  result->xdisp = x;
  result->ydisp = y;
  free(ref_block);
  return result;
}

MV* 
  fast(frame, vcoord, hcoord, ref_block, width, height)
byte *frame;
int vcoord, hcoord;
byte *ref_block;
int width,height;
{
  MV *result;
  byte *disp_block;
  register int i,j,k,error,x,y,minerr;
  int is,ie,js,je; /* start and end value define */
  int is2,ie2,js2,je2; /* start and end value define */
  result=(MV*)malloc(sizeof(MV));
  minerr = 256*256; /* initialize minerr to max error */
  x = -999;
  y = -999;
  /* starting point in pixels of search area */
  is = - DISPLACE; ie = -is; js = is; je = ie;
  /* search are limit check */
  if( vcoord < DISPLACE) 
    is = - vcoord;
  if( height - ( vcoord + MBSIZE ) < DISPLACE )
    ie = height - ( vcoord + MBSIZE );
  if( hcoord < DISPLACE)  
    js = - hcoord;
  if( width - ( hcoord + MBSIZE ) < DISPLACE )
    je = width - ( hcoord + MBSIZE );
  for(i=is;i<=ie;i+=((ie - is)/2))
    for(j=js;j<=je;j+=((je - js)/2)){
	disp_block = get_mblock(frame,vcoord+i,hcoord+j);
        error = 0;
        for(k=0;k<MBSIZE_2;k++) 
          error += abs(ref_block[k] - disp_block[k]);
	if ( error < minerr) {
	  x = j;
	  y = i;
	  minerr = error;
	}
	free(disp_block);
      }
  is2 = ((y - 2 < is) ? is : y - 2);
  ie2 = ((y + 2 > ie) ? ie : y + 2);
  js2 = ((x - 2 < js) ? js : x - 2);
  je2 = ((x + 2 > je) ? je : x + 2);
  for(i=is2;i<=ie2;i++)
    for(j=js2;j<=je2;j++){
	disp_block = get_mblock(frame,vcoord+i,hcoord+j);
        error = 0;
        for(k=0;k<MBSIZE_2;k++) 
          error += abs(ref_block[k] - disp_block[k]);
	if ( error < minerr) {
	  x = j;
	  y = i;
	  minerr = error;
	}
	free(disp_block);
      }
  result->error = minerr; /* initialize result->error to max error */
  result->xdisp = x;
  result->ydisp = y;
  free(ref_block);
  return result;
}
