#include <termios.h>
#include <stdint.h>
#include <unistd.h>
//#include <stdio.h> 
//#include <stdlib.h>
#include <getopt.h>   
#include <fcntl.h> 
#include <sys/ioctl.h>
#include <linux/types.h>
//#include <string.h>
#include <sys/stat.h>
#include <errno.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>
#include <cstdio>
#include <cstring>

#define PORT "/dev/ttyUSB0"   // Device name
#define SPEED B115200         // I/O speed - B9600 and so on 

 
int fd;                       // Serial handler

int open_and_configure_interface (void)
{struct termios tty;          // Device descriptor
  
 fd = open (PORT, O_RDWR);    // Open device
      
 if (fd < 0)
          {printf ("Error %d opening %s: %s\n", errno, PORT, strerror (errno));
           return (-1);
          }
                                    
 memset (&tty, 0, sizeof tty);
 if (tcgetattr (fd, &tty) != 0)
        {printf("ERROR! %s\n", strerror(errno));
         return (-1);
        }

 cfsetospeed (&tty, SPEED);                      // Set output speed 
 cfsetispeed (&tty, SPEED);                      // Set input speed 
 tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;     // 8-bit chars
 tty.c_iflag &= ~IGNBRK;                         // Disable break processing
 tty.c_lflag = 0;                                // No signaling chars, no echo, no canonical processing
 tty.c_oflag = 0;                                // No remapping, no delays
 tty.c_cc[VMIN]  = 0;                            // Read doesn't block
 tty.c_cc[VTIME] = 5;                            // 0.5 seconds read timeout
 tty.c_iflag &= ~(IXON | IXOFF | IXANY);         // No xon/xoff ctrl
 tty.c_cflag |= (CLOCAL | CREAD);                // Ignore modem controls, enable reading
 tty.c_cflag &= ~(PARENB | PARODD);              // No parity
// tty.c_cflag |= 0;
 tty.c_cflag &= ~CSTOPB;                         // Set bit stop
 tty.c_cflag &= ~CRTSCTS;                        // No gandshake

 if (tcsetattr (fd, TCSANOW, &tty) != 0)
        {printf("ERROR! %s\n", strerror(errno));
         return (-2);
        }
        
 return (0);
}



using namespace std;


int main(int argc, char *argv[]){
  open_and_configure_interface();
  sleep(1);                                       // Kernel bug workaround - see text
 
  CvCapture* capture =0;       

  capture = cvCaptureFromCAM(0);
  if(!capture){
   printf("Capture failure\n");
   return -1;
  }

  IplImage * frame = 0;
  
  char message[12];
  
  frame = cvQueryFrame(capture); 
  
  int oldgreen[frame->height+1][frame->width+1];
  int green[frame->height+1][frame->width+1];
  
  frame = cvQueryFrame(capture); 
	
  for(int i=0; i<frame->height-2; i=i+2)
  {
    for(int j=0; j<frame->width-2; j=j+2)
    {
       oldgreen[i/2][j/2] = frame->imageData[frame->widthStep*i + frame->nChannels* j + 1];				
    }
  }
	 
    
  while(true)
  {  
    next:
    frame = cvQueryFrame(capture); 

	for(int i=0; i<frame->height-2; i=i+2)
	{
		for(int j=0; j<frame->width-2; j=j+2)
		{
			green[i/2][j/2] = frame->imageData[frame->widthStep*i + frame->nChannels* j + 1];

			if ((oldgreen[i/2][j/2]+80) < green[i/2][j/2])
			{				  				  
				int m = sprintf(message, "(%d, %d) ", j, i);                  

				write (fd, message, 12 );          //send data
				
				oldgreen[i/2][j/2] = green[i/2][j/2];
				
				goto next;  
			}
			
			oldgreen[i/2][j/2] = green[i/2][j/2];
			  
		}
	}
		
    write (fd, "------ ", 7);      //when nothing is found
  }

  cvDestroyAllWindows() ;
  cvReleaseCapture(&capture);     

  return 0;
}