// Mallard Bane
// A robot that hunts ducks.
//
// Aaron Becker, Ian Ferrel, and Kevin Pang
//
// Using SPU-Toolbox, written by Ross J. Micheals
//         rmicheals@lehigh.edu
//         (c) 1999-2001 Ross J. Micheals
//         GNU Public License

#include <math.h>
#include <string>
#include <iostream>
#include <sstream>
#include <unistd.h>
#include <sys/io.h>
#include <sys/time.h>


#include <SPU-Toolbox/SPU-Toolbox.h>
#include "DPPTUPoint.h"
#include "main_video.h"

#define PPORT 0x378

int RED = 0;
int GREEN = 1;
int BLUE = 2;

int WIDTH = 640;
int HEIGHT = 480;

int UPPERLEFTX = 175;
int UPPERLEFTY = 130;
int LOWERRIGHTX = 450;
int LOWERRIGHTY = 280;

int BOXSIZE = (LOWERRIGHTX - UPPERLEFTX) / 8;

//experimentally determined :)
int GUNUPPERLEFTX = -16;
int GUNUPPERLEFTY = 48;
int GUNLOWERRIGHTX = -109;
int GUNLOWERRIGHTY = 1;
//we should all the other corners,
// ury = 46

int duckx, ducky;
int predx, predy;

int MAX_AGE = 10;
int duckage = MAX_AGE;

int PTUUPPERLEFTPAN = -71;
int PTUUPPERLEFTTILT = 210;
int PTULOWERRIGHTPAN = -458;
int PTULOWERRIGHTTILT = 4;

int BULLETUPPERLEFTX = 223;
int BULLETUPPERLEFTY = 324;
int BULLETLOWERRIGHTX = 229;
int BULLETLOWERRIGHTY = 331;

int GUNWIDTH = GUNUPPERLEFTX - GUNLOWERRIGHTX;
int GUNHEIGHT = GUNUPPERLEFTY - GUNLOWERRIGHTY;

double AIM_THRESHOLD = 20;

int DUCK_THRESHOLD = 40000;
int SCREENDEPTH = 250; // pixels. Measure it for real
int SCREENHEIGHT = LOWERRIGHTY - UPPERLEFTY;
int SCREENWIDTH = LOWERRIGHTX - UPPERLEFTX;
bool done  = false;

int bulletbox;

V4L v4l;
XIH main_win;
DPPTUPoint pantilt("/dev/ttyS0");


timeval time1;
timeval time2;

int dot_x = -1;
int dot_y = -1;
int color = -1;

enum filter { RAW, DIFFERENCED, THRESHOLD, BOX, KILLMODE };
string filternames[5] = {"RAW", "DIFFERENCED", "THRESHOLD", "BOX", 
			 "KILL MODE" };
int filtermode = RAW;
bool image_cap_requested = false;

void done_callback(void);
double ptu_convert_tilt(int);
double ptu_convert_pan(int);
double abs(double);

// Once registered, this callback will be executed when XIH receieves
// a keypress
// This allows dynamic changes in the filtering mode, along with other
// keyboard commands.
void keypress_callback(XIH* xih, const XEvent *xev)
{
  char key = xih->extract_key_as_char(xev);

  bool success = true;
  double delta = 10.0;
  switch(key) {
    
    // filter mode commands
    case '1':
        filtermode = RAW;
        break;
    case '2':
        filtermode = DIFFERENCED;
        break;
    case '3':
        filtermode = THRESHOLD;
        break;
    case '4':
      filtermode = BOX;
      break;
    case '5':
      filtermode = KILLMODE;
      break;
    // pan-tilt commands
    case 'l':
        delta *= -1;
    case 'h':
        success = pantilt.pan_by(delta);
        break;
    case 'k':
        delta *= -1;
    case 'j':
        success = pantilt.tilt_by(delta);
        break;

    // other stuff
    case 'c':
        image_cap_requested = true;
        break;
    case 'q':
      done_callback();
      break;
    case 'd':
      cerr << "duck on screen!" << endl;
      break;
    case 'f':
      cerr << "no duk" << endl;
      break;
  }

  if(!success) {
      cerr << "Pan/Tilt Operation failed\n";
      main_win.set_draw_color("white");
      main_win.draw_text("Pan/Tilt Failed", 50, 50);
      main_win.refresh();
  }
  return;
}

