////////////////////////////////////////////////////////////////////////////
// Fast Track library
//
// This library provides functions to:
//
// 1) Capture images using Video4Linux2
//
// 2) Read images from files
//
// 3) Display images using GDK/X Windows
//
// 4) Find edges forming "squares" matching certain cross-ratio
// conditions. These conditions are projective invariant and therefore
// the squares can be found from any viewing direction.
//
// Documentation is available in a documentation folder
//
///////////////////////////////////////////////////////////////////////////
// Copyright (C) 2004  Joao Hespanha
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
// Modification history:
//
// Before August 27, 2006 
// No idea about what happened before this date...
//
// August 27, 2006
// GNU GPL added
// 
// September 5, 2006
// Flag to capture newest image added to functions
// v4l2Get(Sub)Image. Work of Justin Pearson
///////////////////////////////////////////////////////////////////////////

#include "FastTrack.h"


/*************************************************************************/
// #defines
/*************************************************************************/

////////// Debug for Image Convertions

//#define DEBUG_ENCODING
//#define DEBUG_RLE

////////// Debug defines of Horiz/VertConv()

#define DEBUG_NROWS 8
#define DEBUG_NCOLUMNS 15
//////#define DEBUG_HCONVOLUTION
//////#define DEBUG_VCONVOLUTION
//////#define DEBUG_CONVOLUTION_TIMER

/////////// Debug defines of ?EdgeCross()

//////#define DEBUG_EDGES
//////#define DEBUG_CROSS_RATIO           // print all valid cross-ratios
////#define DEBUG_CORRECT_CROSS_RATIO     // print correct cross-ratios
//#define DEBUG_BEST_CORRECT_CROSS_RATIO     // print cor&best cross-ratios

////#define DEBUG_CALL_OTHER_DIRECTION    // print size of subwindow when matching
//////#define DEBUG_BEST_NOMATCH_CROSS_RATIO// print cor&best c-r without vert match
//#define DEBUG_BEST_MATCH_CROSS_RATIO      // print cor&best c-r with vert match

//////#define OUTPUT_EDGES                // include all edges in output image
//////#define OUTPUT_WRONG_CR             // include edges with wrong c-r
//#define OUTPUT_CORRECT_CR               // include edges with correct c-r

//#define DEBUG_SINGLE_CROSS_TIMER      // printf time elapsed for ?EdgeCross
//#define DEBUG_HV_CROSS_TIMER          // printf time elapsed for HVEdgeCross
//#define DEBUG_HV_CROSS_FINAL_POSITION // printf final position

#define GREY_PLUS_EDGE  50
#define GREY_MINUS_EDGE 150
#define GREY_WRONG_CR   GREY_MINUS_EDGE
#define GREY_CORRECT_CR 255
#define GREY_EXTEND     255

///////// Debug defines of Edge?Extend()
//#define DEBUG_EXTENDEDGES
//#define DEBUG_FINAL_LINE
//#define DEBUG_EXTEND_TIMER
#define OUTPUT_EXTENDED_EDGES

#define CLEAR(x) memset (&(x), 0, sizeof (x))

/*************************************************************************/
// Image processing function
/*************************************************************************/

