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

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/times.h>
#include <fcntl.h>
#include <unistd.h>
#include <math.h>
#include <limits.h>

#include "frame.c"



void usage(char *progname)
{
	fprintf(stderr, "Usage: %s inputfile\n", progname);
}


inline int uc_abs(int x)
{
	return (x < 0) ? -x : x;
}


inline int get_MAD(u_char *Ftmp, u_char *Gtmp, int cols, int mad)
{
	int tmp = 0;

	for(int j=0; j < YBLKELEMS; j++) {
		for(int i=0; i < YBLKELEMS; i++) {
			tmp += uc_abs(*Gtmp - *Ftmp);
							
			Gtmp++;
			Ftmp++;
		}

		if( tmp > mad)
			break;

		/* Skip to beginning of next row */
		Gtmp += cols - YBLKELEMS;
		Ftmp += cols - YBLKELEMS;
						
	} /* End of macro block comparison for a single dx dy */


	return tmp;
}



inline int bound_check(int max_blkrows, int max_blkcols, int blk_row, 
		int blk_col, int xdisp, int ydisp)
{
	if(!blk_row && ydisp < 0) /* Row 0 */
		return 0;

	if(!blk_col && xdisp < 0) /* Col 0 */
		return 0;

	if(!(max_blkrows-blk_row-1) && ydisp > 0)
		return 0;
	
	if(!(max_blkcols-blk_col-1) && xdisp > 0)
		return 0;


	return 1;
}




inline void step_search(u_char *F, u_char *Goffset, int cols,
			int block_rows, int block_cols,
			int blk_row, int blk_col,
			int x_center, int y_center, int stepsz,
			int& curr_mad, int& dxmin, int& dymin)
{
	u_char *Foffset;
	int mad = INT_MAX;


	for(int dx=-stepsz; dx <=stepsz; dx+=stepsz) {
		for(int dy=-stepsz; dy <=stepsz; dy+=stepsz) {

			if( (dx||dy) &&  bound_check(block_rows, block_cols, 
				       blk_row, blk_col, x_center+dx, y_center+dy)) {

				Foffset = F + ((blk_row * YBLKELEMS + y_center+dy) * cols +
					       blk_col * YBLKELEMS + x_center+dx);

				mad = get_MAD(Foffset, Goffset, cols, curr_mad);
				if(mad < curr_mad) {
					dxmin = dx; dymin = dy;
					curr_mad = mad;
				}
			}
		}
	}
}



int mv_3step(Frame<u_char> *reference, Frame<u_char> *source, 
	      int& xvector, int& yvector, int blk_row, int blk_col) 
{

	u_char *F = reference->Getydata();
	u_char *G = source->Getydata();
	int block_rows, block_cols, rows, cols;	
	u_char *Foffset;
	u_char *Goffset;


	int start_x = 0, start_y = 0;
	int dxmin=0, dymin=0;

	int curr_mad = INT_MAX;

	rows = reference->GetRows();
	cols = reference->GetCols();
	block_rows = rows/YBLKELEMS;
	block_cols = cols/YBLKELEMS;

	Goffset = G + ( (blk_row * YBLKELEMS) * cols +  
			(blk_col * YBLKELEMS));

	Foffset = F + ( (blk_row * YBLKELEMS) * cols +  
			(blk_col * YBLKELEMS));

	/* Get MAD for 0,0 */
	curr_mad = get_MAD(Foffset, Goffset, cols, curr_mad);

	/* Search at [0,10], [0,-10], [-10,0], [10,0], 
	   [-10,-10], [-10,10], [10,-10], [10,10] */
	/* Step 1 */
	step_search(F, Goffset, cols, block_rows, block_cols,
		    blk_row, blk_col, start_x, start_y,
		    10, curr_mad, dxmin, dymin);
	start_x += dxmin; start_y += dymin; dxmin = dymin = 0;

	/* Step 2  +-4 */
	step_search(F, Goffset, cols, block_rows, block_cols,
		    blk_row, blk_col, start_x, start_y,
		    4, curr_mad, dxmin, dymin);
	start_x += dxmin; start_y += dymin; dxmin = dymin = 0;

	/* Step 3  +-2 */
	step_search(F, Goffset, cols, block_rows, block_cols,
		    blk_row, blk_col, start_x, start_y,
		    2, curr_mad, dxmin, dymin);
	start_x += dxmin; start_y += dymin; dxmin = dymin = 0;

	/* Step 4  +-1 */
	step_search(F, Goffset, cols, block_rows, block_cols,
		    blk_row, blk_col, start_x, start_y,
		    1, curr_mad, dxmin, dymin);
	start_x += dxmin; start_y += dymin; dxmin = dymin = 0;


	xvector = start_x;
	yvector = start_y;

	return curr_mad;

}


int main(int argc, char **argv) 
{
	int infilefd;
	Frame<u_char> *reference = new Frame<u_char>(CIF_ROWS, CIF_COLS);
	Frame<u_char> *source    = new Frame<u_char>(CIF_ROWS, CIF_COLS);
	int xvec, yvec;
       

	int numtimes = 0;

	if( argc < 2) {
		usage(argv[0]);
		exit(0);
	}

	if( (infilefd = open(argv[1], O_RDONLY)) < 0) {
		perror("file open ");
		usage(argv[0]);
		exit(0);
	}
	
	if( read(infilefd, reference->Getydata(), CIF_YSIZE) != CIF_YSIZE) {
		printf("read error");
	}


	
	for(numtimes=0;; numtimes++) {
		double tmp;
		Frame<u_char> *tmpmat;

		if( lseek(infilefd, CIF_USIZE * 2, SEEK_CUR) < 0) 
			break;

		if( read(infilefd, source->Getydata(), CIF_YSIZE) != CIF_YSIZE) 
			break;


		for(int m=0; m < (288/16); m++) {
			for(int n=0; n < (352/16); n++) {
				int mad;
				mad = mv_3step(reference, source, xvec, yvec, m, n);
				printf("%d  %d   %d  %d      %d\n",
				       m,n,xvec,yvec,mad);

			}
		}


		/* For efficiency simple exchange of source and reference
		   Matrix pointers */
		tmpmat = reference;
		reference = source;
		source = tmpmat;

	}

	close(infilefd);
	return 0;
}