// performs image differencing, given a current image and a previous image
void do_difference(SPU_Image* cur_image, SPU_Image* prev_image)
{
    SPU_u8* pixel;
    SPU_u8* prev_pixel;
    
    int height, width;
    height = cur_image->get_height();
    width = cur_image->get_width();

    // Replace each pixel with the difference between it and the
    // previous pixel.
    
    //        for (int y = 0; y < height; ++y) {
    //          for (int x = 0; x < width; ++x) {
    
	        for (int y = UPPERLEFTY; y < LOWERRIGHTY; ++y) {
	            for (int x = UPPERLEFTX; x < LOWERRIGHTX; ++x) {
            pixel = cur_image->fast_get_pixel(x,y);
            prev_pixel = prev_image->fast_get_pixel(x,y);

	    *pixel = abs((*pixel) - (*prev_pixel));
            *(pixel + 1) = abs((*(pixel + 1)) - (*(prev_pixel + 1)));
            *(pixel + 2) = abs((*(pixel + 2)) - (*(prev_pixel + 2)));
        }
    }

    return;
}

// threshold eliminates all pixels whose total value does not exceed some
// threshold value
void do_threshold(SPU_Image* image)
{
    int threshold = 175 * 3;
    
    SPU_u8* prev_pixel, *cur_pixel, *next_pixel;
    int height, width;
    height = image->get_height();
    width = image->get_width();

    // Replace each pixel with the difference between it and the
    // previous pixel.
    for (int y = 0; y < height; y++) {
        for (int x = 1; x < width - 1; x += 3) {
            prev_pixel = image->fast_get_pixel(x-1,y);
            cur_pixel = image->fast_get_pixel(x,y);
            next_pixel = image->fast_get_pixel(x+1,y);

            if(!(*prev_pixel + *(prev_pixel+1) + *(prev_pixel+2)
                +*cur_pixel + *(cur_pixel+1) + *(cur_pixel+2)
                +*prev_pixel + *(prev_pixel+1) + *(prev_pixel+2)
                > threshold)) 
                *prev_pixel = *(prev_pixel+1) = *(prev_pixel+2) = 
                *next_pixel = *(next_pixel+1) = *(next_pixel+2) = 
                *cur_pixel = *(cur_pixel+1) = *(cur_pixel+2) = 0;
            else
                *prev_pixel = *(prev_pixel+1) = *(prev_pixel+2) = 
                *next_pixel = *(next_pixel+1) = *(next_pixel+2) = 
                *cur_pixel = *(cur_pixel+1) = *(cur_pixel+2) = 255;
        }
    }

    return;
}

// do_box draws a box around where the program thinks the duck is
void do_box(SPU_Image* cur_image)
{
  int increment = BOXSIZE / 2;

  int besti, bestj;
  int best_total = 0;
  int temp = 0;
  SPU_u8* pixel;

  for (int j = UPPERLEFTY; j < LOWERRIGHTY - BOXSIZE; j += increment) {
    for (int i = UPPERLEFTX; i < LOWERRIGHTX - BOXSIZE; i += increment) {
      temp = 0;
      for (int b = j; b < j + BOXSIZE; b += 1) {
	for (int a = i; a < i + BOXSIZE; a += 1) {
	  pixel = cur_image->fast_get_pixel(a,b);
	  temp += *pixel + *(pixel + 1) + *(pixel + 2);
	}
      }
      if (temp > best_total) {
	best_total = temp;
	besti = i;
	bestj = j;
      }
    }
  }

  for (int i = besti; i < besti + BOXSIZE; ++i) {
    pixel = cur_image->fast_get_pixel(i,bestj);
    *pixel = 255;
    *(pixel + 1) = 0;
    *(pixel + 2) = 0;
    pixel = cur_image->fast_get_pixel(i,bestj + BOXSIZE);
      *pixel = 255;
      *(pixel + 1) = 0;
      *(pixel + 2) = 0;
  }
  
  for (int j = bestj; j < bestj + BOXSIZE; ++j) {
    pixel = cur_image->fast_get_pixel(besti,j);
    *pixel = 255;
    *(pixel + 1) = 0;
    *(pixel + 2) = 0;
    pixel = cur_image->fast_get_pixel(besti + BOXSIZE, j);
    *pixel = 255;
    *(pixel + 1) = 0;
    *(pixel + 2) = 0;
  }
  
  /*
  //  This code is just to see our bounding box
  for (int i = UPPERLEFTX; i < LOWERRIGHTX; ++i) {
    pixel = cur_image->fast_get_pixel(i,UPPERLEFTY);
    *pixel = 0;
    *(pixel + 1) = 0;
    *(pixel + 2) = 255;
    pixel = cur_image->fast_get_pixel(i,LOWERRIGHTY);
    *pixel = 0;
    *(pixel + 1) = 0;
    *(pixel + 2) = 255;
  }

  for (int j = UPPERLEFTY; j < LOWERRIGHTY; ++j) {
    pixel = cur_image->fast_get_pixel(UPPERLEFTX,j);
    *pixel = 0;
    *(pixel + 1) = 0;
    *(pixel + 2) = 255;
    pixel = cur_image->fast_get_pixel(LOWERRIGHTX,j);
    *pixel = 0;
    *(pixel + 1) = 0;
    *(pixel + 2) = 255;
  }
*/ 
}