///////////////////////////////////////////////////////////////
// Vertical/Horizontal Edge/Cross-ratio Detection
///////////////////////////////////////////////////////////////
int VHEdgeCross(
   FT_16GrayImage_T *Image,    // original image                        (input)
   FT_16GrayImage_T *Output,   // output image                          (output)
   int FirstColumn,            // region of interest                    (input)
   int FirstRow,
   int LastColumn,
   int LastRow,
   int LengthGradDir,          // length of (half) template in grad dir (input)
   int LengthAveDir,           // length of template in averaging dir   (input)
   int EdgeThreshold,          // edge detection threhold               (input)
   float HCross,               // horizontal cross-ratio                (input)
   float HCrossThreshold,      // horizontal cross-ratio threshold      (input)
   float VCross,               // vertical cross-ratio                  (input)
   float VCrossThreshold,      // vertical cross-ratio threshold        (input)
   float *bestvm,              // slopes of vertical edges (array of 4) (output)
   float *bestvb,              // origin of vertical edges (array of 4) (output)
   float *besthm,              // slopes of horiz. edges (array of 4)   (output)
   float *besthb               // origin of horiz. edges (array of 4)   (output)
   )
{
  // loop indexes
  int i;
  register int j;
  register int imwidth;

  // previous edge positions (must be + - + -), position = -1 means undefined
  float edges[4];

  // cross ratio
  float cr,bestmatch=HUGE_VAL;

  // pointers to scan image
  register signed short *vgrad,*vgrad2,*output;

  // Gradients
  FT_16GrayImage_T HGrad,HGrad2,VGrad,VGrad2;
  
  // Edge images 
  FT_16GrayImage_T HEdge;

  // limits from EdgeHExtend
  int minrow,maxrow,mincol,maxcol,
    thisminrow,thismaxrow,thismincol,thismaxcol,
    row;
  float m[4],b[4],vedges[4];

  int   bestrow;             // row of vertical edges         
  float bestvedges[4];       // vertical edges (array of 4)
  int   bestcol;             // column of horizontal edges 
  float besthedges[4];       // horizontal edges (array of 4)

#ifdef DEBUG_HV_CROSS_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;
  int count=0;

  gettimeofday(&t0,NULL);
#endif

  // check region
  if (FirstRow<0) FirstRow=0;
  if (FirstColumn<0) FirstColumn=0;
  if (LastRow>=Image->height) LastRow=Image->height-1;
  if (LastColumn>=Image->width) LastColumn=Image->width-1;

  // Compute Horizontal gradients
  VertConv(Image,&VGrad,LengthGradDir,LengthAveDir,FirstColumn,FirstRow,LastColumn,LastRow);
  VertConv(&VGrad,&VGrad2,LengthGradDir,LengthAveDir,FirstColumn,FirstRow,LastColumn,LastRow);

  // Input/Output image width
  imwidth=Image->width;

  // adjust output size
  Output->width=Image->width;
  Output->height=Image->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=Output->width*Output->width*sizeof(signed short);

  // set pointers
  vgrad=VGrad.pixels+FirstRow*VGrad.width;
  vgrad2=VGrad2.pixels+FirstRow*VGrad2.width;
  output=Output->pixels+FirstRow*Output->width;

  for (i=FirstColumn;i<=LastColumn;i++) {
    // start with all edges undefined
    edges[0]=edges[1]=edges[2]=edges[3]=-1;

    // row loop
    for (j=FirstRow;j<=LastRow-1;j++) {
      if (abs(vgrad[j*imwidth]) > EdgeThreshold)
	// plus edge
	if (vgrad2[j*imwidth] < 0 && vgrad2[(j+1)*imwidth] > 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[(j)*imwidth]=GREY_PLUS_EDGE;
#endif
	  // save in edges array
	  edges[0]=edges[2];
	  edges[1]=edges[3];
	  edges[2]=(float)j-(float)vgrad2[j*imwidth]/(float)(vgrad2[(j+1)*imwidth]-vgrad2[j*imwidth]);
	  edges[3]=-1; // ready to receive next -1
#ifdef DEBUG_EDGES
	  fprintf(stderr,"VEdgeCross: plus  edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		  edges[2],edges[0],edges[1],edges[2],edges[3]);
#endif
	}
      // minus edge
	else if (vgrad2[j*imwidth] > 0 && vgrad2[(j+1)*imwidth] < 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[(j)*imwidth]=GREY_MINUS_EDGE;
#endif
	  // save in edges array
	  if (edges[3]==-1) {
	    edges[3]=(float)j+(float)vgrad2[j*imwidth]/(float)(vgrad2[j*imwidth]-vgrad2[(j+1)*imwidth]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"VHEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	    if (edges[0]!=-1 && edges[1]!=-1 && edges[2]!=-1) {
	      cr=(edges[0]-edges[1])*(edges[2]-edges[3])/(edges[0]-edges[2])/(edges[1]-edges[3]);
	      if (fabsf(cr-VCross)<=VCrossThreshold) {
#ifdef OUTPUT_CORRECT_CR
		output[(int)(edges[0])*imwidth]=output[(int)(edges[1])*imwidth]=output[(int)(edges[2])*imwidth]=output[(int)(edges[3])*imwidth]=GREY_CORRECT_CR;
#endif
#ifdef DEBUG_CORRECT_CROSS_RATIO
		fprintf(stderr,"VHEdgeCross: **correct c-r=%5.3f, col %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		if (fabsf(cr-VCross)<bestmatch){
		  ////////////////////////
		  // Test Horizontal match
		  ////////////////////////
		  
#ifdef DEBUG_HV_CROSS_TIMER
		  count++;
#endif
#ifdef DEBUG_CALL_OTHER_DIRECTION
		  fprintf(stderr,"VHEdgeCross: calling EdgeHExtend (col %d, rows %5.1f %5.1f %5.1f %5.1f)\n",i,edges[0],edges[1],edges[2],edges[3]);
#endif
		  minrow=LastRow;maxrow=FirstRow;
		  mincol=LastColumn;maxcol=FirstColumn;
		  EdgeHExtend(&VGrad,&VGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[0],m,b,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);
		  EdgeHExtend(&VGrad,&VGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[1],m+1,b+1,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);
		  EdgeHExtend(&VGrad,&VGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[2],m+2,b+2,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);
		  EdgeHExtend(&VGrad,&VGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[3],m+3,b+3,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);

		  minrow=minrow-1- (LengthAveDir-1);
		  maxrow=maxrow+1+ (LengthAveDir-1);
		  mincol=mincol-1- (2*LengthGradDir-1);
		  maxcol=maxcol+1+ (2*LengthGradDir-1);

		  HorizConv(Image,&HGrad,LengthGradDir,LengthAveDir,
			   mincol,minrow,maxcol,maxrow);
		  HorizConv(&HGrad,&HGrad2,LengthGradDir,LengthAveDir,
			   mincol,minrow,maxcol,maxrow);

#ifdef DEBUG_CALL_OTHER_DIRECTION
		  fprintf(stderr,"VHEdgeCross: calling HEdgeCross (row in [%d,%d], col in [%d,%d])\n",minrow,maxrow,mincol,maxcol);
#endif
		  if (!HEdgeCross(&HGrad,&HGrad2,&HEdge,
				  mincol,minrow,maxcol,maxrow,
				  EdgeThreshold,
				  HCross,HCrossThreshold,
				  &row,vedges)
		      && vedges[1]-1<=i   && vedges[2]+1>=i 
		      && edges[1] -1<=row && edges[2] +1>=row ) {
		    bestmatch=fabsf(cr-VCross);
		    bestrow=row;
		    bestcol=i;
		    memcpy(bestvedges,vedges,sizeof(edges));
		    memcpy(besthedges,edges,sizeof(edges));
		    memcpy(besthm,m,sizeof(m));
		    memcpy(besthb,b,sizeof(b));
#ifdef DEBUG_BEST_MATCH_CROSS_RATIO
		    fprintf(stderr,"VHEdgeCross: **BEST & h-match c-r=%5.3f, col %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			    cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		  }
		  else {
#ifdef DEBUG_BEST_NOMATCH_CROSS_RATIO
		    fprintf(stderr,"VHEdgeCross: **best no h-match c-r=%5.3f, col %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			    cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		  }
		  ////////////////////////
		}
		else {
		}
	      } else {
#ifdef OUTPUT_WRONG_CR
		output[(j)*imwidth]=GREY_WRONG_CR;
#endif
#ifdef DEBUG_CROSS_RATIO
		fprintf(stderr,"VHEdgeCross: --valid c-rat %5.3f, col %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
	      }
	    }
	  } else {
	    // two consecutive minus cannot be part of cross ratio
	    edges[0]=edges[1]=edges[2]=-1;
	    edges[3]=(float)j+(float)vgrad2[j*imwidth]/(vgrad2[j*imwidth]-vgrad2[j-1]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"VHEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	  }

	}
    }
    vgrad++;
    vgrad2++;
    output++;
  }

  if (bestmatch!=HUGE_VAL) {
    EdgeVExtend(&HGrad,&HGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestrow,bestvedges[0],bestvm,bestvb,
		&thisminrow,&thismaxrow,&thismincol,&thismaxcol);
    EdgeVExtend(&HGrad,&HGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestrow,bestvedges[1],bestvm+1,bestvb+1,
		&thisminrow,&thismaxrow,&thismincol,&thismaxcol);
    EdgeVExtend(&HGrad,&HGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestrow,bestvedges[2],bestvm+2,bestvb+2,
		&thisminrow,&thismaxrow,&thismincol,&thismaxcol);
    EdgeVExtend(&HGrad,&HGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestrow,bestvedges[3],bestvm+3,bestvb+3,
		&thisminrow,&thismaxrow,&thismincol,&thismaxcol);

#ifdef DEBUG_HV_CROSS_FINAL_POSITION
    fprintf(stderr,"VHEdgeCross: row %3d, edges %6.2f %6.2f %6.2f %6.2f\n",bestrow,bestvedges[0],bestvedges[1],bestvedges[2],bestvedges[3]);
    fprintf(stderr,"VHEdgeCross: col %3d, edges %6.2f %6.2f %6.2f %6.2f\n",bestcol,besthedges[0],besthedges[1],besthedges[2],besthedges[3]); 
#endif
  }

#ifdef DEBUG_HV_CROSS_TIMER
  gettimeofday(&t1,NULL);

  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);
  fprintf(stderr,"VHEdgeCross: time elapsed %7.3fms (%d calls to HEdgeCross)\n",1000*dt,count);
#endif

  return (bestmatch==HUGE_VAL);
}


///////////////////////////////////////////////////////////////
// Horizontal/Vertical Edge/Cross-ratio Detection
///////////////////////////////////////////////////////////////
int HVEdgeCross(
   FT_16GrayImage_T *Image,    // original image                        (input)
   FT_16GrayImage_T *Output,   // output image                          (output)
   int FirstColumn,            // region of interest                    (input)
   int FirstRow,
   int LastColumn,
   int LastRow,
   int LengthGradDir,          // length of (half) template in grad dir (input)
   int LengthAveDir,           // length of template in averaging dir   (input)
   int EdgeThreshold,          // edge detection threhold               (input)
   float HCross,               // horizontal cross-ratio                (input)
   float HCrossThreshold,      // horizontal cross-ratio threshold      (input)
   float VCross,               // vertical cross-ratio                  (input)
   float VCrossThreshold,      // vertical cross-ratio threshold        (input)
   float *bestvm,              // slopes of vertical edges (array of 4) (output)
   float *bestvb,              // origin of vertical edges (array of 4) (output)
   float *besthm,              // slopes of horiz. edges (array of 4)   (output)
   float *besthb               // origin of horiz. edges (array of 4)   (output)
   )
{
  // loop indexes
  int i;
  register int j;

  // previous edge positions (must be + - + -), position = -1 means undefined
  float edges[4];

  // cross ratio
  float cr,bestmatch=HUGE_VAL;

  // pointers to scan image
  register signed short *hgrad,*hgrad2,*output;

  // Gradients
  FT_16GrayImage_T HGrad,HGrad2,VGrad,VGrad2;
  
  // Edge images 
  FT_16GrayImage_T VEdge;

  // limits from EdgeVExtend
  int minrow,maxrow,mincol,maxcol,
    thisminrow,thismaxrow,thismincol,thismaxcol,
    col;
  float m[4],b[4],hedges[4];

  int   bestrow;             // row of vertical edges         
  float bestvedges[4];       // vertical edges (array of 4)
  int   bestcol;             // column of horizontal edges 
  float besthedges[4];       // horizontal edges (array of 4)

#ifdef DEBUG_HV_CROSS_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;
  int count=0;

  gettimeofday(&t0,NULL);
#endif

  // check region
  if (FirstRow<0) FirstRow=0;
  if (FirstColumn<0) FirstColumn=0;
  if (LastRow>=Image->height) LastRow=Image->height-1;
  if (LastColumn>=Image->width) LastColumn=Image->width-1;

  // Compute Horizontal gradients
  HorizConv(Image,&HGrad,LengthGradDir,LengthAveDir,FirstColumn,FirstRow,LastColumn,LastRow);
  HorizConv(&HGrad,&HGrad2,LengthGradDir,LengthAveDir,FirstColumn,FirstRow,LastColumn,LastRow);

  // adjust output size
  Output->width=Image->width;
  Output->height=Image->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=Output->width*Output->width*sizeof(signed short);

  // set pointers
  hgrad=HGrad.pixels+FirstRow*HGrad.width;
  hgrad2=HGrad2.pixels+FirstRow*HGrad2.width;
  output=Output->pixels+FirstRow*Output->width;

  for (i=FirstRow;i<=LastRow;i++) {
    // start with all edges undefined
    edges[0]=edges[1]=edges[2]=edges[3]=-1;

    // row loop
    for (j=FirstColumn;j<=LastColumn-1;j++) {
      if (abs(hgrad[j]) > EdgeThreshold)
	// plus edge
	if (hgrad2[j] < 0 && hgrad2[j+1] > 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[j]=GREY_PLUS_EDGE;
#endif
	  // save in edges array
	  edges[0]=edges[2];
	  edges[1]=edges[3];
	  edges[2]=(float)j-(float)hgrad2[j]/(float)(hgrad2[j+1]-hgrad2[j]);
	  edges[3]=-1; // ready to receive next -1
#ifdef DEBUG_EDGES
	  fprintf(stderr,"HVEdgeCross: plus  edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		  edges[2],edges[0],edges[1],edges[2],edges[3]);
#endif
	}
      // minus edge
	else if (hgrad2[j] > 0 && hgrad2[j+1] < 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[j]=GREY_MINUS_EDGE;
#endif
	  // save in edges array
	  if (edges[3]==-1) {
	    edges[3]=(float)j+(float)hgrad2[j]/(float)(hgrad2[j]-hgrad2[j+1]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"HVEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	    if (edges[0]!=-1 && edges[1]!=-1 && edges[2]!=-1) {
	      cr=(edges[0]-edges[1])*(edges[2]-edges[3])/(edges[0]-edges[2])/(edges[1]-edges[3]);
	      if (fabsf(cr-HCross)<=HCrossThreshold) {
#ifdef OUTPUT_CORRECT_CR
		output[(int)edges[0]]=output[(int)edges[1]]=output[(int)edges[2]]=output[(int)edges[3]]=GREY_CORRECT_CR;
#endif
#ifdef DEBUG_CORRECT_CROSS_RATIO
		fprintf(stderr,"HVEdgeCross: **correct c-r=%5.3f, row %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		if (fabsf(cr-HCross)<bestmatch){
		  ////////////////////////
		  // Test Vertical match
		  ////////////////////////

#ifdef DEBUG_HV_CROSS_TIMER
		  count++;
#endif		  
#ifdef DEBUG_CALL_OTHER_DIRECTION
		  fprintf(stderr,"HVEdgeCross: calling EdgeVExtend (row %d, cols %5.1f %5.1f %5.1f %5.1f)\n",i,edges[0],edges[1],edges[2],edges[3]);
#endif
		  minrow=LastRow;maxrow=FirstRow;
		  mincol=LastColumn;maxcol=FirstColumn;
		  EdgeVExtend(&HGrad,&HGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[0],m,b,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);
		  EdgeVExtend(&HGrad,&HGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[1],m+1,b+1,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);
		  EdgeVExtend(&HGrad,&HGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[2],m+2,b+2,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);
		  EdgeVExtend(&HGrad,&HGrad2,Output, //
			      FirstColumn,FirstRow,LastColumn,LastRow,
			      EdgeThreshold,i,edges[3],m+3,b+3,
			      &thisminrow,&thismaxrow,&thismincol,&thismaxcol);
		  minrow=MIN(minrow,thisminrow);maxrow=MAX(maxrow,thismaxrow);
		  mincol=MIN(mincol,thismincol);maxcol=MAX(maxcol,thismaxcol);

		  minrow=minrow-1- (2*LengthGradDir-1);
		  maxrow=maxrow+1+ (2*LengthGradDir-1);
		  mincol=mincol-1- (LengthAveDir-1);
		  maxcol=maxcol+1+ (LengthAveDir-1);

		  VertConv(Image,&VGrad,LengthGradDir,LengthAveDir,
			   mincol,minrow,maxcol,maxrow);
		  VertConv(&VGrad,&VGrad2,LengthGradDir,LengthAveDir,
			   mincol,minrow,maxcol,maxrow);

#ifdef DEBUG_CALL_OTHER_DIRECTION
		  fprintf(stderr,"HVEdgeCross: calling VEdgeCross (row in [%d,%d], col in [%d,%d])\n",minrow,maxrow,mincol,maxcol);
#endif
		  if (!VEdgeCross(&VGrad,&VGrad2,&VEdge,
				  mincol,minrow,maxcol,maxrow,
				  EdgeThreshold,
				  VCross,VCrossThreshold,
				  &col,hedges)
		      && hedges[1]-1<=i   && hedges[2]+1>=i 
		      && edges[1] -1<=col && edges[2] +1>=col ) {
		    bestmatch=fabsf(cr-HCross);
		    bestrow=i;
		    bestcol=col;
		    memcpy(bestvedges,edges,sizeof(edges));
		    memcpy(besthedges,hedges,sizeof(edges));
		    memcpy(bestvm,m,sizeof(m));
		    memcpy(bestvb,b,sizeof(b));
#ifdef DEBUG_BEST_MATCH_CROSS_RATIO
		    fprintf(stderr,"HVEdgeCross: **BEST & v-match c-r=%5.3f, row %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			    cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		  }
		  else {
#ifdef DEBUG_BEST_NOMATCH_CROSS_RATIO
		    fprintf(stderr,"HVEdgeCross: **best no v-match c-r=%5.3f, row %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			    cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		  }
		  ////////////////////////
		}
		else {
		}
	      } else {
#ifdef OUTPUT_WRONG_CR
		output[j]=GREY_WRONG_CR;
#endif
#ifdef DEBUG_CROSS_RATIO
		fprintf(stderr,"HVEdgeCross: --valid c-rat %5.3f, row %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
	      }
	    }
	  } else {
	    // two consecutive minus cannot be part of cross ratio
	    edges[0]=edges[1]=edges[2]=-1;
	    edges[3]=(float)j+(float)hgrad2[j]/(hgrad2[j]-hgrad2[j-1]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"HVEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	  }

	}
    }
    hgrad+=HGrad.width;
    hgrad2+=HGrad2.width;
    output+=Output->width;
  }

  if (bestmatch!=HUGE_VAL) {
    EdgeHExtend(&VGrad,&VGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestcol,besthedges[0],besthm,besthb,&thisminrow,&thismaxrow,&thismincol,&thismaxcol);
    EdgeHExtend(&VGrad,&VGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestcol,besthedges[1],besthm+1,besthb+1,&thisminrow,&thismaxrow,&thismincol,&thismaxcol);
    EdgeHExtend(&VGrad,&VGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestcol,besthedges[2],besthm+2,besthb+2,&thisminrow,&thismaxrow,&thismincol,&thismaxcol);
    EdgeHExtend(&VGrad,&VGrad2,Output,FirstColumn,FirstRow,LastColumn,LastRow,
		EdgeThreshold,
		bestcol,besthedges[3],besthm+3,besthb+3,&thisminrow,&thismaxrow,&thismincol,&thismaxcol);

#ifdef DEBUG_HV_CROSS_FINAL_POSITION
    fprintf(stderr,"HVEdgeCross: row %3d, edges %6.2f %6.2f %6.2f %6.2f\n",bestrow,bestvedges[0],bestvedges[1],bestvedges[2],bestvedges[3]);
    fprintf(stderr,"HVEdgeCross: col %3d, edges %6.2f %6.2f %6.2f %6.2f\n",bestcol,besthedges[0],besthedges[1],besthedges[2],besthedges[3]);
#endif
  }

#ifdef DEBUG_HV_CROSS_TIMER
  gettimeofday(&t1,NULL);

  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);
  fprintf(stderr,"HVEdgeCross: time elapsed %7.3fms  (%d calls to VEdgeCross)\n",1000*dt,count);
#endif
  
  return (bestmatch==HUGE_VAL);
}


///////////////////////////////////////////////////////////////
// Convertion of 4-horizontal/4-vertical lines (as returned by 
// VHEdgeCross() and HVEdgeCross() to 8 intersection points
///////////////////////////////////////////////////////////////
void Lines2Points(
   float *vm,  // slopes of vertical edges       (array of 4) (input)
   float *vb,  // origin of vertical edges       (array of 4) (input)
   float *hm,  // slopes of horiz. edges         (array of 4) (input)
   float *hb,  // origin of horiz. edges         (array of 4) (input)
   float *px,  // x coordinates of intersections (array of 8) (output)
               // big square clock-wise from top-left, 
               // followed by small square
   float *py   // y coordinates of intersections (array of 8) (output)
   )           // same order as px
{
  // big square
  px[0]=(vm[0]*hb[0]+vb[0])/(1-vm[0]*hm[0]);
  py[0]=(hm[0]*vb[0]+hb[0])/(1-vm[0]*hm[0]);
  px[1]=(vm[3]*hb[0]+vb[3])/(1-vm[3]*hm[0]);
  py[1]=(hm[0]*vb[3]+hb[0])/(1-vm[3]*hm[0]);
  px[2]=(vm[3]*hb[3]+vb[3])/(1-vm[3]*hm[3]);
  py[2]=(hm[3]*vb[3]+hb[3])/(1-vm[3]*hm[3]);
  px[3]=(vm[0]*hb[3]+vb[0])/(1-vm[0]*hm[3]);
  py[3]=(hm[3]*vb[0]+hb[3])/(1-vm[0]*hm[3]);

  // small square
  px[4]=(vm[1]*hb[1]+vb[1])/(1-vm[1]*hm[1]);
  py[4]=(hm[1]*vb[1]+hb[1])/(1-vm[1]*hm[1]);
  px[5]=(vm[2]*hb[1]+vb[2])/(1-vm[2]*hm[1]);
  py[5]=(hm[1]*vb[2]+hb[1])/(1-vm[2]*hm[1]);
  px[6]=(vm[2]*hb[2]+vb[2])/(1-vm[2]*hm[2]);
  py[6]=(hm[2]*vb[2]+hb[2])/(1-vm[2]*hm[2]);
  px[7]=(vm[1]*hb[2]+vb[1])/(1-vm[1]*hm[2]);
  py[7]=(hm[2]*vb[1]+hb[2])/(1-vm[1]*hm[2]);
}

///////////////////////////////////////////////////////////////
// Find line by extending on edge vertically
///////////////////////////////////////////////////////////////
void EdgeVExtend(
   FT_16GrayImage_T *Grad,     // horizontal gradient                   (input)
   FT_16GrayImage_T *Grad2,    // horizontal "2nd" gradient             (input)
   FT_16GrayImage_T *Output,   // output image                          (output)
   int FirstColumn,            // region of interest                    (input)
   int FirstRow,
   int LastColumn,
   int LastRow,
   int EdgeThreshold,          // edge detection threhold               (input)
   int   row,                  // row of vertical edge to extend        (input)
   float edge,                 // vertical edge (column) to extend      (input)
   float *m,                   // slope of vertical edge                (output)
   float *b,                   // origin of vertical edge               (output)
   int *minrow,                // row where edge starts                 (output)
   int *maxrow,                // row where edge ends                   (output)
   int *mincolumn,             // column where edge starts              (output)
   int *maxcolumn              // column where edge ends                (output)
   )
{
  // loop indexes
  int i;
  register int j;

  // pointers to scan image
  register signed short *grad,*grad2,*output;

  // edges
  float ThisEdge,LastEdge;
  
  // interpolation
  float sx,sy,sxx,sxy,syy;
  int n;

#ifdef DEBUG_EXTEND_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;

  gettimeofday(&t0,NULL);
#endif

  // check region
  if (FirstRow<0) FirstRow=0;
  if (FirstColumn<0) FirstColumn=0;
  if (LastRow>=Grad->height) LastRow=Grad->height-1;
  if (LastColumn>=Grad->width) LastColumn=Grad->width-1;

  // adjust output size
  Output->width=Grad->width;
  Output->height=Grad->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=Output->width*Output->width*sizeof(signed short);

  // place initial edge in the image
#ifdef OUTPUT_EXTENDED_EDGES
  Output->pixels[row*Output->width+(int)floorf(edge+0.5)]=GREY_EXTEND;
#endif

  *minrow=*maxrow=row;
  *maxcolumn=ceilf(edge);
  *mincolumn=floorf(edge);

  sx=row;
  sxx=row*row;
  sy=edge;
  syy=edge*edge;
  sxy=row*edge;
  n=1;

  // loop down from given row
  LastEdge=edge;

  grad=Grad->pixels+(row+1)*Grad->width;
  grad2=Grad2->pixels+(row+1)*Grad2->width;
  output=Output->pixels+(row+1)*Output->width;

  for (i=row+1;i<=LastRow;i++) {
   // row loop
    ThisEdge=-1;
    for (j=floorf(LastEdge)-1;j<=ceilf(LastEdge)+1;j++) {
      if (abs(grad[j]) > EdgeThreshold)
	if ((grad2[j] < 0 && grad2[j+1] > 0) ||
	    (grad2[j] > 0 && grad2[j+1] < 0) ) {
	  ThisEdge=(float)j-(float)grad2[j]/(float)(grad2[j+1]-grad2[j]);
#ifdef OUTPUT_EXTENDED_EDGES
	  output[(int)floorf(ThisEdge+0.5)]=GREY_EXTEND;
#endif
	  *maxrow=i;
	  if (ThisEdge < *mincolumn)
	    *mincolumn=floorf(ThisEdge);
	  if (ThisEdge > *maxcolumn)
	    *maxcolumn=ceilf(ThisEdge);
	  n++;
	  sx+=i;
	  sxx+=i*i;
	  sy+=ThisEdge;
	  syy+=ThisEdge*ThisEdge;
	  sxy+=i*ThisEdge;
#ifdef DEBUG_EXTENDEDGES
	  fprintf(stderr,"EdgeHExtend: row %d, edge at %5.1f, col in [%d,%d], row in [%d,%d]\n",i,ThisEdge,*mincolumn,*maxcolumn,*minrow,*maxrow);
#endif
	}
    }
    if (ThisEdge==-1)
      break;
    else
      LastEdge=ThisEdge;
    
    grad+=Grad->width;
    grad2+=Grad2->width;
    output+=Output->width;
  }

  // loop up from given row
  LastEdge=edge;

  grad=Grad->pixels+(row-1)*Grad->width;
  grad2=Grad2->pixels+(row-1)*Grad2->width;
  output=Output->pixels+(row-1)*Output->width;

  for (i=row-1;i>=FirstRow;i--) {
   // row loop
    ThisEdge=-1;
    for (j=floorf(LastEdge)-1;j<=ceilf(LastEdge)+1;j++) {
      if (abs(grad[j]) > EdgeThreshold)
	if ((grad2[j] < 0 && grad2[j+1] > 0) ||
	    (grad2[j] > 0 && grad2[j+1] < 0) ) {
	  ThisEdge=(float)j-(float)grad2[j]/(float)(grad2[j+1]-grad2[j]);
#ifdef OUTPUT_EXTENDED_EDGES
	  output[(int)floorf(ThisEdge+0.5)]=GREY_EXTEND;
#endif
	  *minrow=i;
	  n++;
	  sx+=i;
	  sxx+=i*i;
	  sy+=ThisEdge;
	  syy+=ThisEdge*ThisEdge;
	  sxy+=i*ThisEdge;
#ifdef DEBUG_EXTENDEDGES
	  fprintf(stderr,"EdgeHExtend: row %d, edge at %5.1f, col in [%d,%d], row in [%d,%d]\n",i,ThisEdge,*mincolumn,*maxcolumn,*minrow,*maxrow);
#endif
	}
    }
    if (ThisEdge==-1)
      break;
    else
      LastEdge=ThisEdge;
    
    grad-=Grad->width;
    grad2-=Grad2->width;
    output-=Output->width;
  }

  *m=(n*sxy-sx*sy)/(n*sxx-sx*sx);
  *b=(sxx*sy-sx*sxy)/(n*sxx-sx*sx);

#ifdef DEBUG_FINAL_LINE
  fprintf(stderr,"EdgeVExtend: col = %8.3f\trow + %8.3f\t(%d points)\n",*m,*b,n);
#endif

#ifdef DEBUG_EXTEND_TIMER
  gettimeofday(&t1,NULL);
  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);  
  fprintf(stderr,"EdgeVExtend: time elapsed %7.3fms\n",1000*dt);
#endif
}


///////////////////////////////////////////////////////////////
// Find line by extending on edge horizontally
///////////////////////////////////////////////////////////////
void EdgeHExtend(
   FT_16GrayImage_T *Grad,     // vertical gradient                     (input)
   FT_16GrayImage_T *Grad2,    // vertical "2nd" gradient               (input)
   FT_16GrayImage_T *Output,   // output image                          (output)
   int FirstColumn,            // region of interest                    (input)
   int FirstRow,
   int LastColumn,
   int LastRow,
   int EdgeThreshold,          // edge detection threhold               (input)
   int   column,               // column of horizontal edge to extend   (input)
   float edge,                 // horizontal edge (row) to extend       (input)
   float *m,                   // slope of horizontal edge              (output)
   float *b,                   // origin of horizontal edge             (output)
   int *minrow,                // row where edge starts                 (output)
   int *maxrow,                // row where edge ends                   (output)
   int *mincolumn,             // column where edge starts              (output)
   int *maxcolumn              // column where edge ends                (output)
   )
{
  // loop indexes
  int i;
  register int j;
  register int imwidth;

  // pointers to scan image
  register signed short *grad,*grad2,*output;

  // edges
  float ThisEdge,LastEdge;
  
  // interpolation
  float sx,sy,sxx,sxy,syy;
  int n;

#ifdef DEBUG_EXTEND_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;

  gettimeofday(&t0,NULL);
#endif

  // check region
  if (FirstRow<0) FirstRow=0;
  if (FirstColumn<0) FirstColumn=0;
  if (LastRow>=Grad->height) LastRow=Grad->height-1;
  if (LastColumn>=Grad->width) LastColumn=Grad->width-1;

  // Input/Output image width
  imwidth=Grad->width;

  // adjust output size
  Output->width=imwidth;
  Output->height=Grad->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=imwidth*imwidth*sizeof(signed short);

  // place initial edge in the image
#ifdef OUTPUT_EXTENDED_EDGES
  Output->pixels[column+(int)floorf(edge+0.5)*imwidth]=GREY_EXTEND;
#endif

  *maxcolumn=*mincolumn=column;
  *maxrow=ceilf(edge);
  *minrow=floorf(edge);

  sx=column;
  sxx=column*column;
  sy=edge;
  syy=edge*edge;
  sxy=column*edge;
  n=1;

  // loop right from given columns
  LastEdge=edge;

  grad=Grad->pixels+(column+1);
  grad2=Grad2->pixels+(column+1);
  output=Output->pixels+(column+1);

  for (i=column+1;i<=LastColumn;i++) {
   // column loop
    ThisEdge=-1;
    for (j=floorf(LastEdge)-1;j<=ceilf(LastEdge)+1;j++) {
      if (abs(grad[j*imwidth]) > EdgeThreshold)
	if ((grad2[j*imwidth] < 0 && grad2[(j+1)*imwidth] > 0) ||
	    (grad2[j*imwidth] > 0 && grad2[(j+1)*imwidth] < 0) ) {
	  ThisEdge=(float)j-(float)grad2[j*imwidth]/(float)(grad2[(j+1)*imwidth]-grad2[j*imwidth]);
#ifdef OUTPUT_EXTENDED_EDGES
	  output[(int)floorf(ThisEdge+0.5)*imwidth]=GREY_EXTEND;
#endif
	  *maxcolumn=i;
	  if (ThisEdge < *minrow)
	    *minrow=floorf(ThisEdge);
	  if (ThisEdge > *maxrow)
	    *maxrow=ceilf(ThisEdge);
	  n++;
	  sx+=i;
	  sxx+=i*i;
	  sy+=ThisEdge;
	  syy+=ThisEdge*ThisEdge;
	  sxy+=i*ThisEdge;
#ifdef DEBUG_EXTENDEDGES
	  fprintf(stderr,"EdgeHExtend: col %d, edge at %5.1f, col in [%d,%d], row in [%d,%d]\n",i,ThisEdge,*mincolumn,*maxcolumn,*minrow,*maxrow);
#endif
	}
    }
    if (ThisEdge==-1)
      break;
    else
      LastEdge=ThisEdge;
    
    grad++;
    grad2++;
    output++;
  }

  // loop left from given column
  LastEdge=edge;

  grad=Grad->pixels+(column-1);
  grad2=Grad2->pixels+(column-1);
  output=Output->pixels+(column-1);

  for (i=column-1;i>=FirstColumn;i--) {
   // column loop
    ThisEdge=-1;
    for (j=floorf(LastEdge)-1;j<=ceilf(LastEdge)+1;j++) {
      if (abs(grad[j*imwidth]) > EdgeThreshold)
	if ((grad2[j*imwidth] < 0 && grad2[(j+1)*imwidth] > 0) ||
	    (grad2[j*imwidth] > 0 && grad2[(j+1)*imwidth] < 0) ) {
	  ThisEdge=(float)j-(float)grad2[j*imwidth]/(float)(grad2[(j+1)*imwidth]-grad2[j*imwidth]);
#ifdef OUTPUT_EXTENDED_EDGES
	  output[(int)floorf(ThisEdge+0.5)*imwidth]=GREY_EXTEND;
#endif
	  *mincolumn=i;
	  n++;
	  sx+=i;
	  sxx+=i*i;
	  sy+=ThisEdge;
	  syy+=ThisEdge*ThisEdge;
	  sxy+=i*ThisEdge;
#ifdef DEBUG_EXTENDEDGES
	  fprintf(stderr,"EdgeHExtend: col %d, edge at %5.1f, col in [%d,%d], row in [%d,%d]\n",i,ThisEdge,*mincolumn,*maxcolumn,*minrow,*maxrow);
#endif
	}
    }
    if (ThisEdge==-1)
      break;
    else
      LastEdge=ThisEdge;
    
    grad--;
    grad2--;
    output--;
  }

  *m=(n*sxy-sx*sy)/(n*sxx-sx*sx);
  *b=(sxx*sy-sx*sxy)/(n*sxx-sx*sx);

#ifdef DEBUG_FINAL_LINE
  fprintf(stderr,"EdgeHExtend: col = %8.3f\trow + %8.3f\t(%d points)\n",*m,*b,n);
#endif

#ifdef DEBUG_EXTEND_TIMER
  gettimeofday(&t1,NULL);
  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);
  fprintf(stderr,"EdgeHExtend: time elapsed %7.3fms\n",1000*dt);
#endif
}


///////////////////////////////////////////////////////////////
// Horizontal Edge/Cross-ratio Detection
///////////////////////////////////////////////////////////////
int HEdgeCross(
   FT_16GrayImage_T *Grad,     // horizontal gradient                   (input)
   FT_16GrayImage_T *Grad2,    // horizontal gradient                   (input)
   FT_16GrayImage_T *Output,   // output image                          (output)
   int FirstColumn,            // region of interest                    (input)
   int FirstRow,
   int LastColumn,
   int LastRow,
   int EdgeThreshold,          // edge detection threhold               (input)
   float HCross,               // horizontal cross-ratio                (input)
   float HCrossThreshold,      // horizontal cross-ratio threshold      (input)
   int   *bestrow,             // row of vertical edges                 (output)
   float *bestedges            // vertical edges (array of 4)           (output)
   )
{
  // loop indexes
  int i;
  register int j;

  // previous edge positions (must be + - + -), position = -1 means undefined
  float edges[4];

  // cross ratio
  float cr,bestmatch=HUGE_VAL;

  // pointers to scan image
  register signed short *grad,*grad2,*output;

#ifdef DEBUG_SINGLE_CROSS_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;

  gettimeofday(&t0,NULL);
#endif

  // check region
  if (FirstRow<0) FirstRow=0;
  if (FirstColumn<0) FirstColumn=0;
  if (LastRow>=Grad->height) LastRow=Grad->height-1;
  if (LastColumn>=Grad->width) LastColumn=Grad->width-1;

  // adjust output size
  Output->width=Grad->width;
  Output->height=Grad->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=Output->width*Output->width*sizeof(signed short);

  // set pointers
  grad=Grad->pixels+FirstRow*Grad->width;
  grad2=Grad2->pixels+FirstRow*Grad2->width;
  output=Output->pixels+FirstRow*Output->width;

  for (i=FirstRow;i<=LastRow;i++) {
    // start with all edges undefined
    edges[0]=edges[1]=edges[2]=edges[3]=-1;

    // row loop
    for (j=FirstColumn;j<=LastColumn-1;j++) {
      if (abs(grad[j]) > EdgeThreshold)
	// plus edge
	if (grad2[j] < 0 && grad2[j+1] > 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[j]=GREY_PLUS_EDGE;
#endif
	  // save in edges array
	  edges[0]=edges[2];
	  edges[1]=edges[3];
	  edges[2]=(float)j-(float)grad2[j]/(float)(grad2[j+1]-grad2[j]);
	  edges[3]=-1; // ready to receive next -1
#ifdef DEBUG_EDGES
	  fprintf(stderr,"HEdgeCross: plus  edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		  edges[2],edges[0],edges[1],edges[2],edges[3]);
#endif
	}
      // minus edge
	else if (grad2[j] > 0 && grad2[j+1] < 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[j]=GREY_MINUS_EDGE;
#endif
	  // save in edges array
	  if (edges[3]==-1) {
	    edges[3]=(float)j+(float)grad2[j]/(float)(grad2[j]-grad2[j+1]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"HEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	    if (edges[0]!=-1 && edges[1]!=-1 && edges[2]!=-1) {
	      cr=(edges[0]-edges[1])*(edges[2]-edges[3])/(edges[0]-edges[2])/(edges[1]-edges[3]);
	      if (fabsf(cr-HCross)<=HCrossThreshold) {
#ifdef OUTPUT_CORRECT_CR
		output[(int)edges[0]]=output[(int)edges[1]]=output[(int)edges[2]]=output[(int)edges[3]]=GREY_CORRECT_CR;
#endif
		if (fabsf(cr-HCross)<bestmatch){
		  bestmatch=fabsf(cr-HCross);
		  *bestrow=i;
		  memcpy(bestedges,edges,sizeof(edges));
#ifdef DEBUG_BEST_CORRECT_CROSS_RATIO
		fprintf(stderr,"HEdgeCross:  **BEST           c-r=%5.3f, row %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		}
		else {
#ifdef DEBUG_CORRECT_CROSS_RATIO
		fprintf(stderr,"HEdgeCross: **correct c-r=%5.3f, row %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		}
	      } else {
#ifdef OUTPUT_WRONG_CR
		output[j]=GREY_WRONG_CR;
#endif
#ifdef DEBUG_CROSS_RATIO
		fprintf(stderr,"HEdgeCross: --valid c-rat %5.3f, row %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
	      }
	    }
	  } else {
	    // two consecutive minus cannot be part of cross ratio
	    edges[0]=edges[1]=edges[2]=-1;
	    edges[3]=(float)j+(float)grad2[j]/(grad2[j]-grad2[j-1]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"HEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	  }

	}
    }
    grad+=Grad->width;
    grad2+=Grad2->width;
    output+=Output->width;
  }

#ifdef DEBUG_SINGLE_CROSS_TIMER
  gettimeofday(&t1,NULL);
  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);
  fprintf(stderr,"HEdgeCross: time elapsed %7.3fms\n",1000*dt);
#endif

  return (bestmatch==HUGE_VAL);
}


///////////////////////////////////////////////////////////////
// Vertical Edge/Cross-ratio Detection
///////////////////////////////////////////////////////////////
int VEdgeCross(
   FT_16GrayImage_T *Grad,     // vertical gradient                     (input)
   FT_16GrayImage_T *Grad2,    // vertical "2nd" gradient               (input)
   FT_16GrayImage_T *Output,   // output image                          (output)
   int FirstColumn,            // region of interest
   int FirstRow,
   int LastColumn,
   int LastRow,
   int EdgeThreshold,          // edge detection threhold               (input)
   float VCross,               // vertical cross-ratio                  (input)
   float VCrossThreshold,      // vertical cross-ratio threshold        (input)
   int   *bestcolumn,          // column of vertical edges              (output)
   float *bestedges            // horizontal edges (array of 4)         (output)
   )
{
  // loop indexes
  int i;
  register int j;
  register int imwidth;

  // previous edge positions (must be + - + -), position = -1 means undefined
  float edges[4];

  // cross ratio
  float cr,bestmatch=HUGE_VAL;

  // pointers to scan image
  register signed short *grad,*grad2,*output;

#ifdef DEBUG_SINGLE_CROSS_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;

  gettimeofday(&t0,NULL);
#endif

  // check region
  if (FirstRow<0) FirstRow=0;
  if (FirstColumn<0) FirstColumn=0;
  if (LastRow>=Grad->height) LastRow=Grad->height-1;
  if (LastColumn>=Grad->width) LastColumn=Grad->width-1;

  // Input/Output image width
  imwidth=Grad->width;

  // adjust output size
  Output->width=imwidth;
  Output->height=Grad->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=imwidth*imwidth*sizeof(signed short);

  // set pointers
  grad=Grad->pixels+FirstColumn;
  grad2=Grad2->pixels+FirstColumn;
  output=Output->pixels+FirstColumn;

  for (i=FirstColumn;i<=LastColumn;i++) {
    // start with all edges undefined
    edges[0]=edges[1]=edges[2]=edges[3]=-1;

    // row loop
    for (j=FirstRow;j<=LastRow-1;j++) {
      if (abs(grad[j*imwidth]) > EdgeThreshold)
	// plus edge
	if (grad2[j*imwidth] < 0 && grad2[(j+1)*imwidth] > 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[j*imwidth]=GREY_PLUS_EDGE;
#endif
	  // save in edges array
	  edges[0]=edges[2];
	  edges[1]=edges[3];
	  edges[2]=(float)j-(float)grad2[j*imwidth]/(float)(grad2[(j+1)*imwidth]-grad2[j*imwidth]);
	  edges[3]=-1; // ready to receive next -1
#ifdef DEBUG_EDGES
	  fprintf(stderr,"VEdgeCross: plus  edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		  edges[2],edges[0],edges[1],edges[2],edges[3]);
#endif
	}
      // minus edge
	else if (grad2[j*imwidth] > 0 && grad2[(j+1)*imwidth] < 0 ) {
#ifdef OUTPUT_EDGES	  
	  output[j*imwidth]=GREY_MINUS_EDGE;
#endif
	  // save in edges array
	  if (edges[3]==-1) {
	    edges[3]=(float)j+(float)grad2[j*imwidth]/(float)(grad2[j*imwidth]-grad2[(j+1)*imwidth]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"VEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	    if (edges[0]!=-1 && edges[1]!=-1 && edges[2]!=-1) {
	      cr=(edges[0]-edges[1])*(edges[2]-edges[3])/(edges[0]-edges[2])/(edges[1]-edges[3]);
	      if (fabsf(cr-VCross)<=VCrossThreshold) {
		output[(int)edges[0]*imwidth]=output[(int)edges[1]*imwidth]=output[(int)edges[2]*imwidth]=output[(int)edges[3]*imwidth]=GREY_CORRECT_CR;
		if (fabsf(cr-VCross)<bestmatch){
		  bestmatch=fabsf(cr-VCross);
		  *bestcolumn=i;
		  memcpy(bestedges,edges,sizeof(edges));
#ifdef DEBUG_BEST_CORRECT_CROSS_RATIO
		fprintf(stderr,"VEdgeCross:  **BEST &         c-r=%5.3f, col %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		}
		else {
#ifdef DEBUG_CORRECT_CROSS_RATIO
		fprintf(stderr,"VEdgeCross: **correct c-r=%5.3f, col %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
		}
	      } else {
#ifdef OUTPUT_WRONG_CR
		output[j*imwidth]=GREY_WRONG_CR;
#endif
#ifdef DEBUG_CROSS_RATIO
		fprintf(stderr,"VEdgeCross: --valid c-rat %5.3f, col %3d, edges %5.1f %5.1f %5.1f %5.1f\n",
			cr,i,edges[0],edges[1],edges[2],edges[3]);
#endif
	      }
	    }
	  } else {
	    // two consecutive minus cannot be part of cross ratio
	    edges[0]=edges[1]=edges[2]=-1;
	    edges[3]=(float)j+(float)grad2[j*imwidth]/(grad2[j*imwidth]-grad2[j-1]);
#ifdef DEBUG_EDGES
	    fprintf(stderr,"VEdgeCross: minus edge at %5.1f, edges %5.1f %5.1f %5.1f %5.1f\n",
		    edges[3],edges[0],edges[1],edges[2],edges[3]);
#endif
	  }

	}
    }
    grad++;
    grad2++;
    output++;
  }

#ifdef DEBUG_SINGLE_CROSS_TIMER
  gettimeofday(&t1,NULL);
  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);  
  fprintf(stderr,"VEdgeCross: time elapsed %7.3fms\n",1000*dt);
#endif

  return (bestmatch==HUGE_VAL);
}


///////////////////////////////////////////////////////////////
// Horizontal Convolution
///////////////////////////////////////////////////////////////
void HorizConv(
   FT_16GrayImage_T *Input,   // original image                         (input)
   FT_16GrayImage_T *Output,  // output image                           (output)
   int LengthGradDir,               // horizontal length of (half) template   (input)
   int LengthAveDir,               // vertical length of template            (input)
   int FirstColumn,           // region of interest                     (input)
   int FirstRow,
   int LastColumn,
   int LastRow)
{
  int i,imwidth;
  register int j;
  // pointers to scan image
  register signed short *input,*output;
  // previous row
  register signed short *prev_output;
  // before LengthAveDir row
  register signed short *bef_input;

  // corrected first/last row/column
  int fc,fr,lc,lr;

#ifdef DEBUG_CONVOLUTION_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;

  gettimeofday(&t0,NULL);
#endif

  // Input/Output image width
  imwidth=Input->width;

  // adjust output size
  Output->width=imwidth;
  Output->height=Input->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=imwidth*imwidth*sizeof(signed short);

  // adjust size to get convolution correct inside area
  fr = FirstRow    - (LengthAveDir-1);
  lr = LastRow     + (LengthAveDir-1);
  fc = FirstColumn - (2*LengthGradDir-1);
  lc = LastColumn  + (2*LengthGradDir-1);

  if (fr<0) {
    fr=0; }
  if (fc<0) {
    fc=0; }
  if (lr>=Input->height)
    lr=Input->height-1;
  if (lc>=imwidth)
    lc=imwidth-1;

#ifdef DEBUG_HCONVOLUTION
  fprintf(stderr,"Input\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Input->pixels[i*imwidth+j]);
    fprintf(stderr,"\n"); }
#endif

  // set pointers
  input=bef_input=Input->pixels+fr*imwidth;
  output=prev_output=Output->pixels+fr*imwidth;

  // first row

    output[fc]=0;
    for (j=fc;j<fc+LengthGradDir;j++)
      output[fc]+=input[j];
    for (j=fc+1;j<fc+LengthGradDir+1;j++)
      output[j]=output[j-1]-2*input[j-1]+input[j+LengthGradDir-1];
    for (j=fc+LengthGradDir+1;j<=lc-LengthGradDir+1;j++)
      output[j]=output[j-1]+input[j-LengthGradDir-1]-2*input[j-1]+input[j+LengthGradDir-1];
    for (j=lc-LengthGradDir+2;j<=lc;j++)
      output[j]=output[j-1]+input[j-LengthGradDir-1]-2*input[j-1];
    input+=imwidth;
    output+=imwidth;


#ifdef DEBUG_HCONVOLUTION
  fprintf(stderr,"Output 1st row\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n"); }
#endif


  // remaining LengthAveDir-1 row
  for (i=1;i<LengthAveDir;i++) {
    output[fc]=prev_output[fc];
    for (j=fc;j<fc+LengthGradDir;j++)
      output[fc]+=input[j];
    for (j=fc+1;j<fc+LengthGradDir+1;j++)
      output[j]=prev_output[j]-prev_output[j-1]+output[j-1]
	    -2*input[j-1]+input[j+LengthGradDir-1];
    for (j=fc+LengthGradDir+1;j<=lc-LengthGradDir+1;j++)
      output[j]=prev_output[j]-prev_output[j-1]+output[j-1]
	    +input[j-LengthGradDir-1]-2*input[j-1]+input[j+LengthGradDir-1];
    for (j=lc-LengthGradDir+2;j<=lc;j++)
      output[j]=prev_output[j]+output[j-1]+input[j-LengthGradDir-1]-2*input[j-1];

    input+=imwidth;
    output+=imwidth;
    prev_output+=imwidth;
  }

#ifdef DEBUG_HCONVOLUTION
  fprintf(stderr,"Output 2nd set of rows\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n"); }
#endif

  // all other rows
  for (i=fr+LengthAveDir;i<=lr;i++) {
    output[fc]=prev_output[fc];
    for (j=fc;j<fc+LengthGradDir;j++)
      output[fc]+=input[j]-bef_input[j];
    for (j=fc+1;j<fc+LengthGradDir+1;j++)
      output[j]=prev_output[j]-prev_output[j-1]+output[j-1]
	     -2*    input[j-1]+    input[j+LengthGradDir-1]
	     +2*bef_input[j-1]-bef_input[j+LengthGradDir-1];
    for (j=fc+LengthGradDir+1;j<=lc-LengthGradDir+1;j++) {
      output[j]=prev_output[j]-prev_output[j-1]+output[j-1]
	    +    input[j-LengthGradDir-1]-2*    input[j-1]+    input[j+LengthGradDir-1]
	    -bef_input[j-LengthGradDir-1]+2*bef_input[j-1]-bef_input[j+LengthGradDir-1];}
    for (j=lc-LengthGradDir+2;j<=lc;j++)
      output[j]=prev_output[j]-prev_output[j-1]+output[j-1]
	    +    input[j-LengthGradDir-1]-2*    input[j-1]
	    -bef_input[j-LengthGradDir-1]+2*bef_input[j-1];

    input+=imwidth;
    output+=imwidth;
    bef_input+=imwidth;
    prev_output+=imwidth;
  }

#ifdef DEBUG_HCONVOLUTION
  fprintf(stderr,"Output all rows\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n");}
#endif

  // clear wrong edges
  for (i=fr;i<FirstRow;i++)
    for (j=fc;j<=lc;j++)
      Output->pixels[i*imwidth+j]=0;
  for (i=fr;i<=lr;i++)
    for (j=fc;j<FirstColumn;j++)
      Output->pixels[i*imwidth+j]=0;
  for (i=LastRow+1;i<=lr;i++)
    for (j=fc;j<=lc;j++)
      Output->pixels[i*imwidth+j]=0;
  for (i=fr;i<=lr;i++)
    for (j=LastColumn+1;j<=lc;j++)
      Output->pixels[i*imwidth+j]=0;

#ifdef DEBUG_HCONVOLUTION
  fprintf(stderr,"After cleanup \n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n");}
#endif

#ifdef DEBUG_CONVOLUTION_TIMER
  gettimeofday(&t1,NULL);
  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);
  fprintf(stderr,"HorizConv: time elapsed %7.3fms\n",1000*dt);
#endif
}


///////////////////////////////////////////////////////////////
// Vertical Convolution
///////////////////////////////////////////////////////////////
void VertConv(
   FT_16GrayImage_T *Input,   // original image                         (input)
   FT_16GrayImage_T *Output,  // output image                           (output)
   int LengthGradDir,               // vertical length of (half) template     (input)
   int LengthAveDir,               // horizontal length of template          (input)
   int FirstColumn,           // region of interest (input)
   int FirstRow,
   int LastColumn,
   int LastRow)
{
  // pointers to scan image
  register signed short *input,*output;
  // previous row
  register signed short *prev_output;
  // before LengthGradDir row
  register signed short *bef_input;
  int i,jmin,jmax;
  register int j;
  register int imwidth;
  register int vminus,vplus;

  // corrected first/last row/column
  int fc,fr,lc,lr;

#ifdef DEBUG_CONVOLUTION_TIMER
  // timing 
  struct timeval t0,t1;
  float dt;

  gettimeofday(&t0,NULL);
#endif

  // Input/Output image width
  imwidth=Input->width;

  // adjust output size
  Output->width=imwidth;
  Output->height=Input->height;
  Output->type=FT_16BITGREY;
  Output->pixelslength=imwidth*imwidth*sizeof(signed short);

  // adjust size to get convolution correct inside area
  fr = FirstRow    - (2*LengthGradDir-1);
  lr = LastRow     + (2*LengthGradDir-1);
  fc = FirstColumn - (LengthAveDir-1);
  lc = LastColumn  + (LengthAveDir-1);
  if (fr<0) {
    fr=0; }
  if (fc<0) {
    fc=0; }
  if (lr>=Input->height)
    lr=Input->height-1;
  if (lc>=imwidth)
    lc=imwidth-1;

#ifdef DEBUG_VCONVOLUTION
  fprintf(stderr,"Input\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Input->pixels[i*imwidth+j]);
    fprintf(stderr,"\n"); }
#endif

  // set pointers
  input=bef_input=Input->pixels+fc;
  output=prev_output=Output->pixels+fc;

  // first column
  {
    output[fr*imwidth]=0;
    for (j=fr;j<fr+LengthGradDir;j++)
      output[fr*imwidth]+=input[j*imwidth];
    for (j=fr+1;j<fr+LengthGradDir+1;j++)
      output[j*imwidth]=output[(j-1)*imwidth]-2*input[(j-1)*imwidth]+input[(j+LengthGradDir-1)*imwidth];
    for (j=fr+LengthGradDir+1;j<=lr-LengthGradDir+1;j++)
      output[j*imwidth]=output[(j-1)*imwidth]+input[(j-LengthGradDir-1)*imwidth]-2*input[(j-1)*imwidth]+input[(j+LengthGradDir-1)*imwidth];
    for (j=lr-LengthGradDir+2;j<=lr;j++)
      output[j*imwidth]=output[(j-1)*imwidth]+input[(j-LengthGradDir-1)*imwidth]-2*input[(j-1)*imwidth];
    input++;
    output++;
  }

#ifdef DEBUG_VCONVOLUTION
  fprintf(stderr,"Output 1st row\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n"); }
#endif

  // remaining LengthAveDir-1 columns
  for (i=1;i<LengthAveDir;i++) {
    output[fr*imwidth]=prev_output[fr*imwidth];
    for (j=fr;j<fr+LengthGradDir;j++)
      output[fr*imwidth]+=input[j*imwidth];
    for (j=fr+1;j<fr+LengthGradDir+1;j++)
      output[j*imwidth]=prev_output[j*imwidth]-prev_output[(j-1)*imwidth]+output[(j-1)*imwidth]
	    -2*input[(j-1)*imwidth]+input[(j+LengthGradDir-1)*imwidth];
    for (j=fr+LengthGradDir+1;j<=lr-LengthGradDir+1;j++)
      output[j*imwidth]=prev_output[j*imwidth]-prev_output[(j-1)*imwidth]+output[(j-1)*imwidth]
	    +input[(j-LengthGradDir-1)*imwidth]-2*input[(j-1)*imwidth]+input[(j+LengthGradDir-1)*imwidth];
    for (j=lr-LengthGradDir+2;j<=lr;j++)
      output[j*imwidth]=prev_output[j*imwidth]+output[(j-1)*imwidth]+input[(j-LengthGradDir-1)*imwidth]-2*input[(j-1)*imwidth];

    input++;
    output++;
    prev_output++;
  }

#ifdef DEBUG_VCONVOLUTION
  fprintf(stderr,"Output 2nd set of rows\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n"); }
#endif

  // all other columns
  jmin=(fr+LengthGradDir+1)*imwidth;
  jmax=(lr-LengthGradDir+1)*imwidth;
  vminus=(LengthGradDir+1)*imwidth;
  vplus=(LengthGradDir-1)*imwidth;
  for (i=fc+LengthAveDir;i<=lc;i++) {
    output[fr*imwidth]=prev_output[fr*imwidth];
    for (j=fr;j<fr+LengthGradDir;j++)
      output[fr*imwidth]+=input[j*imwidth]-bef_input[j*imwidth];
    for (j=fr+1;j<fr+LengthGradDir+1;j++)
      output[j*imwidth]=prev_output[j*imwidth]-prev_output[(j-1)*imwidth]
	     +output[(j-1)*imwidth]
	     -2*    input[(j-1)*imwidth]+    input[(j+LengthGradDir-1)*imwidth]
	     +2*bef_input[(j-1)*imwidth]-bef_input[(j+LengthGradDir-1)*imwidth];
    for (j=jmin;j<=jmax;j+=imwidth){
      output[j]=prev_output[j]-prev_output[j-imwidth]
	    +output[j-imwidth]
	    +    input[j-vminus]-2*    input[j-imwidth]
	    +    input[j+vplus]
	    -bef_input[j-vminus]+2*bef_input[j-imwidth]
	    -bef_input[j+vplus];}
    for (j=lr-LengthGradDir+2;j<=lr;j++)
      output[j*imwidth]=prev_output[j*imwidth]-prev_output[(j-1)*imwidth]
	    +output[(j-1)*imwidth]
	    +    input[(j-LengthGradDir-1)*imwidth]-2*    input[(j-1)*imwidth]
	    -bef_input[(j-LengthGradDir-1)*imwidth]+2*bef_input[(j-1)*imwidth];

    input++;
    output++;
    bef_input++;
    prev_output++;
  }

#ifdef DEBUG_VCONVOLUTION
  fprintf(stderr,"Output all rows\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n");}
#endif

  // clear wrong edges
  for (i=fr;i<FirstRow;i++) 
    for (j=fc;j<=lc;j++) 
      Output->pixels[i*imwidth+j]=0;
  for (i=fr;i<=lr;i++)
    for (j=fc;j<FirstColumn;j++)
      Output->pixels[i*imwidth+j]=0;
  for (i=LastRow+1;i<=lr;i++)
    for (j=fc;j<=lc;j++)
      Output->pixels[i*imwidth+j]=0;
  for (i=fr;i<=lr;i++)
    for (j=LastColumn+1;j<=lc;j++)
      Output->pixels[i*imwidth+j]=0;

#ifdef DEBUG_VCONVOLUTION
  fprintf(stderr,"After cleanup\n   ");
  for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
    fprintf(stderr,"%5d",j);
  fprintf(stderr,"\n"); 
  for (i=fr;i<fr+DEBUG_NROWS;i++) {
    fprintf(stderr,"%3d",i);
    for (j=fc;j<fc+DEBUG_NCOLUMNS;j++)
      fprintf(stderr,"%5d",(int)Output->pixels[i*imwidth+j]);
    fprintf(stderr,"\n");}
#endif

#ifdef DEBUG_CONVOLUTION_TIMER
  gettimeofday(&t1,NULL);
  dt=(t1.tv_sec-t0.tv_sec)+.000001*(t1.tv_usec-t0.tv_usec);
  fprintf(stderr,"VertConv: time elapsed %7.3fms\n",1000*dt);
#endif
}

/*************************************************************************/
// Image convertion functions
/*************************************************************************/

///////////////////////////////////////////////////////////////
// Convert any type to 8 bit image
///////////////////////////////////////////////////////////////
void ConvertAnyto8(FT_16GrayImage_T *Input,FT_8GrayImage_T *Output)
{
  switch(Input->type) {
  case FT_4BITGREY:
    Convert4to8((FT_4GrayImage_T *)Input,Output);
    break;
  case FT_16BITGREY:
    Convert16to8((FT_16GrayImage_T *)Input,Output);
    break;
  case FT_8BITRLEGREY:
    Convert8RLEto8((FT_8RLEGrayImage_T *)Input,Output);
    break;
  case FT_8BITGREY:
    memcpy(Output,Input,FT_IMAGESIZE(Input));
    break;
  default:
    fprintf(stderr,"ConvertAnyto8: type %d not (yet) supported\n",Input->type);
    exit(1); 
  }
}

///////////////////////////////////////////////////////////////
// Convert 4 to 8 bit image
///////////////////////////////////////////////////////////////
void Convert4to8(FT_4GrayImage_T *Input,FT_8GrayImage_T *Output)
{
  register unsigned char *input;
  register unsigned char *output;

  register int i,j;

  if (Input->type!=FT_4BITGREY) {
    fprintf(stderr,"Convert4to8: input image is not 4BITGREY, it is %d\n",Input->type);
    exit(1); }

  Output->width=Input->width;
  Output->height=Input->height;
  Output->type=FT_8BITGREY;
  Output->pixelslength=Output->width*Output->height*sizeof(unsigned char);

  input=Input->pixels;
  output=Output->pixels;
  for (i=0;i<Input->height;i++) {
    for (j=0;j<Input->width;j+=2){
      output[j]=input[j>>1] & 0xf0;
      output[j+1]=(input[j>>1] & 0x0f)<<4;
#ifdef DEBUG_ENCODING
      fprintf(stderr,"input[%d]=%x (%lx), output[%d]=%x, output[%d]=%x\n",
	      j>>1,input[j>>1],input+(j>>1),j,output[j],j+1,output[j+1]);
#endif
    }
    input+=Input->width>>1;
    output+=Output->width;
  }
}

///////////////////////////////////////////////////////////////
// Convert 16 to 4 bit image
///////////////////////////////////////////////////////////////
void Convert16to4(FT_16GrayImage_T *Input,FT_4GrayImage_T *Output)
{
  register signed short *input;
  register unsigned char *output;

  register int i,j,mx=1;

  if (Input->type!=FT_16BITGREY) {
    fprintf(stderr,"Convert16to4: input image is not 16BITGREY, it is %d\n",Input->type);
    exit(1); }

  Output->width=Input->width;
  Output->height=Input->height;
  Output->type=FT_4BITGREY;
  Output->pixelslength=Output->width*Output->height*sizeof(unsigned char)/2;

  input=Input->pixels-1;
  for (i=Input->height*Input->width;i>0;i--) {
    if (abs(*++input)>mx)
      mx=abs(*input);
  }
    
  input=Input->pixels;
  output=Output->pixels;
  for (i=0;i<Input->height;i++) {
    for (j=0;j<Input->width;j+=2) {
      output[j>>1]=((abs(input[j])*15/mx)<<4)|(abs(input[j+1])*15/mx);
#ifdef DEBUG_ENCODING
      fprintf(stderr,"input[%d]=%x, input[%d]=%x, output[%d]=%x (%lx)\n",
	      j,abs(input[j])*15/mx,j+1,abs(input[j+1])*15/mx,
	      j>>1,output[j>>1],output+(j>>1));
#endif
    }
    input+=Input->width;
    output+=Output->width>>1;
  }
}

///////////////////////////////////////////////////////////////
// Convert 16 to 8 bit image
///////////////////////////////////////////////////////////////
void Convert16to8(FT_16GrayImage_T *Input,FT_8GrayImage_T *Output)
{
  register signed short *input;
  register unsigned char *output;

  register int i,j,mx=1;

  if (Input->type!=FT_16BITGREY) {
    fprintf(stderr,"Convert16to8: input image is not 16BITGREY, it is %d\n",Input->type);
    exit(1); }

  Output->width=Input->width;
  Output->height=Input->height;
  Output->type=FT_8BITGREY;
  Output->pixelslength=Output->width*Output->height*sizeof(unsigned char);

  input=Input->pixels-1;
  for (i=Input->height*Input->width;i>0;i--) {
    if (abs(*++input)>mx)
      mx=abs(*input);
  }
  
  input=Input->pixels;
  output=Output->pixels;
  for (i=0;i<Input->height;i++) {
    for (j=0;j<Input->width;j++)
      output[j]=abs(input[j])*255/mx;
    input+=Input->width;
    output+=Output->width;
  }
  
}

///////////////////////////////////////////////////////////////
// Convert 16 to RLE 8bit image
///////////////////////////////////////////////////////////////
void Convert16to8RLE(FT_16GrayImage_T *Input,FT_8RLEGrayImage_T *Output,
		     int threshold)
{
  register signed short *input;
  register unsigned char *output;

  register int i,mx=1;

  register int thisvalue,rlevalue,sum,length;
  int rlelength;

  if (Input->type!=FT_16BITGREY) {
    fprintf(stderr,"Convert16to8RLE: input image is not 16BITGREY, it is %d\n",Input->type);
    exit(1); }

  Output->width=Input->width;
  Output->height=Input->height;
  Output->type=FT_8BITRLEGREY;

  input=Input->pixels-1;
  for (i=Input->height*Input->width;i>0;i--) {
    if (abs(*++input)>mx)
      mx=abs(*input);
  }
  
  input=Input->pixels;
  output=Output->pixels;
  
  rlelength=0;
  rlevalue=abs(input[0])*255/mx;
  length=sum=0;
  for (i=Input->height*Input->width;i>0;i--) {
    thisvalue=abs(*input++)*255/mx;
    if (abs(thisvalue-rlevalue)<=threshold && length<255){
      sum+=thisvalue;
      length++;
#ifdef DEBUG_RLE
      fprintf(stderr,"extending: rle=%d, this=%d, thres=%d, ave=%4d length=%4d\n",rlevalue,thisvalue,threshold,sum/length,length);
#endif
    } else {
      rlelength += 2;
      if (rlelength>FT_MAX_RLELENGHT) {
	fprintf(stderr,"Convert16to8RLE: maximum RLE length exceeded (%d>%d)\n",rlelength,FT_MAX_RLELENGHT);
	exit(1); }
      *output++ = sum/length;  // average value
      *output++ = length;
#ifdef DEBUG_RLE
      fprintf(stderr,"new output: (%3d,%3d)\n",output[-2],output[-1]);
#endif
      sum=rlevalue=thisvalue;
      length=1;
    }
  }
  Output->pixelslength=rlelength;

#ifdef DEBUG_ENCODING
  input=Input->pixels;
  output=Output->pixels;
  fprintf(stderr,"Convert16to8RLE: 8Bit\n");
  for (i=0;i<DEBUG_NCOLUMNS*DEBUG_NROWS;i++) 
    fprintf(stderr,"%4d ",(int)abs(*input++)*255/mx);
  fprintf(stderr,"\nConvert16to8RLE: 8BitRLE\n");
  for (i=0;i<DEBUG_NCOLUMNS;i++) 
    fprintf(stderr,"(%3d,%3d) ",(int)*output++,(int)*output++);
  fprintf(stderr,"\n");
#endif
}

///////////////////////////////////////////////////////////////
// Convert RLE 8bit image to 8bit gray
///////////////////////////////////////////////////////////////
void Convert8RLEto8(FT_8RLEGrayImage_T *Input,FT_8GrayImage_T *Output)
{
  register unsigned char *input;
  register unsigned char *output;

  register int i,mx=1;

  register int thisvalue,rlevalue,sum,length;
  int rlelength;

  if (Input->type!=FT_8BITRLEGREY) {
    fprintf(stderr,"Convert8RLEto8: input image is not 8BITRLEGREY, it is %d\n",Input->type);
    exit(1); }

  Output->width=Input->width;
  Output->height=Input->height;
  Output->type=FT_8BITGREY;
  Output->pixelslength=Output->width*Output->height*sizeof(unsigned char);
  
  input=Input->pixels;
  output=Output->pixels;

  for (i=Input->pixelslength;i>0;i--) {
    memset(output,input[0],input[1]);
    output+=input[1];
    input+=2;
  }
  
}


/*************************************************************************/
// X windows functions
/*************************************************************************/


///////////////////////////////////////////////////////////////
// X windows redraw function
///////////////////////////////////////////////////////////////
gboolean on_drawarea_expose(GtkWidget *widget,
		 GdkEventExpose *event,
		 FT_8GrayImage_T *Gray8Image)
{
  //  printf("on_drawarea_expose(widget=0x%08x, event=0x%08x, Image=0x%08x)\n",widget,event,Gray8Image);
  gdk_draw_gray_image(widget->window, widget->style->fg_gc[GTK_STATE_NORMAL],
		      0, 0, Gray8Image->width, Gray8Image->height,
		      GDK_RGB_DITHER_MAX, Gray8Image->pixels, Gray8Image->width);
}

///////////////////////////////////////////////////////////////
// Process X windows redraw events
///////////////////////////////////////////////////////////////
void ProcessXEvents()
{
    while(gtk_events_pending()) 
      gtk_main_iteration();
}


///////////////////////////////////////////////////////////////
// Display image on a new X window
///////////////////////////////////////////////////////////////
GtkWidget *DisplayNewImage(FT_8GrayImage_T *Gray8Image,char *windowname)
{
  // create window
  
  GtkWidget *window,
            *drawarea;

  window=gtk_window_new(GTK_WINDOW_TOPLEVEL);
  gtk_window_set_title((GtkWindow*)window,windowname);
  drawarea = gtk_drawing_area_new();
  gtk_drawing_area_size(GTK_DRAWING_AREA(drawarea), 
			Gray8Image->width,Gray8Image->height);
  gtk_container_add(GTK_CONTAINER(window), drawarea);
  gtk_signal_connect(GTK_OBJECT(drawarea), "expose-event",
		     GTK_SIGNAL_FUNC(on_drawarea_expose), Gray8Image);
  gtk_widget_show_all(window);
  
  return drawarea;
}

///////////////////////////////////////////////////////////////
// Ask X to redraw the image
///////////////////////////////////////////////////////////////
void UpdateImageDisplay(GtkWidget *drawarea)
{
  gtk_widget_queue_draw(drawarea);
  ProcessXEvents();
}

/*************************************************************************/
// Load & Save image functions
/*************************************************************************/

///////////////////////////////////////////////////////////////
// Load Image from file
///////////////////////////////////////////////////////////////
void LoadImage(char *filename,FT_16GrayImage_T *Output)
{
  int i;
  register int j;
  GdkPixbuf *pixbuf;
  register unsigned char *input;
  register signed short *output;
  int mx,mn;

  // load image from file (automatically detects type)
#ifdef GDK20
  pixbuf=gdk_pixbuf_new_from_file(filename,NULL);
#else
  pixbuf=gdk_pixbuf_new_from_file(filename);
#endif
  if (!pixbuf) {
    fprintf(stderr,"LoadImage : Error reading file\n");
    exit(1);
  }

  /* Output some information about the image
  fprintf(stderr,"filename \"%s\", width = %d, height = %d, rowstride = %d, n_channels = %d, bits_per_sample = %d, \n",filename,
	  gdk_pixbuf_get_width(pixbuf),gdk_pixbuf_get_height(pixbuf),
	  gdk_pixbuf_get_rowstride(pixbuf),
	  gdk_pixbuf_get_n_channels(pixbuf),
	  gdk_pixbuf_get_bits_per_sample(pixbuf));
  */

  // Convert to FT_ format
  Output->width=gdk_pixbuf_get_width(pixbuf);
  Output->height=gdk_pixbuf_get_height(pixbuf);
  Output->type=FT_16BITGREY;
  Output->pixelslength=Output->width*Output->height*sizeof(signed short);

  if (Output->width>FT_MAX_IMAGEWIDTH || Output->height>FT_MAX_IMAGEHEIGHT) {
    fprintf(stderr,
	    "filename \"%s\" too large: width = %d (%d), height = %d (%d)\n",
	    filename,
	    Output->width,FT_MAX_IMAGEWIDTH,
	    Output->height,FT_MAX_IMAGEHEIGHT);
    exit(1);
  }

  input=gdk_pixbuf_get_pixels(pixbuf);
  output=Output->pixels;

  mx=mn=input[0];

  for (i=0;i<Output->height;i++) {
    for (j=0;j<Output->width;j++) {
      output[j]=input[j*gdk_pixbuf_get_n_channels(pixbuf)];
      //      output[j]=output[j]>0;
      if (output[j]>mx)
	mx=output[j];
      if (output[j]<mn)
	mn=output[j];}

    output+=Output->width;
    input+=gdk_pixbuf_get_rowstride(pixbuf); }

  /*
  fprintf(stderr,"LoadImage: max = %d, min = %d\n",mx,mn);
  */
}


void SavePGM8(char *filename, FT_8GrayImage_T *Image)
{
  int fd;
  char str[100];

  if (Image->type!=FT_8BITGREY) {
    fprintf(stderr,"SavePGM8: image is not 8BITGREY, it is %d\n",Image->type);
    exit(1); }

  if ((fd=open(filename,O_CREAT|O_TRUNC|O_RDWR,S_IRUSR|S_IWUSR))<0) {
    perror("SavePGM8");
    exit(1); }

  sprintf(str,"P5 %d %d 255\n",Image->width,Image->height);
  write(fd,str,strlen(str));
  write(fd, &(Image->pixels), Image->pixelslength);
  close(fd);
}

#ifndef NOV4L2

/*************************************************************************/
// Video4Linux2 functions
/*************************************************************************/

///////////////////////////////////////////////////////////////
// Global variables for stream capture
///////////////////////////////////////////////////////////////
struct streamCaptureBuffer_S *streamCaptureBuffers = NULL;
static unsigned int nStreamCaptureBuffers       = 0;
int    streamCaptureWidth=0,streamCaptureHeight=0;
int    streamCapturePixelFormat=0;

///////////////////////////////////////////////////////////////
// Initialize V4L2 device and starts stream capture
///////////////////////////////////////////////////////////////
int v4l2Init(
  char* deviceName,           // device name, typically "/dev/video0" (input)
  int videoSource,            // index of the video source (0)        (input)
  v4l2_std_id videoStandard,  // video standard (V4L2_STD_NTSC_M)     (input)
  int pixelFormat,            // desired pixel format                 (input)
             // http://v4l2spec.bytesex.org/spec-single/v4l2.html#RGB-FORMATS
  enum v4l2_field fieldOrder, // desired field order                  (input)
              // http://v4l2spec.bytesex.org/spec-single/v4l2.html#V4L2-FIELD
  int width,                  // window capture width                 (input)
  int height,                 // window capture height                (input)
  int nBuffers                // desired # of buffers for stream capture (input)
 )
{
  int fd;

  // Open device
  if((fd=open(deviceName, O_RDWR|O_NONBLOCK))<0) {
    perror("v4l2Init: open(device)");
    exit(1);
  }

  // Make sure device can capture in streaming mode
  struct v4l2_capability cap;
  if(ioctl(fd, VIDIOC_QUERYCAP, &cap)<0) {
    perror("v4l2Init: ioctl(VIDIOC_QUERYCAP)");
    exit(1); }
  if (! cap.capabilities&V4L2_CAP_VIDEO_CAPTURE) {
    fprintf(stderr,"v4l2Init: ioctl(VIDIOC_QUERYCAP): device cannot capture video\n");
    exit(1); }
  if (! cap.capabilities&V4L2_CAP_STREAMING) {
    fprintf(stderr,"v4l2Init: ioctl(VIDIOC_QUERYCAP): device cannot capture video as stream\n");
    exit(1); }

  // Set video standard
  if (ioctl(fd, VIDIOC_S_STD,&videoStandard)<0) {
    perror ("v4l2Init: VIDIOC_S_STD");
    exit(1); }

  // Select input source
  if (ioctl(fd, VIDIOC_S_INPUT, &videoSource)<0) {
    perror ("v4l2Init: VIDIOC_S_INPUT");
    exit(1); }

  // Set data format
  struct v4l2_format fmt;
  CLEAR (fmt);
  fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width       = streamCaptureWidth = width; 
  fmt.fmt.pix.height      = streamCaptureHeight = height;
  fmt.fmt.pix.pixelformat = streamCapturePixelFormat = pixelFormat;
  fmt.fmt.pix.field       = fieldOrder;   
  if (ioctl(fd, VIDIOC_S_FMT, &fmt)<0) {
    perror("v4l2Init: VIDIOC_S_FMT");
    exit(1); }
  
  // Make sure format was set
  if (ioctl(fd, VIDIOC_G_FMT, &fmt)<0) {
    perror("v4l2Init: VIDIOC_G_FMT");
    exit(1); }
  if (fmt.fmt.pix.width!=width || fmt.fmt.pix.height!=height ||
      fmt.fmt.pix.pixelformat!=pixelFormat || fmt.fmt.pix.field!=fieldOrder) {
    fprintf(stderr,"v4l2Init: VIDIOC_G_FMT: failed\n");
    fprintf(stderr,"\t width       : desired %d, current %d\n",width,fmt.fmt.pix.width);
    fprintf(stderr,"\t height      : desired %d, current %d)\n",height,fmt.fmt.pix.height);
    fprintf(stderr,"\t pixel format: desired 0x%x, current 0x%x)\n",pixelFormat,fmt.fmt.pix.pixelformat);
    fprintf(stderr,"\t field order : desired 0x%x, current 0x%x)\n",fieldOrder,fmt.fmt.pix.field);
    exit(1); }
  
  // Set crop to default values (if device supports it)
  struct v4l2_cropcap cropcap;
  struct v4l2_crop crop;
  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(fd,VIDIOC_CROPCAP, &cropcap)<0) {
    perror("v4l2Init: device does not appear to support video cropping\n\tioctl(VIDIOC_CROPCAP) (nonfatal warning)"); }
  else {
    crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    crop.c = cropcap.defrect; /* reset to default */
    if (ioctl(fd, VIDIOC_S_CROP, &crop)<0) {
      perror("v4l2Init: device does not appear to support ioctl(VIDIOC_S_CROP) (nonfatal warning)");}
  }

  // Request buffers for MEMORY_MMAP stream capture
  struct v4l2_requestbuffers req;
  CLEAR (req);
  req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.count  = nBuffers;
  req.memory = V4L2_MEMORY_MMAP;
  if (ioctl(fd, VIDIOC_REQBUFS, &req)<0) {
    perror("v4l2Init: VIDIOC_REQBUFS");
    exit(1); }
  if (req.count < 2) {
    fprintf (stderr, "v4l2Init: VIDIOC_REQBUFS: Insufficient buffer memory on device, only %d buffers available\n",req.count);
    exit(1); }
  
  // Allocate memory for buffer pointers
  if (!(streamCaptureBuffers = calloc (req.count, sizeof (*streamCaptureBuffers)))) {
    perror("v4l2Init: calloc: Out of memory");
    exit(1);}
  
  // Mmap buffers
  for (nStreamCaptureBuffers = 0; nStreamCaptureBuffers < req.count; ++nStreamCaptureBuffers) {
    struct v4l2_buffer buf;
    CLEAR (buf);
    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory      = V4L2_MEMORY_MMAP;
    buf.index       = nStreamCaptureBuffers;
    if (ioctl(fd, VIDIOC_QUERYBUF, &buf)<0) {
      perror("v4l2Init: VIDIOC_QUERYBUF");
      return -1; }
    
    streamCaptureBuffers[nStreamCaptureBuffers].length = buf.length;
    streamCaptureBuffers[nStreamCaptureBuffers].start =
      mmap (NULL /* start anywhere */,
	    buf.length,
	    PROT_READ | PROT_WRITE /* required */,
	    MAP_SHARED /* recommended */,
	    fd, buf.m.offset);
    
    if (MAP_FAILED == streamCaptureBuffers[nStreamCaptureBuffers].start) {
      perror("v4l2Init: mmap failed");
      return -1;
    }
  }

  // Enqueue capturing buffer in driver's incoming queue
  int i;
  for (i = 0; i < nStreamCaptureBuffers; ++i) {
    struct v4l2_buffer buf;
    CLEAR (buf);
    buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index  = i;
    if (ioctl(fd, VIDIOC_QBUF, &buf)<0) {
      perror("v4l2Init: VIDIOC_QBUF");
      exit(1); }
  }
  
  // Start capturing
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(fd, VIDIOC_STREAMON, &type)<0) {
    perror("v4l2Init: VIDIOC_STREAMON");
    exit(1); }
  return fd;
}

///////////////////////////////////////////////////////////////
// Set image parameters for capture
///////////////////////////////////////////////////////////////
void v4l2SetParameters(
     int fd,          // device file handle for ioctl
     int brightness,  // desired brightness, i.e., the black level
     int contrast,    // desired brightness, i.e., luma gain
     int saturation,  // desired color saturation, i.e., chroma gain
     int hue)         // desired hue or color balance
                      // all parameters from 0..65535
{
  // Stop capturing (aborts current DMAs and removes all buffers from queues)
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(fd, VIDIOC_STREAMOFF, &type)<0) {
    perror("VIDIOC_STREAMOFF");
    exit(1); }

  // Change controls 
  struct v4l2_control ctrl;
  ctrl.id=V4L2_CID_BRIGHTNESS;
  ctrl.value=brightness;
  if (ioctl(fd, VIDIOC_S_CTRL, &ctrl)<0) {
    perror ("VIDIOC_S_CTRL");
    exit(1); }
  ctrl.id=V4L2_CID_CONTRAST;
  ctrl.value=contrast;
  if (ioctl(fd, VIDIOC_S_CTRL, &ctrl)<0) {
    perror ("VIDIOC_S_CTRL");
    exit(1); }
  ctrl.id=V4L2_CID_SATURATION;
  ctrl.value=saturation;
  if (ioctl(fd, VIDIOC_S_CTRL, &ctrl)<0) {
    perror ("VIDIOC_S_CTRL");
    exit(1); }
  ctrl.id=V4L2_CID_HUE;
  ctrl.value=hue;
  if (ioctl(fd, VIDIOC_S_CTRL, &ctrl)<0) {
    perror ("VIDIOC_S_CTRL");
    exit(1); }

  // Enqueue capturing buffer in driver's incoming queue
  int i;
  for (i = 0; i < nStreamCaptureBuffers; ++i) {
    struct v4l2_buffer buf;
    CLEAR (buf);
    buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index  = i;
    if (ioctl(fd, VIDIOC_QBUF, &buf)<0) {
      perror("VIDIOC_QBUF");
      exit(1); }
  }
  
  // Start capturing
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(fd, VIDIOC_STREAMON, &type)<0) {
    perror("VIDIOC_STREAMON");
    exit(1); }
}

///////////////////////////////////////////////////////////////
// Get an whole image from a v4l2 device 
// (returns the index of the buffer read or <0 if no image was read)
///////////////////////////////////////////////////////////////
int v4l2GetImage(
  int fd,                   // device file handle for ioctl            (input)
  FT_16GrayImage_T *Output, // image buffer to be filled               (output)
  struct timeval *tstamp,   // time stamp with start-of-capture time   (output)
  int *sequence,            // frame sequence number                   (output)
  struct timeval *timeout,  // timeout for waiting for new frame (input/output)
                            // outputs time left before timeout
  int *mnptr,               // minimum grey level (0..255)             (output)
                            // not computed if NULL (saves time)
  int *mxptr,               // maximum grey level (0..255)             (output)
                            // not computed if NULL (which saves time)
  int lastPicFlag)          // when nonzero, gets the most recent      (input) 
                            //               image available
                            // when zero, gets the oldest image not yet read
{
  int rc;
  struct timeval timeZero;
  timeZero.tv_sec = 0; timeZero.tv_usec = 0;
  fd_set fdSet;    
    
  FD_ZERO (&fdSet);        // initialize  the  descriptor  set
  FD_SET (fd, &fdSet);     // add the file descriptor fd to the set
  
  // wait for buffer in output queue or timeout
  switch (select(fd + 1, &fdSet, NULL, NULL, timeout)) {
  case 0:	
    perror("v4l2GetImage: select timeout");
    return -1;
  case -1:
    perror("v4l2GetImage: select");
    exit(1);
  }
    
  // dequeue filled buffer from the driver's outgoing queue.
  struct v4l2_buffer buf;
  CLEAR (buf);
  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;
  if (ioctl(fd, VIDIOC_DQBUF, &buf)<0) {
    switch (errno) {
    case EAGAIN:
      return -2;
    default:
      perror("v4l2GetImage: VIDIOC_DQBUF");
      exit(1);
    }
  }

  // Do you want the most recent image?
  if (lastPicFlag) {
    // If so, keep asking for more images until there are no more. 
    // Then use the last one.
     while (1) 
     {
      rc=select(fd + 1, &fdSet, NULL, NULL, &timeZero);
      if (rc==0) {
          break; // we have no more buffers waiting for us
      }
      else if (rc==-1) {
        perror("v4l2GetImage: select 2");
        exit(1);
      }  
    
      // re-queue the previous buffer
      // (let go of the one we're holding)
      if (ioctl(fd, VIDIOC_QBUF, &buf)<0) {
        perror("v4l2GetImage: VIDIOC_QBUF");
        exit(1);
      }
    
      // dequeue filled buffer from the driver's outgoing queue.
      // (grab the next one)
      CLEAR (buf);
      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory = V4L2_MEMORY_MMAP;
      if (ioctl(fd, VIDIOC_DQBUF, &buf)<0) {
        switch (errno) {
        case EAGAIN:
          return -3;
        default:
          perror("v4l2GetImage: VIDIOC_DQBUF");
          exit(1);
          }
       }
    
     }// while loop
  } // "if flag" loop
  
  
  // fill out time stamp output variables
  tstamp->tv_sec=buf.timestamp.tv_sec;
  tstamp->tv_usec=buf.timestamp.tv_usec;
  *sequence=buf.sequence;

  // copy image to Output buffer
  Output->width=streamCaptureWidth;
  Output->height=streamCaptureHeight;
  Output->type=FT_16BITGREY;
  Output->pixelslength=Output->width*Output->height*sizeof(signed short);

  if (Output->width>FT_MAX_IMAGEWIDTH || Output->height>FT_MAX_IMAGEHEIGHT) {
    fprintf(stderr,
	    "v4l2GetImage: image too large: width = %d (%d), height = %d (%d)\n",
	    Output->width,FT_MAX_IMAGEWIDTH,
	    Output->height,FT_MAX_IMAGEHEIGHT);
    exit(1);
  }

  // pointers to scan image
  register unsigned char *input;
  register signed short *output;
  register int i;
  register unsigned short mn,mx;
  
  input=streamCaptureBuffers[buf.index].start;
  output=Output->pixels;

  // scan image and (maybe) compute min and max grey levels
  if (mxptr && mnptr) {
    switch (streamCapturePixelFormat) {
    case V4L2_PIX_FMT_BGR24:
      mx=mn=input[0]+input[1]+input[2];
      for (i=Output->width*Output->height;i>0;i--,input+=3,output++) {
	*output = input[0]+input[1]+input[2]; 
	if (*output>mx)
	  mx=*output;
	if (*output<mn)
	  mn=*output;}
      *mxptr=mx/3;
      *mnptr=mn/3;
      break;
    case V4L2_PIX_FMT_GREY:
      mx=mn=input[0];
      for (i=Output->width*Output->height;i>0;i--,output++) {
	*output=*input++; 
	if (*output>mx)
	  mx=*output;
	if (*output<mn)
	  mn=*output; }
      *mxptr=mx;
      *mnptr=mn;
      break;
    default:
      fprintf(stderr,"v4l2GetImage: unsupported pixels format 0x%x\n",
	      streamCapturePixelFormat);
      exit(1); }
  } else {
    switch (streamCapturePixelFormat) {
    case V4L2_PIX_FMT_BGR24:
      for (i=Output->width*Output->height;i>0;i--,input+=3)
	(*output++) = input[0]+input[1]+input[2]; 
      break;
    case V4L2_PIX_FMT_GREY:
      for (i=Output->width*Output->height;i>0;i--)
	*output++=*input++; 
      break;
    default:
      fprintf(stderr,"v4l2GetImage: unsupported pixels format 0x%x\n",
	      streamCapturePixelFormat);
      exit(1); }
  }    
  
  /* re-queue the buffer */
  if (ioctl(fd, VIDIOC_QBUF, &buf)<0) {
    perror("v4l2GetImage: VIDIOC_QBUF");
    exit(1);}

  return buf.index;
}

///////////////////////////////////////////////////////////////
// Get only a portion of an image from the v4l2 device 
// (returns the index of the buffer read or <0 if no image was read)
///////////////////////////////////////////////////////////////
int v4l2GetSubImage(
  int fd,                   // device file handle for ioctl            (input)
  FT_16GrayImage_T *Output, // Image buffer to be filled               (output)
  struct timeval *tstamp,   // time stamp with start-of-capture time   (output)
  int *sequence,            // frame sequence number                   (output)
  struct timeval *timeout,  // timeout for waiting for new frame (input/output)
                            // outputs time left before timeout
  int FirstColumn,          // region of interest                      (input)
  int FirstRow,
  int LastColumn,
  int LastRow,
  int *mnptr,               // minimum grey level (0..255)             (output)
                            // not computed if NULL (saves time)
  int *mxptr,               // maximum grey level (0..255)             (output)
                            // not computed if NULL (which saves time)
  int lastPicFlag)          // when nonzero, gets the most recent      (input) 
                            //               image available
                            // when zero, gets the oldest image not yet read
{
  int rc;
  struct timeval timeZero;
  timeZero.tv_sec = 0; timeZero.tv_usec = 0;
  fd_set fdSet;    
    
  FD_ZERO (&fdSet);        // initialize  the  descriptor  set
  FD_SET (fd, &fdSet);     // add the file descriptor fd to the set
    
  // wait for buffer in output queue or timeout
  switch (select (fd + 1, &fdSet, NULL, NULL, timeout)) {
  case 0:	
    perror("v4l2GetSubImage: select timeout\n");
    return -1;
  case -1:
    perror("v4l2GetSubImage: select");
    exit(1);
  }
    
  // dequeue filled buffer from the driver's outgoing queue.
  struct v4l2_buffer buf;
  CLEAR (buf);
  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;
  if (ioctl(fd, VIDIOC_DQBUF, &buf)<0) {
    switch (errno) {
    case EAGAIN:
      return -2;
    default:
      perror("v4l2GetSubImage: VIDIOC_DQBUF");
      exit(1);
    }
  }

  // Do you want the most recent image?
  if (lastPicFlag) {
    // If so, keep asking for more images until there are no more. 
    // Then use the last one.
     while (1) 
     {
      rc=select(fd + 1, &fdSet, NULL, NULL, &timeZero);
      if (rc==0) {
          break; // we have no more buffers waiting for us
      }
      else if (rc==-1) {
        perror("select 2");
        exit(1);
      }  
    
      // re-queue the previous buffer
      // (let go of the one we're holding)
      if (ioctl(fd, VIDIOC_QBUF, &buf)<0) {
        perror("v4l2GetSubImage: VIDIOC_QBUF");
        exit(1);
      }
        
      // dequeue filled buffer from the driver's outgoing queue.
      // (grab the next one)
      CLEAR (buf);
      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory = V4L2_MEMORY_MMAP;
      if (ioctl(fd, VIDIOC_DQBUF, &buf)<0) {
        switch (errno) {
        case EAGAIN:
          return -3;
        default:
          perror("v4l2GetSubImage: VIDIOC_DQBUF");
          exit(1);
          }
       }
    
     }// while loop
  } // "if flag" loop
  
  
  // fill out time stamp output variables
  tstamp->tv_sec=buf.timestamp.tv_sec;
  tstamp->tv_usec=buf.timestamp.tv_usec;
  *sequence=buf.sequence;

  // check region
  if (FirstRow<0) FirstRow=0;
  if (FirstColumn<0) FirstColumn=0;
  if (LastRow>=streamCaptureHeight) LastRow=streamCaptureHeight-1;
  if (LastColumn>=streamCaptureWidth) LastColumn=streamCaptureWidth-1;

  // copy image to Output buffer
  Output->width=streamCaptureWidth;
  Output->height=streamCaptureHeight;
  Output->type=FT_16BITGREY;
  Output->pixelslength=Output->width*Output->height*sizeof(signed short);

  if (Output->width>FT_MAX_IMAGEWIDTH || Output->height>FT_MAX_IMAGEHEIGHT) {
    fprintf(stderr,
	    "v4l2GetSubImage: image too large: width = %d (%d), height = %d (%d)\n",
	    Output->width,FT_MAX_IMAGEWIDTH,
	    Output->height,FT_MAX_IMAGEHEIGHT);
    exit(1);
  }

  // pointers to scan image
  register unsigned char *input;
  register signed short *output;
  register unsigned short i,j,mn,mx;
  
  input=streamCaptureBuffers[buf.index].start+FirstRow*streamCaptureWidth;
  output=Output->pixels+FirstRow*streamCaptureWidth;

  // scan image and (maybe) compute min and max grey levels
  if (mxptr && mnptr) {
    switch (streamCapturePixelFormat) {
    case V4L2_PIX_FMT_BGR24:
      mx=mn=input[0]+input[1]+input[2];
      for (i=FirstRow;i<=LastRow;i++) {
	for (j=FirstColumn;j<=LastColumn-1;j++) {
	  output[j] = input[3*j]+input[3*j+1]+input[3*j+2];
	  if (output[j]>mx)
	    mx=output[j];
	  if (output[j]<mn)
	    mn=output[j];}
	output+=Output->width;
	input+=3*streamCaptureWidth; }
      *mxptr=mx/3;
      *mnptr=mn/3;
      break;
    case V4L2_PIX_FMT_GREY:
      mx=mn=input[0];
      for (i=FirstRow;i<=LastRow;i++) {
	for (j=FirstColumn;j<=LastColumn-1;j++) {
	  output[j] = input[j];
	  if (output[j]>mx)
	    mx=output[j];
	  if (output[j]<mn)
	    mn=output[j];}
	output+=Output->width;
	input+=streamCaptureWidth; }
      *mxptr=mx;
      *mnptr=mn;
      break;
    default:
      fprintf(stderr,"v4l2GetSubImage: unsupported pixels format 0x%x\n",
	      streamCapturePixelFormat);
      exit(1); }
  } else {
    switch (streamCapturePixelFormat) {
    case V4L2_PIX_FMT_BGR24:
      for (i=FirstRow;i<=LastRow;i++) {
	for (j=FirstColumn;j<=LastColumn-1;j++)
	  output[j] = input[3*j]+input[3*j+1]+input[3*j+2];
	output+=Output->width;
	input+=3*streamCaptureWidth; }
      break;
    case V4L2_PIX_FMT_GREY:
      for (i=FirstRow;i<=LastRow;i++) {
	for (j=FirstColumn;j<=LastColumn-1;j++)
	  output[j] = input[j];
	output+=Output->width;
	input+=streamCaptureWidth; }
      break;
    default:
      fprintf(stderr,"v4l2GetSubImage: unsupported pixels format 0x%x\n",
	      streamCapturePixelFormat);
      exit(1); }
  }    
  
  /* re-queue the buffer */
  if (ioctl(fd, VIDIOC_QBUF, &buf)<0) {
    perror("v4l2GetSubImage: VIDIOC_QBUF");
    exit(1);}

  return buf.index;
}

///////////////////////////////////////////////////////////////
// Stops stream capture and closes V4L2 device
///////////////////////////////////////////////////////////////
void v4l2End(int fd)  // device file handle for ioctl
{
  // Stop capturing
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(fd, VIDIOC_STREAMOFF, &type)<0) {
    perror("v4l2End: VIDIOC_STREAMOFF");
    exit(1); }

  // Unmap and release buffers
  int i;
  for (i = 0; i < nStreamCaptureBuffers; ++i)
    if (munmap(streamCaptureBuffers[i].start,streamCaptureBuffers[i].length)<0){
      perror("v4l2End: munmap");
      exit(1); }
  free(streamCaptureBuffers); 
  close(fd);
}

#endif

