/********************************************************************
 * Project: Dissolve Detection                Justin Miller 
 * File: hausd_corr.c        HAUSD_CORR.C
 *
 * Purpose: Implements in the callable procedure manhattan_hausd()
 *   the hausdorff distance correlation stereo matching, using the
 *   manhattan distance.
 * Limitations: There is no bound checking on input parameters. 
 * This File: 
 *   -manhattan_hausd() procedure, computes 1D correlation
 *   -manhattan_dialate() procedure, dialates a pbm by delta.
 *   -densify_output() procedure, coverts output to dense format.
 *   -hausd_2d() procedure, computes global correlation of 2 
 *    images.
 *
 * Last Sanity Check: 950220.15
 *
 * ToDo: eventually I might want to implement a distance transform
 *   dialate, but the manhattan appears to be as fast for delta that
 *   are less than 8 or so. (and right now I'm so drained I don't 
 *   think I'd be able to figure it out).
 *
 * See Also: correlation.c
 * Copyright (c) 1995 Cornell University.  All Rights Reserved.
 ********************************************************************/
static char rcsid[] = "@(#)$Header: /usr/u/jmiller/prog/ImTrans/src/RCS/hausd_corr.c,v 1.1 1995/07/01 17:05:59 jmiller Exp jmiller $";

#include <stdio.h>
#include "math.h"
#include "misc.h"
#include "image.h"
#include "dbug.h"
#include "assert.h"
#include "routines.h"
#include "hausd_corr.h"

/* MAGIC NUMBERS*/

/* PROTOTYPES FOR INTERNAL PROTOTYPES*/
BinaryImage manhattan_dialate(BinaryImage inim, int delta);
void densify_output(BinaryImage edge, GrayImage disparity,
		    int cX, int cY);
void OLDmanhattan_dilate_wstor(BinaryImage inim, BinaryImage outim, 
			    BinaryImage tempim, int delta);
/* GLOBAL VARIABLES*/

/* PROCEDURES*/

void manhattan_dilate_wstor(BinaryImage inim, BinaryImage *outimPtr, 
			    BinaryImage *tempimPtr, int delta){
  BinaryImage current, next, holdim;
  int curDelta;
  int x, y, maxX, maxY, limX, limY;

  assert(delta >= 0);
  
  maxX = (int)imGetWidth(inim);
  maxY = (int)imGetHeight(inim);
  limX = maxX - 1; 
  limY = maxY - 1;

  if(delta == 0){
    memcpy(&(imRef(*outimPtr,0,0)),&(imRef(inim,0,0)),(unsigned)(maxX*maxY));
    return;
  }
  
  current = inim;
  next = *outimPtr;
  for(curDelta = delta; curDelta > 0; curDelta--){

    /* y = 0 ; x = 0*/
    if(imRef(current, 0, 0) == 1){
      imRef(next, 0, 0) = 1;
      imRef(next, 1, 0) = 1;
      imRef(next, 0, 1) = 1;
    }
    /* y = 0 middle x*/
    for(x = 1; x < limX; x++){
      if(imRef(current, x, 0) == 1){
	imRef(next, x-1,0) = 1;
	imRef(next, x  ,0) = 1;
	imRef(next, x+1,0) = 1;
	imRef(next, x  ,1) = 1;
      }
    }
    /* y = 0 x = limX*/
    if(imRef(current, limX, 0) == 1){
      imRef(next, limX, 0) = 1;
      imRef(next, limX, 1) = 1;
      imRef(next, limX-1, 0) = 1;
    }

    /* middle Y*/
    for(y = 1; y < limY; y++){
      if(imRef(current, 0, y) == 1){
	imRef(next, 0, y-1) = 1;
	imRef(next, 0, y) = 1;
	imRef(next, 1, y) = 1;
	imRef(next, 0, y+1) = 1;
      }
      for(x = 1; x < limX; x++){
	if(imRef(current, x, y) == 1){
	  imRef(next, x, y-1) = 1;
	  imRef(next, x-1, y) = 1;
	  imRef(next, x, y) = 1;
	  imRef(next, x+1, y) = 1;
	  imRef(next, x, y+1) = 1;
	}
      }
      if(imRef(current, 0, y) == 1){
	imRef(next, limX, y-1) = 1;
	imRef(next, limX-1, y) = 1;
	imRef(next, limX, y) = 1;
	imRef(next, limX, y+1) = 1;
      }
    }

    /* y = limY*/
    if(imRef(current, 0, limY) == 1){
      imRef(next, 0, limY-1) = 1;
      imRef(next, 0, limY) = 1;
      imRef(next, 1, limY) = 1;
    }
    /* y = limX middle x*/
    for(x = 1; x < limX; x++){
      if(imRef(current, x, limY) == 1){
	imRef(next, x  ,limY-1) = 1;
	imRef(next, x-1,limY) = 1;
	imRef(next, x  ,limY) = 1;
	imRef(next, x+1,limY) = 1;
      }
    }
    /* y = limX x = limX*/
    if(imRef(current, limX,limY) == 1){
      imRef(next, limX, limY-1) = 1;
      imRef(next, limX-1, limY) = 1;
      imRef(next, limX, limY) = 1;
    }
    
    holdim = (curDelta == delta) ? *tempimPtr : current;
    current = next;
    next = holdim;
  }
  *tempimPtr = next;
  *outimPtr = current;
  
  return;
}


/* format for the forward and reverse arrays:
  The arrays should be of size 2*rangeX+1, 2*rangeY+1.
  Values:
    the values in the interval -1 <= f <= 0 are reversed for hausd_2d.
    for any value < -1 the disparity at that position will be examined, 
      and consequently overwritten.
    the values above 1 are not used by hausd_2d.
  Output Values:
    
  Early Termination, if(quitEarly) hausd_2d will stop on the first
    disparity with a fraction >= earlyThr.
*/