void find_duck(int upperleftx, int upperlefty, int lowerrightx,
	       int lowerrighty, SPU_Image* cur_image, int increment)
{
  SPU_u8* pixel;
  int temp = 0;
  int bestx, besty;
  int best_total = 0;

  for (int j = upperlefty; j < lowerrighty - BOXSIZE; j += increment) {
    for (int i = upperleftx; i < lowerrightx - BOXSIZE; i += increment) {
      temp = 0;
      for (int b = j; b < j + BOXSIZE; b += 1) {
	for (int a = i; a < i + BOXSIZE; a += 1) {
	  pixel = cur_image->fast_get_pixel(a,b);
	  temp += *(pixel + 1) + *(pixel + 2); // no red channel 
	}
      }
      // bestx,besty = upperleft corner of duck
      if (temp > best_total) {
	best_total = temp;
	bestx = i;
	besty = j;
      }
    }
  }
  if (best_total > DUCK_THRESHOLD) {
    duckage = 0;
    duckx = bestx;
    ducky = besty;
  }
}

void draw_box(SPU_Image* cur_image, int upleftx, int uplefty,
	      int lowleftx, int lowlefty, int color)
{
  if (color > 2) { // only have 3 colors
    return;
  }
  SPU_u8* pixel;
    for (int i = upleftx; i < lowleftx; ++i) {
      pixel = cur_image->fast_get_pixel(i,uplefty);
      for (int j = 0; j < 3; ++j) {
	*(pixel + j) = 0;
      }
      *(pixel + color) = 255;

      pixel = cur_image->fast_get_pixel(i,lowlefty);
      for (int j = 0; j < 3; ++j) {
	*(pixel + j) = 0;
      }
      *(pixel + color) = 255;
    }
    
    for (int i = uplefty; i < lowlefty; ++i) {
      pixel = cur_image->fast_get_pixel(upleftx,i);
      for (int j = 0; j < 3; ++j) {
	*(pixel + j) = 0;
      }
      *(pixel + color) = 255;
      pixel = cur_image->fast_get_pixel(lowleftx, i);
      for (int j = 0; j < 3; ++j) {
	*(pixel + j) = 0;
      }
      *(pixel + color) = 255;
    }  
}

void draw_target(SPU_Image* cur_image)
{
  draw_box(cur_image, duckx, ducky, duckx + BOXSIZE, ducky + BOXSIZE, RED);
}  

