#include "prj.h"
#include <time.h>

int mv_scan_1(short frame,short row,short column,short num,int old_mad, short *vector_y,short *vector_x)
{
  register short mx,my; /* testing motion vectors */
  int curr_mad,value; /* current minimum mad value and current value */
  short vy,vx;
  short min_x,min_y,max_x,max_y; 

  min_x=(column-N2<0)? 0:1-N2+3;
  max_x=(column+N2>COLUMN)? 0:N2-1-3;
  min_y=(row-N2<0)? 0:1-N2+3;
  max_y=(row+N2>ROW)? 0:N2-1-3;

  curr_mad=mad2(frame,row,column,0,0,old_mad,num);
  vy=0;vx=0;
  for(my=min_y;my<=max_y;my++)
    for(mx=min_y;mx<max_x;mx++){
      value=mad2(frame,row,column,my,mx,curr_mad,num);
      tot_count16++;
      if(value<curr_mad){
	vy=my;
	vx=mx;
	curr_mad=value;
      }else if(value==curr_mad)
	if(vx*vx+vy*vy>mx*mx+my*my){
	  vy=my;
	  vx=mx;
	}
    }

  mv16_y[frame][row/N2][column/N2]=vy;
  mv16_x[frame][row/N2][column/N2]=vx;

  *vector_y=vy;
  *vector_x=vx;

  return curr_mad;
}

int mv_scan(short frame,short row,short column,short num,int old_mad, short *vector_y,short *vector_x)
{
  register short mx,my; /* testing motion vectors */
  int curr_mad,value; /* current minimum mad value and current value */
  short vy,vx;
  short min_x,min_y,max_x,max_y; 
  short pre_mx,pre_my;

  pre_my=mv_y[frame-1][row/N2][column/N2];
  pre_mx=mv_x[frame-1][row/N2][column/N2];

  min_x=(column-N2<0)? 0:1-N2+3;
  min_x=(min_x<pre_mx-5)? pre_mx-5:min_x;
  max_x=(column+N2>COLUMN)? 0:N2-1-3;
  max_x=(max_x>pre_mx+5)? pre_mx+5:max_x;
  min_y=(row-N2<0)? 0:1-N2+3;
  min_y=(min_y<pre_my-5)? pre_my-5:min_y;
  max_y=(row+N2>ROW)? 0:N2-1-3;
  max_y=(max_y>pre_my+5)? pre_my+5:max_y;

  curr_mad=mad2(frame,row,column,0,0,old_mad,num);
  vy=0;vx=0;
  for(my=min_y;my<=max_y;my+=2)
    for(mx=min_y;mx<max_x;mx+=2){
      value=mad2(frame,row,column,my,mx,curr_mad,num);
      tot_count++;
      if(value<curr_mad){
	vy=my;
	vx=mx;
	curr_mad=value;
      }else if(value==curr_mad)
	if(vx*vx+vy*vy>mx*mx+my*my){
	  vy=my;
	  vx=mx;
	}
    }

  mv16_y[frame][row/N2][column/N2]=vy;
  mv16_x[frame][row/N2][column/N2]=vx;

  *vector_y=vy;
  *vector_x=vx;

  return curr_mad;
}



int mv_fine(short frame,short row,short column,short num,short neighbors,int old_mad,short *vector_y,short *vector_x)
{
  register short mx,my; /* testing motion vectors */
  register unsigned short count;
  int curr_mad,value; /* current minimum mad value and current value */
  short min_y,max_y,min_x,max_x;
  int old_mx,old_my,vx,vy;

  curr_mad=old_mad;
  old_my=*vector_y;
  old_mx=*vector_x;
  vy=old_my;vx=old_mx;

  for(count=0;count<=neighbors;count++){
    my=old_my+neighbor_y[count];
    mx=old_mx+neighbor_x[count];
    if(mx<1-N2 || my<1-N2 || mx>N2-1 || my>N2-1) continue;
    if(my<0 && row==0) continue;
    if(my>0 && row==ROW-N2) continue;
    if(mx<0 && column==0) continue;
    if(mx>0 && column==COLUMN-N2) continue;

    value=mad2(frame,row,column,my,mx,curr_mad,num);
    if(num==256) tot_count256++;
    else tot_count64++;
    if(value<curr_mad){
      vy=my;
      vx=mx;
      curr_mad=value;
    }else if(value==curr_mad)
      if(vx*vx+vy*vy>mx*mx+my*my){
	vy=my;
	vx=mx;	
      }
  }
  *vector_y=vy;
  *vector_x=vx;

  return curr_mad;
}

int mv1(short frame,short row,short column)
{
  short vector_x,vector_y;
  int curr_mad;

  curr_mad=mv_scan_1(frame,row,column,16,99999,&vector_y,&vector_x);
  curr_mad=mad2(frame,row,column,vector_y,vector_x,99999,64);
  curr_mad=mv_fine(frame,row,column,64,20,curr_mad,&vector_y,&vector_x);
  curr_mad=mad2(frame,row,column,vector_y,vector_x,99999,256);
  curr_mad=mv_fine(frame,row,column,256,16,curr_mad,&vector_y,&vector_x);
  
  mv_y[frame][row/N2][column/N2]=vector_y;
  mv_x[frame][row/N2][column/N2]=vector_x;
  
  return curr_mad;

}

int mv(short frame,short row,short column)
{
  short vector_x,vector_y;
  int curr_mad;
  /*
  curr_mad=mv_scan(frame,row,column,16,99999,&vector_y,&vector_x);
  */
  vector_y=mv_y[frame-1][row/N2][column/N2];
  vector_x=mv_x[frame-1][row/N2][column/N2];
  
  curr_mad=mad2(frame,row,column,vector_y,vector_x,99999,64); 
  curr_mad=mv_fine(frame,row,column,64,48,curr_mad,&vector_y,&vector_x);
  
  curr_mad=mad2(frame,row,column,vector_y,vector_x,99999,256);
  curr_mad=mv_fine(frame,row,column,256,4,curr_mad,&vector_y,&vector_x);
 
  mv_y[frame][row/N2][column/N2]=vector_y;
  mv_x[frame][row/N2][column/N2]=vector_x;
  
  return curr_mad;

}