#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 <time.h>
#include <iostream.h>
#include <math.h>

const int COMPOSITE_1 = 1;
EviCamera EYEBALL;

SerialPort s_port;

float** sats;
float** hues;

int makeThreshold(Image* i, Image* analysis);
bool greenMatch(float** s_array, float** h_array, int j, int k);
bool redMatch(float** s_array, float** h_array, int j, int k);
bool yellowMatch(float** s_array, float** h_array, int j, int k);
bool centerTarget(int target_x, int target_y);
void sendForward(int duration);
void sendBack(int duration);
void sendLeft(int duration);
void sendRight(int duration);
void sendStop();

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

  //EYEBALL.CreateRS232Link();
  //EYEBALL.InitializeCamera();

  s_port.openPort();
  
  V4L camera("DonutVision",
	     "/dev/video",
	     COMPOSITE_1,
	     VIDEO_MODE_NTSC,
	     VIDEO_PALETTE_RGB24,
	     320,
	     240,
	     24);
  
  SDL display("DonutVision", camera.image());

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

  SDL threshold("DonutVision", analysis);
  //cout << "Ready to loop" << endl;
  
  int width = 320;
  int height = 240;

  sats = new float*[height];
  for (int j = 0; j < height; j++) 
    sats[j] = new float[width];
  
  hues = new float*[height];
  for (int k = 0; k < height; k++)
    hues[k] = new float[width];
  
  
  while (true) {
    camera.readAhead();
    camera.read(display.image());
    Image* i = display.image();
    
    makeThreshold(i, analysis);
    display.write();
    camera.advance();
  }

  s_port.closePort();
}

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

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

  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;
    }
  }
  
  //cout << "hue = " << hues[100][100] << ", sat = " << sats[100][100] << endl;
  
  int redMinJ, redMaxJ, redMinK, redMaxK, yelMinJ, yelMaxJ, yelMinK, yelMaxK;
  int blueMinJ, blueMaxJ, blueMinK, blueMaxK;
  redMinJ = yelMinJ = blueMinJ = height;
  redMinK = yelMinK = blueMinK = width;
  redMaxJ = yelMaxJ = blueMaxJ = redMaxK = yelMaxK = blueMaxK = 0;
  
  //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 (redMatch(sats, hues, j, k)) {
	//i->setPixel(k,j,255,127,127);
	if (j < redMinJ) 
	  redMinJ = j;
	if (j > redMaxJ)
	  redMaxJ = j;
	if (k < redMinK)
	  redMinK = k;
	if (k > redMaxK)
	  redMaxK = k;
      }
      if (yellowMatch(sats, hues, j, k)) {
	//i->setPixel(k,j,127,255,127);
	if (j < yelMinJ)
	  yelMinJ = j;
	if (j > yelMaxJ)
	  yelMaxJ = j;
	if (k < yelMinK)
	  yelMinK = k;
	if (k > yelMaxK)
	  yelMaxK = k;
	  }
      if (greenMatch(sats,hues,j,k)) { 
	//i->setPixel(k,j,127,255,127);
	if (j < blueMinJ)
	  blueMinJ = j;
        if (j > blueMaxJ)
	  blueMaxJ = j;
        if (k < blueMinK)
	  blueMinK = k;
        if (k > blueMaxK)
	  blueMaxK = k;
      }
    }
  }

  
  
  int rc_x = redMinK + (redMaxK-redMinK) / 2;
  int rc_y = redMinJ + (redMaxJ-redMinJ) / 2;
  int yc_x = yelMinK + (yelMaxK-yelMinK) / 2;
  int yc_y = yelMinJ + (yelMaxJ-yelMinJ) / 2;
  int bc_x = blueMinK + (blueMaxK-blueMinK) / 2;
  int bc_y = blueMinJ + (blueMaxJ-blueMinJ) / 2;

  /*

  cout << "damn dirty red max k, min k, max j, min j: " << redMaxK << " "
       << redMinK << " " << redMaxJ << " " << redMinJ << endl;

  cout << "yellah  max k, min k, max j, min j: " << yelMaxK << " "
       << yelMinK << " " << yelMaxJ << " " << yelMinJ << endl;
  
  cout << "sad pathetic blue  max k, min k, max j, min j: " << blueMaxK << " "
       << blueMinK << " " << blueMaxJ << " " << blueMinJ << endl;
  */
  

  if ((redMaxK > redMinK) && (redMaxJ > redMinJ)) {
    i->rect(redMinK,redMinJ,redMaxK,redMaxJ,0,255,0);
    i->line(rc_x-2,rc_y,rc_x+2,rc_y,255,255,255);
    i->line(rc_x,rc_y-2,rc_x,rc_y+2,255,255,255);
  }

  if ((yelMaxK > yelMinK) && (yelMaxJ > yelMinJ)) {
    i->rect(yelMinK,yelMinJ,yelMaxK,yelMaxJ,0,0,255);
    i->line(yc_x-2,yc_y,yc_x+2,yc_y,255,255,255);
    i->line(yc_x,yc_y-2,yc_x,yc_y+2,255,255,255);
  }
  
  if ((blueMaxK > blueMinK) && (blueMaxJ > blueMinJ)) {
    i->rect(blueMinK,blueMinJ,blueMaxK,blueMaxJ,255,255,255);
    i->line(bc_x-2,bc_y,bc_x+2,bc_y,255,255,255);
    i->line(bc_x,bc_y-2,bc_x,bc_y+2,255,255,255);  
    i->line(bc_x, bc_y, yc_x, yc_y, 0,0,255);
    i->line(rc_x, rc_y, yc_x, yc_y, 255,0,0);
    
    double range = sqrt((yc_x - bc_x) * (yc_x - bc_x) + 
			(yc_y - bc_y) * (yc_y - bc_y));
    
    int deltaXyb = yc_x - bc_x;
    int deltaYyb = yc_y - bc_y;
    
    int deltaXry = rc_x - yc_x;
    int deltaYry = rc_y - yc_y;
    
    double robotAngle = atan2((double)deltaYry,(double)deltaXry);
    double handAngle = atan2((double)deltaYyb,(double)deltaXyb);
    double angleDiff = robotAngle - handAngle;
    
    if (range > 150) {
      // have time to turn away 
      if (angleDiff > (M_PI / 6.0) ) {      
	sendLeft(5);
      }
      else if (angleDiff < -(M_PI / 6.0) ) {
	sendRight(5);
      }
    }
    else {
      // run!
      if (angleDiff < M_PI_2 && angleDiff > -(M_PI_2)) {
	// run forward
	sendForward(10);
      }
      else {
	// run backward
	sendBack(10);
      }
    }
  }
  return 0;
  //centerTarget(c_x, c_y);
}