void do_killmode(SPU_Image* cur_image)
{
  int oldx = duckx;
  int oldy = ducky;

  draw_box(cur_image, BULLETUPPERLEFTX, BULLETUPPERLEFTY, 
	   BULLETLOWERRIGHTX, BULLETLOWERRIGHTY, RED);

  if (duckage++ < MAX_AGE) {
    find_duck(max(duckx-BOXSIZE, UPPERLEFTX),
	      max(ducky-BOXSIZE, UPPERLEFTY),
	      min(duckx + 2*BOXSIZE, LOWERRIGHTX),
	      min(ducky + 2*BOXSIZE, LOWERRIGHTY),
	      cur_image, BOXSIZE/8);

    predict_duck(oldx, oldy, duckx, ducky);
    draw_box(cur_image, predx, predy, predx + BOXSIZE, predy + BOXSIZE, GREEN);
  }
  else {
    duckage = 0;
    find_duck(UPPERLEFTX, UPPERLEFTY, LOWERRIGHTX, LOWERRIGHTY, cur_image,
	      BOXSIZE/2);
  }

  pantilt.point_at(convert_x(predx + BOXSIZE/2), 
		   convert_y(predy + BOXSIZE/2), SCREENDEPTH);


  if (duckx != oldx || ducky != oldy) {
    gettimeofday(&time2,NULL);//,&zone);
    draw_target(cur_image);

    if(should_fire(cur_image)) { // Check how long its been since we've fired
      
      /*
      // Check if the duck is in the horizontal bounding box
      bool inBox = false;
      if(bestj > 220) 
      bestj = 220;
      else if(bestj < 175)
      bestj = 175;
      else
      inBox = true;
      */

      
      // If the difference between where we want to shoot and where we are
      // (converted to gun coordinates) is < AIM_THRESHOLD, then shoot
      
      if (abs(convert_x(predx + BOXSIZE / 2) -
	      ptu_convert_pan(pantilt.get_pan_step_position())) < 
	  AIM_THRESHOLD &&
	  abs(convert_y(predy + BOXSIZE / 2) -
	      ptu_convert_tilt(pantilt.get_tilt_step_position())) < 
	  AIM_THRESHOLD)
	//    && inBox) 
	{
	  outb(0xFF, PPORT);
	  usleep(100);
	  outb(0x00, PPORT);
	  gettimeofday(&time1,NULL);//,&zone);
	  cerr << "duckx " << duckx << " ducky " << ducky << endl;
	  cerr << "FIRE!" << endl;
	}
    }
  }
}
// checks the currect filtering mode and calls the appropriate function
// to do image operations.
void process_image(SPU_Image* cur_image, SPU_Image* prev_image)
{
    switch(filtermode) {
        case RAW:
            break;
        case DIFFERENCED:
            do_difference(cur_image, prev_image);
            break;
        case THRESHOLD:
            do_difference(cur_image, prev_image);
            do_threshold(cur_image);
            break;
        case BOX:
            do_difference(cur_image, prev_image);
	    do_box(cur_image);
	    break;
        case KILLMODE:
	    do_difference(cur_image, prev_image);
	    do_killmode(cur_image);
    }
    return;
}

double convert_x(int duck_x)
{
    return GUNUPPERLEFTX - double(duck_x - UPPERLEFTX) / SCREENWIDTH * GUNWIDTH;
}

double convert_y(int duck_y)
{
  return GUNUPPERLEFTY - double(duck_y - UPPERLEFTY) / SCREENHEIGHT * GUNHEIGHT; 
}

double ptu_convert_pan(int pan)
{
  return GUNUPPERLEFTX - double(pan - PTUUPPERLEFTPAN) / 
    (PTULOWERRIGHTPAN - PTUUPPERLEFTPAN) * GUNWIDTH;
}

double ptu_convert_tilt(int tilt)
{
  return GUNUPPERLEFTY + double(tilt - PTUUPPERLEFTTILT) /
    (PTUUPPERLEFTTILT - PTULOWERRIGHTTILT) * GUNHEIGHT;
}

double abs(double x)
{
  if (x < 0)
    return -x;
  else 
    return x;
}

