////////////////////////////////////////////////////////////////////////////
// Fast Track example III
//
// Usage: 
// 1st start image-display server the remove machine
// 2nd exexute: v4l2FT_NCS
//
// 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.
//
// All images are sent to an image display server. Communication is
// done through the NCS libraries.
//
///////////////////////////////////////////////////////////////////////////
// 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 "FastTrack.h"
#include "NCS.h"

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

#define EDGE_THRESHOLD 100
#define CRATIO_H .25
#define CRATIO_V .25
#define CRATIO_THRESHOLD .02

#define SERVERIP "128.111.56.204"  // IP address of the image-display server

// Function to convert image prior to transmit (compression)
// 8bit encoding for UDP
//#define UDPCONVERT(a,b) Convert16to8(a,b)
// 4bit encoding for UDP
//#define UDPCONVERT(a,b) Convert16to4(a,b)
// 8bit RLE encoding for UDP
#define UDPCONVERT(a,b) Convert16to8RLE(a,b,16)


FT_16GrayImage_T SubImage,Image,HVOutput,VHOutput;
FT_8GrayImage_T  buffer;

int main(int argc, char *argv[])
{
  int width=320,height=240;
  float vm[4],vb[4],hm[4],hb[4];
  int sockfd,vidfd,i,j,mn,mx,rc,videoseq;
  unsigned long packetseq,oldseq;

  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
		 );

  ////// adjust brightness and contrast
  int contrast=32767/4,brightness=32767*2;

  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);

  ////// initialize UDP Socket //////////////////////////////////////
  sockfd=InitSendUDPSocket(SERVERIP,NCS_SEND_PORT);

  ////// v4l2GetImage() ////////////////////////////////////////////////
  oldseq=videoseq;
  for (i=0;i<1000;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
    
    if (rc<0) fprintf(stderr,"v4l2GetImage: rc = %d\n",rc);
    
    if (videoseq!=oldseq+1) {
      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;
      fprintf(stdout,"*** Frame missed: rc %d last seq %5d, this seq %5d tstamp = %6.2fms wait = %6.2fms\n",rc,oldseq,videoseq,dt,dt1);
    }
    oldseq=videoseq;
    
    if ((videoseq & 0x3)==0) {  // select just a few frames
      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);
      
      ////// 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,
		       CRATIO_H,CRATIO_THRESHOLD,
		       CRATIO_V,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,
		       CRATIO_H,CRATIO_THRESHOLD,
		       CRATIO_V,CRATIO_THRESHOLD,
		       vm,vb,hm,hb)) {
      } else { fprintf(stderr,"Cross ratio not found!\n"); }
      */

      ////// Send image ////////////////////////////////////////
      UDPCONVERT(&HVOutput,(void*)&buffer);
      int length=FT_IMAGESIZE(&buffer);
      SendUDPSocket(sockfd,           // socket handle                  (input) 
		    0,                // packet sender id               (input)
		    &packetseq,       // sequence number of last packet (input)
		    &tstamp,          // data time stamp for packet     (input)
		    (char*)&buffer,   // data in packet                 (input)
		    length);          // length of data (in bytes)      (input)
    }
  }
  

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