bool redMatch(float** s_array, float** h_array, int j, int k) {
  int counter = 0;
  
  for (int h = j - 1; h <= j + 1; h++) {
    for (int i = k - 1; i <= k + 1; i++) {
      if ((h_array[h][i] > 210) && (h_array[h][i] < 270) &&
	  (s_array[h][i] > .5) && (s_array[h][i] < .75)) {
        counter++;
      }
    }
  }
  return (counter > 5);
}

bool yellowMatch(float** s_array, float** h_array, int j, int k) {
  int counter = 0;
  
  for (int h = j - 1; h <= j + 1; h++) {
    for (int i = k - 1; i <= k + 1; i++) {
      if ((h_array[h][i] < 210) && (h_array[h][i] > 150) &&
	  (s_array[h][i] > .5) && (s_array[h][i] < .75)) {
        counter++;
      }
    }
  }
  return (counter > 5);
}
bool greenMatch(float** s_array, float** h_array, int j, int k) {

  int counter = 0;
  
  for (int h = j - 1; h <= j + 1; h++)
    for (int i = k - 1; i <= k + 1; i++) {
      
      if ((h_array[h][i] > 90) && (h_array[h][i] < 150) &&
	  (s_array[h][i] > .5) && (s_array[h][i] < .75)) {
      	counter++;
      }
    }
  
  return (counter > 5);
}

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

  return true;
}

void sendForward(int duration) {

  if (duration > 0 && duration < 16) { 
    byte message = 32 + duration;
    s_port.send(message);
  }
}

void sendBack(int duration) {
  if (duration > 0 && duration < 16) { 
    byte message = 48 + duration;
    s_port.send(message);
  }
}

void sendLeft(int duration) {
 if (duration > 0 && duration < 16) { 
    byte message = 64 + duration;
    s_port.send(message);
  }

}

void sendRight(int duration) {
 if (duration > 0 && duration < 16) { 
    byte message = 80 + duration;
    s_port.send(message);
  }
}

void sendStop() {
  byte message = 16;
  s_port.send(message);
}