#include "SerialPort.hh"
#include "SDL.hh"
#include "V4L.hh"
#include "VideoIn.hh"
#include "VideoOut.hh"
#include "Image.hh"
#include "videodev.h"
#include "camctl.hh"

#include <iostream.h>

const int COMPOSITE_1 = 1;
EviCamera EYEBALL;

void makeThreshold(Image* i, Image* analysis);
bool positiveMatch(float** s_array, float** h_array, int j, int k);
bool centerTarget(int target_x, int target_y);

int main(int argc, char** argv) {

  EYEBALL.CreateRS232Link();
  EYEBALL.InitializeCamera();
  
  V4L camera("Laser Camera",
	     "/dev/video",
	     COMPOSITE_1,
	     VIDEO_MODE_NTSC,
	     VIDEO_PALETTE_RGB24,
	     320,
	     240,
	     24);
  
  SDL display("Laser Camera", camera.image());

  unsigned char* img_data = new unsigned char[320*240*3];
  
  Image* analysis = new Image("an", 320, 240, 24, img_data);

  SDL threshold("Image Analysis", analysis);
  cout << "Ready to loop" << endl;
  
  while (true) {
    camera.readAhead();
    camera.read(display.image());
    Image* i = display.image();
    
    makeThreshold(i, analysis);

    display.write();
    camera.advance();
  }
}

void makeThreshold(Image* i, Image* analysis) {

  int width = i->width();
  int height = i->height();

  float** sats = new float*[height];
  for (int j = 0; j < height; j++) 
    sats[j] = new float[width];

  float** hues = new float*[height];
  for (int k = 0; k < height; k++)
    hues[k] = new float[width];

  for (int j = 0; j < height; j++) {
    for (int k = 0; k < width; k++) {
      int red =   i->getPixel(k,j,0);
      int green = i->getPixel(k,j,1);
      int blue =  i->getPixel(k,j,2);
      float h,s,b;
      i->RGB2HSV((float)red, (float)green, (float)blue, &h, &s, &b);
      sats[j][k] = s;
      hues[j][k] = h;
    }
  }
  
  int minJ = height;
  int maxJ = 0;
  int minK = width;
  int maxK = 0;

  for (int j = 1; j < (height - 1); j++) {
    for (int k = 1; k < (width - 1); k++) {
      if (positiveMatch(sats,hues,j,k)) { 
	i->setPixel(k,j,127,255,127);
	if (j < minJ)
	  minJ = j;
	if (j > maxJ)
	  maxJ = j;
	if (k < minK)
	  minK = k;
	if (k > maxK)
	  maxK = k;
      }
    }
  }

  for (int j = 1; j < height; j++) {
    delete sats[j];
    delete hues[j];
  }
  delete sats;
  delete hues;
  
  int c_x = minK + (maxK-minK) / 2;
  int c_y = minJ + (maxJ-minJ) / 2;

  i->rect(minK,minJ,maxK,maxJ,0,0,255);
  i->line(c_x-2,c_y,c_x+2,c_y,255,255,255);
  i->line(c_x,c_y-2,c_x,c_y+2,255,255,255);

  centerTarget(c_x, c_y);
}

bool positiveMatch(float** s_array, float** h_array, int j, int k) {

  int counter = 0;
  bool result = false;

  for (int h = j - 1; h <= j + 1; h++)
    for (int i = k - 1; i <= k + 1; i++) {
      
      if ((0 < h_array[h][i]) && (h_array[h][i] < 40) &&
	    (.34 < s_array[h][i]) && (s_array[h][i] < .6)) {
	counter++;
      }
    }
  if (counter > 4)
    result = true;
  
  return result;
}

bool centerTarget(int target_x, int target_y) {
  int center_x = 160;
  int center_y = 120;
  int x_scale = 1;
  int y_scale = 1;
  EYEBALL.PTDRelPos(PAN_SPD_MAX, TILT_SPD_MAX, 
		    x_scale * (target_x - center_x),
		    y_scale * (center_y - target_y));
}