////////////////////////////////////////////////////////////////////////////
// Fast Track example II
//
// Usage: v4l2FT
//
// This program runs FastTrack on a stream of images captures from a
// Video2Linux2 capture device. Prior to processing the brightness and
// contrast are optimized for best recognition, which typically is
// obtained with low-contrast images.
//
// This program appears in the FastTrack documentation.
//
///////////////////////////////////////////////////////////////////////////
// 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
// 
///////////////////////////////////////////////////////////////////////////

#include "../lib/FastTrack.h"

#define TEMPLATE_LONG_SIZE  3   // in the direction of derivative
#define TEMPLATE_SHORT_SIZE 2   // for averaging

#define EDGE_THRESHOLD 100
#define HORZ_CROSSRATIO .25
#define VERT_CROSSRATIO .25
#define CRATIO_THRESHOLD .02


FT_16GrayImage_T SubImage,Image,HVOutput,VHOutput;
FT_8GrayImage_T  SubImage8,Image8,HVOutput8,VHOutput8;

int main(int argc, char *argv[])
{
  int width=640,height=480;
  float vm[4],vb[4],hm[4],hb[4];
  int vidfd,i,j,mn,mx,rc,videoseq;

  struct timeval timeout,tstamp,t0,t1;
  float dt,dt1,dt2;

  gettimeofday(&t0,NULL);

  ////// initialize gtk /////////////////////////////////////////////
  gtk_init(&argc, &argv);
  gdk_rgb_init();

  // Initialize device and start capture
  vidfd=v4l2Init("/dev/video0",          // device
		 0,                      // source
		 V4L2_STD_NTSC_M,        // standard
		 V4L2_PIX_FMT_GREY,      // pixel format
		 // V4L2_PIX_FMT_BGR24,     // pixel format
		 V4L2_FIELD_INTERLACED,  // field order
		 // V4L2_FIELD_BOTTOM,         // field order
		 width,                  // window capture width
		 height,                 // window capture height
		 4                       // number of buffers
		 );

  ////// auto-adjust of brightness and contrast
  // int contrast=32767,brightness=32767;     // normal contrast
  int contrast=32767/4,brightness=32767*2; // low contrast

  for (i=0;i<100;i++) {
    v4l2SetParameters(vidfd,
		      brightness,    // brightness
		      contrast,      // contrast
		      32767,         // saturation
		      32767);        // hue
    
    timeout.tv_sec = 2;timeout.tv_usec = 0;
    rc=v4l2GetSubImage(vidfd,           // device file handle for ioctl
		       &SubImage,          // Image
		       &tstamp,         // time stamp with start-of-capture time
		       &videoseq,       // frame sequence number
		       &timeout,        // maximum waiting time 
		       width/4,height/4,3*width/4,3*height/4,
		     //0,0,width-1,height-1,
		       &mn,             // minimum grey level
		       &mx,             // maximum grey level
		       1);              // get latest image
    
    if (mx>250)      brightness=brightness/1.05;
    else if (mx<240) brightness=brightness*1.02;
    else break;

    if (brightness>65535) {
      brightness=65535;
      contrast=contrast*1.02; }
  }

  printf("v4l2 Sub Image: min = %d, max = %d (brightness= %5d, contrast = %5d)\n",mn,mx,brightness,contrast);

  ////// v4l2GetImage() ////////////////////////////////////////////////
  for (i=0;i<10;i++) {
    timeout.tv_sec = 2;timeout.tv_usec = 0;
    rc=v4l2GetImage(vidfd,           // device file handle for ioctl
		    &Image,          // Image
		    &tstamp,         // time stamp with start-of-capture time
		    &videoseq,       // frame video sequence number
		    &timeout,        // maximum waiting time 
		    &mn,             // minimum grey level
		    &mx,             // maximum grey level
		    1);              // get latest image
    printf("v4l2 Image: min = %d, max = %d\n",mn,mx);
    
    if (rc<0) fprintf(stderr,"v4l2GetImage: rc = %d\n",rc);
    
    gettimeofday(&t1,NULL);
    dt=1000*(tstamp.tv_sec-t0.tv_sec)+.001*(tstamp.tv_usec-t0.tv_usec);
    dt1=2000-1000*timeout.tv_sec-.001*timeout.tv_usec;
    dt2=1000*(t1.tv_sec-t0.tv_sec)+.001*(t1.tv_usec-t0.tv_usec);
    fprintf(stdout,"rc %d seq %5d tstamp = %6.2fms wait = %6.2fms now = %6.2fms\n",rc,videoseq,dt,dt1,dt2);
    fflush (stdout);
  }
  
  ////// call HVEdgeCross() /////////////////////////////////////////
  fprintf(stderr,"Horizontal-Vertical detection (HVEdgeCross)\n");
  //  memset(HVOutput.pixels,0,Image.pixelslength);
  memcpy(HVOutput.pixels,Image.pixels,Image.pixelslength);
  if (!HVEdgeCross(&Image,&HVOutput,0,0,Image.width-1,Image.height-1,
                   TEMPLATE_LONG_SIZE,TEMPLATE_SHORT_SIZE,EDGE_THRESHOLD,
                   HORZ_CROSSRATIO,CRATIO_THRESHOLD,
                   VERT_CROSSRATIO,CRATIO_THRESHOLD,
                   vm,vb,hm,hb)) {
  } else { fprintf(stderr,"Cross ratio not found!\n"); }

  ////// call VHEdgeCross() ////////////////////////////////////////
  fprintf(stderr,"Vertical-Horizontal detection (VHEdgeCross)\n");
  memset(VHOutput.pixels,0,Image.pixelslength);
  //  memcpy(VHOutput.pixels,Image.pixels,Image.pixelslength);
  if (!VHEdgeCross(&Image,&VHOutput,0,0,Image.width-1,Image.height-1,
                   TEMPLATE_LONG_SIZE,TEMPLATE_SHORT_SIZE,EDGE_THRESHOLD,
                   HORZ_CROSSRATIO,CRATIO_THRESHOLD,
                   VERT_CROSSRATIO,CRATIO_THRESHOLD,
                   vm,vb,hm,hb)) {
  } else { fprintf(stderr,"Cross ratio not found!\n"); }

  Convert16to8(&SubImage,&SubImage8);
  DisplayNewImage(&SubImage8,"Sub Image");

  Convert16to8(&Image,&Image8);
  SavePGM8("frame0.pgm",&Image8);
  DisplayNewImage(&Image8,"Original Image");

  Convert16to8(&HVOutput,&HVOutput8);
  DisplayNewImage(&HVOutput8,"HVEdgeCross (full image)");

  Convert16to8(&VHOutput,&VHOutput8);
  DisplayNewImage(&VHOutput8,"VHEdgeCross (full image)");

  ////// gtk_main() loop ///////////////////////////////////////////
  gtk_main();
}