void 
_hausd_2d_wstor(BinaryImage model, BinaryImage image, 
		BinaryImage modelD,BinaryImage imageD,
		int rangeX, int rangeY,
		double *forwardPtr, double *reversePtr,
		double earlyThr, unsigned char quitEarly,
		int *modelNpts){
  int x, y, maxX, maxY, limX, limY;
  unsigned char *bxPtr, *yRowPtr, *yPtr, *yStopPtr;
  int dispX, dispY;
  int nptsL, nptsR, matchF, matchR;
  int offX, offY;
  double *forward, *reverse;

  maxX = (int)imGetWidth(model);
  maxY = (int)imGetHeight(model);
  limX = maxX - rangeX;
  limY = maxY - rangeY;
  assert(2*rangeX+1 < maxX);
  assert(2*rangeY+1 < maxY);

  /* count the number of on pixels in the image image*/
  nptsL = nptsR = 0;
  for(y = rangeY; y < limY; y++){
    for( x = rangeX; x < limX ; x++){
      nptsR += imRef(image, x, y);
    }
  }

  /* Calculate the Hausdorff Distance*/
/*  for(RECT_TRAVERSE_INIT(offX,offY,rangeX,rangeY);
      !(RECT_TRAVERSE_STOP(offX,offY,rangeX,rangeY)) &&
      !(quitEarly && earlyThr < 0.0);
      RECT_TRAVERSE(offX,offY,rangeX,rangeY)){
*/
  for(BOX_SPIRAL_INIT(offX,offY,rangeY);
      !(BOX_SPIRAL_STOP(offX,offY,rangeY)) &&
      !(quitEarly && earlyThr < 0.0);
      BOX_SPIRAL(offX,offY)){
    
    dispX = offX + rangeX;
    dispY = offY + rangeY;
    forward = forwardPtr + dispY*((rangeX<<1)+1) + dispX;
    reverse = reversePtr + dispY*((rangeX<<1)+1) + dispX;
    
    matchF = matchR = 0;
    /* Calculate the reverse distance */    
    if(*reverse < -1.0){
      if(nptsR != 0){
	for(yPtr = &(imRef(image,rangeX,rangeY)), 
	    yRowPtr = yPtr + limX - rangeX,
	    yStopPtr = yPtr+(limY-rangeY)*maxX,
	    bxPtr = &(imRef(modelD,dispX,dispY)); 
		  yPtr < yStopPtr; 
		  
		  yPtr += rangeX<<1, 
		  bxPtr += (rangeX)<<1,
		  yRowPtr += maxX) {
		for(;  yPtr < yRowPtr ;){
		  /* AND together the two points*/
		  matchR += ((*(yPtr++)) & (*(bxPtr++)));
		}
	      }
	      assert(matchR <= nptsR);
	      *reverse = (double)matchR /(double)nptsR;
	      if(*reverse > earlyThr)
		  earlyThr = -1.0;
	    }
	    else{
	      *reverse = -0.01;
	    }
	  }
	  /* Calculate the forward distance */    
	  if(*forward < -1.0){
	    nptsL = 0;
	    for(yPtr = &(imRef(imageD,rangeX,rangeY)),
		yRowPtr = yPtr + limX - rangeX,
		yStopPtr = yPtr+(limY-rangeY)*(maxX),
		bxPtr = &(imRef(model,dispX,dispY)); 
		yPtr < yStopPtr;
		yPtr+= (rangeX)<<1,
		yRowPtr += maxX,
		bxPtr += rangeX<<1){
	      for(; yPtr < yRowPtr; yPtr++,bxPtr++){
		/* AND together the two points*/
		nptsL += *bxPtr;
		matchF += ((*yPtr) & (*bxPtr));
	      }
	    }
	    assert(matchF <= nptsL);
	    if(modelNpts != (int *)NULL)
		*modelNpts = nptsR;
	    if(nptsL == 0)
		*forward = -0.01;
	    else{
	      *forward = (double)matchF / (double)nptsL;
	      if(*forward > earlyThr)
		  earlyThr = -1.0;
	    }
	  }
  }
  return;
}


/* returns an image containing the edges that were not accounted for*/
BinaryImage
edgeDiff(BinaryImage model, BinaryImage image,
	 BinaryImage modelD, BinaryImage imageD,
	 int rangeX, int rangeY, int dispX, int dispY){
  BinaryImage diffI;
  int maxX, maxY, limX, limY;
  unsigned char *modelPtr, *yRowPtr, *imagePtr, *yStopPtr, *outPtr;
  
  maxX = (int)imGetWidth(model);
  maxY = (int)imGetHeight(model);
  limX = maxX - rangeX;
  limY = maxY - rangeY;

  diffI = (BinaryImage)imNew(IMAGE_BINARY,maxX,maxY);
  dispX += rangeX;
  dispY += rangeY;

  /* Calculate the forward distance */    
  for(outPtr = &(imRef(diffI,rangeX,rangeY)), 
      modelPtr = &(imRef(model,dispX,dispY)),
      imagePtr = &(imRef(imageD,rangeX,rangeY)), 
      yStopPtr = imagePtr+(limY-rangeY)*maxX,
      yRowPtr = imagePtr + limX - rangeX;
      imagePtr < yStopPtr; 
      imagePtr += rangeX<<1, yRowPtr += maxX, outPtr += rangeX<<1, 
      modelPtr += rangeX<<1){
    for(; imagePtr < yRowPtr; imagePtr++,modelPtr++,outPtr++){
      /* AND together the two points*/
      *outPtr = ~(*imagePtr) & (*modelPtr);
    }
  }
  return(diffI);
}