// Called when the main XIH control thread dies.
//
void done_callback(void) {

  // Tell V4L++ to stop capturing.
  v4l.stop();

  // Terminate the main loop
  done = true;

  return;
}

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

    /* Get access to the parallel port */
    if (ioperm(PPORT, 3, 1)) {perror("ioperm"); exit(1);}

    // set up timer stuff for gun firing
    gettimeofday(&time1,NULL);//&zone);

  pantilt.set_pan_speed(100.0);
  pantilt.set_tilt_speed(140.0);

    pantilt.point_at(0, 0, SCREENDEPTH);    

    /*
    int x,y;
    while(1) {
        cin >> x>> y;
        pantilt.point_at(convert_x(x), convert_y(y), SCREENDEPTH);
	sleep(2);

	cerr << convert_x(x) << endl;
	cerr << convert_y(y) << endl;
	cerr << ptu_convert_pan(pantilt.get_pan_step_position()) << endl;
	cerr << ptu_convert_tilt(pantilt.get_tilt_step_position()) << endl;
    }
    */
    /*
      int x, y;
    while(1) {
      cin >> x >> y;
      pantilt.point_at(x,y,SCREENDEPTH);
      sleep(1);

      cerr << pantilt.get_pan_step_position() << endl;
      cerr << pantilt.get_tilt_step_position() << endl;
    }
    */

    char *cm = "rgb24";

    int width  = 640;
    int height = 480;

    main_win.init(width, height, cm, "Mallard Bane v0.3");
    main_win.register_callback(XIH::CB_KeyPress, keypress_callback);

    // Set the function to be called when XIH is finished. This must be
    // set *before* XIH is started.
    //
    XIH_set_finished_callback(done_callback);

    if (!v4l.init(cm, 1, width, height, "/dev/video")) 
        exit(1);

    // main_image will be displayed
    SPU_Image main_image(cm, width, height);
    // last_image is the previous main_image
    SPU_Image last_image(cm, width, height);
    // current_image is a temp image
    SPU_Image current_image(cm, width, height);

    // Start the XIH thread. This function will wait until the XIH
    // thread is started before it returns.
    //
    XIH::start();

    // Start the V4L thread. Like the XIH thread, this funciton will
    // wait until the V4L thread is started before it returns. Notice
    // that unlike XIH, there is a V4L thread per instance/device.
    //
    v4l.start_polled_mode();

    int image_counter = 0;
    while (!done) {

        // Make sure we can lock and draw to the image first!
        //
        if (XIH_lock() && main_win.can_draw()) {
            v4l.put_Image(main_image);

	      // Store the current image
	      current_image.assign(&main_image, true, true);
	      
	      process_image(&main_image, &last_image);
	    
	      if(image_cap_requested) {
                image_cap_requested = false;
                stringstream ss;
                ss << "mbane" << image_counter++ << ".jpg";
                main_image.save_jpeg(ss.str().c_str());
	      }
	      
	      main_win.fill(main_image);
	      
	      main_win.set_draw_color("white");
	      main_win.draw_text(filternames[filtermode].c_str(), 20, 20);
	      
	      main_win.refresh();
	      
	      last_image.assign(&current_image, true, true);
	    
	      XIH_unlock();
	}
    }

    // Prevent premature exit.
    XIH::stop_all();
    XIH::wait();

    pantilt.point_at(0, 0, SCREENDEPTH);

    return(0);
}


bool delay_ok() 
{
  long threshold = 350000;
    return(((time2.tv_sec - time1.tv_sec) == 0 
            && (time2.tv_usec - time1.tv_usec) > threshold)
            ||
            ((time2.tv_sec - time1.tv_sec == 1) 
            && (1000000 + time2.tv_usec - time1.tv_usec > threshold))
            ||
            (time2.tv_sec - time1.tv_sec > 1));
}

// Determines if we've just started a round
bool round_start(SPU_Image* cur_image)
{
  int BULLETTHRESHOLD = 8000;
  SPU_u8* pixel;  
  int temp = 0;
  for (int i = BULLETUPPERLEFTX; i < BULLETLOWERRIGHTX; ++i) {
    for (int j = BULLETUPPERLEFTY; j < BULLETLOWERRIGHTY; ++j) {
      pixel = cur_image->fast_get_pixel(i,j);
      temp += *pixel + *(pixel + 1) + *(pixel + 2);
    }
  }
  //cerr << "bulletbox " << temp << endl;

  // This determines if we've just reloaded (it also picks up when we've fired
  // sometimes as well because of the flash...
  if (temp > BULLETTHRESHOLD && bulletbox < BULLETTHRESHOLD) {
    //    cerr << "RELOAD" << endl;
    bulletbox = temp;
    gettimeofday(&time1,NULL);
    return true;
  }
  else {
    bulletbox = temp;
    return false;
  }
}

bool should_fire(SPU_Image* cur_image)
{
  // If the round hasn't just started and the delay is ok, then we should fire
  // This doesn't work perfectly, it still wastes shots at the beginning of
  // rounds and can fire quite rapidly during gameplay
  return !round_start(cur_image) && delay_ok() && 
    !near_dog(duckx,ducky);//!near_dog(predx, predy);
}

void predict_duck(int oldx, int oldy, int curx, int cury)
{
  double delay = 3;
  predx = curx + int(delay*(curx - oldx));
  predy = cury + int(delay*(cury - oldy));
  if (predx > LOWERRIGHTX - BOXSIZE)
    predx = LOWERRIGHTX - BOXSIZE;
  if (predx < UPPERLEFTX)
    predx = UPPERLEFTX;
  if (predy > LOWERRIGHTY - BOXSIZE)
    predy = LOWERRIGHTY - BOXSIZE;
  if (predy < UPPERLEFTY)
    predy = UPPERLEFTY;
};

bool near_dog(int x, int y)
{
  return x > 245 && x < 350 && y > 230;
}