/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "BWBallDetector.h"
#include <algorithm>
#include <stdio.h>

using std::max;
using std::min;

BWBallDetector::BWBallDetector() {
  y_data = new int32[bw_image_width * bw_image_height];
  u_data = new int32[bw_image_width * bw_image_height];
  v_data = new int32[bw_image_width * bw_image_height];
  black_data = new int32[bw_image_width * bw_image_height];
  white_data = new int32[bw_image_width * bw_image_height];
  region_data = new int32[bw_image_width * bw_image_height];

  working = new int32[bw_image_width * bw_image_height];
  regions = new BWRegion[max_regions];
  row_score_count = new int32[bw_image_height];
  num_regions = 0;
}

BWBallDetector::~BWBallDetector() {
  delete[] y_data;
  delete[] region_data;
  delete[] working;
  delete[] regions;

  y_data = region_data = working = NULL;
  regions = NULL;
}

void BWBallDetector::gradientFilter(int32 *in_buf, int32 *out_buf) {
  
  // Values of the 4 points we'll sample to calculate
  // the gradient.
  int32 x_minus, x_plus, y_minus, y_plus;
  
  // Offsets we add to origin to get to other two points
  int32 x_m_offset, x_p_offset, y_m_offset, y_p_offset;
  
  // In addition to DX and DY, we'll also calculate the gradient
  // using pixels diagonally above and below the origin. We'll
  // take the max of this and the X-Y based gradient.
  int32 a_m_offset, a_p_offset, b_m_offset, b_p_offset;

  // We don't set the pixels on the border of the image. Zero
  // them.
  setBorder(out_buf, 0);

  x_m_offset = -1;
  x_p_offset = 1;
  y_m_offset = -bw_image_width;
  y_p_offset = bw_image_width;
  
  a_m_offset = -bw_image_width - 1;
  a_p_offset = +bw_image_width + 1;
  b_m_offset = -bw_image_width + 1;
  b_p_offset = +bw_image_width - 1;

  for(int32 y=1; y<bw_image_height - 1; y++) {
    for(int32 x=1; x<bw_image_width - 1; x++) {
      
      // We should track this with additions to avoid the multiply
      int32 origin_offset = y*bw_image_width + x;

      // Grab our points
      x_minus = in_buf[origin_offset + x_m_offset];
      x_plus  = in_buf[origin_offset + x_p_offset];
      y_minus = in_buf[origin_offset + y_m_offset];
      y_plus  = in_buf[origin_offset + y_p_offset];

      int32 dx = x_plus - x_minus;
      int32 dy = y_plus - y_minus;

      // Technically, we should take the square root here. But it's
      // very slow on MIPS chips, so we don't and just square our
      // threshold instead. Note: 32-bits are your friend - otherwise
      // you're gonna overflow.
      out_buf[origin_offset] = (int32)(dx*dx + dy*dy);
      
      x_minus = in_buf[origin_offset + a_m_offset];
      x_plus  = in_buf[origin_offset + a_p_offset];
      y_minus = in_buf[origin_offset + b_m_offset];
      y_plus  = in_buf[origin_offset + b_p_offset];
      
      dx = x_plus - x_minus;
      dy = y_plus - y_minus;
      
      int32 temp_grad = (int32)(dx*dx + dy*dy);
      
      if(temp_grad > out_buf[origin_offset])
	out_buf[origin_offset] = (temp_grad);
    }
  }
}

void BWBallDetector::setBorder(int32 *buf, int32 value) {
  
  int32 first_offset, last_offset;
  
  // Set top and bottom row
  last_offset = bw_image_width*bw_image_height - bw_image_width;
  for(int32 i=0; i<bw_image_width; i++) {
    buf[i] = value;
    buf[last_offset + i] = value;
  }

  // Set left and right columns
  first_offset = 0;
  last_offset = bw_image_width - 1;
  for(int32 i=0; i<bw_image_height; i++) {
    
    buf[first_offset] = value;
    buf[last_offset] = value;
    
    first_offset += bw_image_width;
    last_offset += bw_image_width;
  }
}

void BWBallDetector::threshold(int32 *buf, 
			       int32 value, int32 replace_with,
			       bool clear_above) {

  if(clear_above) {
    for(int32 i=0; i<bw_image_width*bw_image_height; i++) {
      if(buf[i] > value)
	buf[i] = replace_with;
    }
  } else {
    for(int32 i=0; i<bw_image_width*bw_image_height; i++) {
      if(buf[i] < value)
	buf[i] = replace_with;
    }
  }
}

int32 BWBallDetector::findConnectedComponents(int32 *in_buf,
					    int32 *out_buf) {

  // We define the cannonical element of a connected region as
  // the element with the lowest offset in data. So we need to
  // go through and label all nonzero values with their offset.
  // Zero values are labeled with -1
  // to indicate that they belong to no region.
  
  for(int32 i=0; i<bw_image_width*bw_image_height; i++) {
    if(in_buf[i] > 0)
      out_buf[i] = i;
    else
      out_buf[i] = -1;
  }
  
  // So now each point is labeled as it's own cannonical
  // element. We need to see if points are connected to
  // others. If they are, we set that points cannonical
  // element to the lesser of it's original one and it's
  // neighbors cannonical elm. (And we update the neighbor
  // too if our cannonical elm is less than its c.e.)
  // We only test the pixel above us and the pixel to our
  // left to avoid extra work.
  
  int32 left_offset = -1;
  int32 top_offset = -bw_image_width;
  for(int32 i=0; i<bw_image_width*bw_image_height; i++) {

    // We only need to check non-background pixels
    if(out_buf[i] >= 0) {
      // Find the offsets of our neighbors in data
      int32 left = i + left_offset;
      int32 top = i + top_offset;
      
      int32 my_label = out_buf[i];
      
      // Find the cannonical element of the pixel to our left
      // if it's not background. Use -1 if it's background.
      int32 left_label = -1;
      if(left >= 0 &&
	 out_buf[left] >= 0) {
	
	// Find cannonical left label. A cannonical element points to
	// itself in the array. All other elements point further up
	// their tree toward their cannonical element.
	left_label = out_buf[left];
	while(left_label != out_buf[left_label])
	  left_label = out_buf[left_label];
      } 
      
      int32 top_label = -1;
      if(top >= 0 && 
	 out_buf[top] >= 0) {
	
	top_label = out_buf[top];
	while(top_label!=out_buf[top_label])
	  top_label = out_buf[top_label];
      }
      
      /* This pixel needs to join the tree with the lowest
	 cannonical element. If by doing so it connects the
	 other adjacent pixel to said tree, we need to update
	 the cannonical element of the adjacent pixel to reflect
	 the new connection.
      */
      
      if(left_label >= 0 &&
	 top_label >= 0) {
	
	// Okay, both of our neighbors are valid. Which has the
	// lower cannonical element?
	if(left_label < top_label) {
	  // left_label *must* be < my_label here.
	  my_label = left_label;
	   
	  // Do we need to relabel top_label's tree with our
	  // label? top_label = out_buf[top_label] = cannonical
	  // element of that tree. So we only need to reset
	  // out_buf[top_label] to point to our cannonical element
	  // to reset to cannonical element of all points in
	  // top_label's tree (as they all point to out_buf[top_label]).
	  if(my_label < top_label) {
	    out_buf[top_label] = my_label;
	  }
	} else {
	  my_label = top_label;
	  
	  if(my_label < left_label)
	    out_buf[left_label] = my_label;
	}
      } else if(left_label >= 0) {
	my_label = left_label;
      } else if(top_label >= 0) {
	my_label = top_label;
      }
      
      out_buf[i] = my_label;
    }
  }
  
  // Make sure each pixel is labeled with it's cannonical
  // element.
  for(int32 i=0; i<bw_image_width*bw_image_height; i++) {
    if(out_buf[i] >= 0) {
      int32 label = out_buf[i];
      while(label != out_buf[label])
	label = out_buf[label];
      
      out_buf[i] = label;
    }
  }

  // So now we have a forest of trees where each node is a 
  // cannonical element and points to itself or the node
  // points to it's cannonical element via 1 link. The only
  // problem is that our cannonical elems range from 0 -> width*height
  // and are not contiguous. We should fix that.
  
  // Recall that the canonical element always has the
  // lowest offset of all elements in each region. So
  // we'll relabel it before any of it's children.
  
  int32 first_element = 1;
  for(int32 i=0; i<bw_image_width*bw_image_height; i++) {
    if(out_buf[i]==i) {
      // We are a root node and need to be relabeled.
      out_buf[i] = first_element++;
    } else if(out_buf[i] >= 0) {
      // We're a child node and point at our root which
      // has already been relabeled.
      out_buf[i] = out_buf[out_buf[i]];
    } else {
      // we're background
      out_buf[i] = 0;
    }
  }

  return first_element - 1;
}

/* The robot's camera puts status information in the bottom
   left 16 pixels of the image */
void BWBallDetector::stripStatus(int32 *buf) {
  int32 offset = bw_image_width * bw_image_height - bw_image_width;
  for(int32 i=0; i<16; i++) {
    buf[offset] = buf[offset - bw_image_width];
    offset++;
  }
}

void BWBallDetector::gatherRegionStats(int32 *buf,
				       int32 _num_regions) {
  num_regions = _num_regions;
  
  for(int32 i=0; i<num_regions; i++) {
    regions[i].num_pixels = 0;
    regions[i].mean_x = regions[i].mean_y = 0;
    regions[i].min_x = regions[i].min_y = 
      bw_image_width + bw_image_height;
    regions[i].max_x = regions[i].max_y = 0;
    regions[i].mean_r = regions[i].var_r = 0;
    regions[i].sum_x = regions[i].sum_y = 0;
    regions[i].sum_xx = regions[i].sum_xy = regions[i].sum_yy = 0;
    regions[i].sum_xxx = regions[i].sum_xxy = 
      regions[i].sum_xyy = regions[i].sum_yyy = 0;
    regions[i].fit_x = regions[i].fit_y = regions[i].fit_r = 0;
    regions[i].scores[BWRegion::mean_row_score] = 0;
  }
  
  // Gather stats such as bounding box about all of our
  // regions.
  
  // Recall that region zero is the background. So we
  // don't track info if data[i]==0 and we need to subtract
  // 1 to get the offsets into the regions array.
  int32 x, y;
  int32 region_offset;
  for(int32 i=0; i<bw_image_width * bw_image_height; i++) {
    
    // gather stats for the edge detected version of the
    // buffer.
    if(buf[i] > 0) {

      region_offset = buf[i] - 1;

      regions[region_offset].num_pixels++;

      x = i % bw_image_width;
      y = i/bw_image_width;

      regions[region_offset].scores[BWRegion::mean_row_score]
	+= row_score_count[y];
      
      regions[region_offset].sum_x += x;
      regions[region_offset].sum_y += y;

      // Track the bounding box of our region
      if(x < regions[region_offset].min_x)
	regions[region_offset].min_x = x;
      if(y < regions[region_offset].min_y)
	regions[region_offset].min_y = y;
      
      if(x > regions[region_offset].max_x)
	regions[region_offset].max_x = x;
      if(y > regions[region_offset].max_y)
	regions[region_offset].max_y = y;

      // calculate various sum-foo for the circle fitting
      // code. You do need 32-bit integers for this - 16
      // will overflow. Badly.
      regions[region_offset].sum_xx += x*x;
      regions[region_offset].sum_xy += x*y;
      regions[region_offset].sum_yy += y*y;
      regions[region_offset].sum_xxx += x*x*x;
      regions[region_offset].sum_xxy += x*x*y;
      regions[region_offset].sum_xyy += x*y*y;
      regions[region_offset].sum_yyy += y*y*y;
    }
  }

  // Go through our regions and divide to calculate our
  // means.
  for(int32 i=0; i<num_regions; i++) {
    regions[i].mean_x = regions[i].sum_x / regions[i].num_pixels;
    regions[i].mean_y = regions[i].sum_y / regions[i].num_pixels;
    regions[i].scores[BWRegion::mean_row_score] /= regions[i].num_pixels;
  }
  
  /* We need to calculate several intermediate terms to fit a
     circle to our data.
  */
  for(int32 i=0; i<num_regions; i++) {
    double n = regions[i].num_pixels;
    
    double A = n * regions[i].sum_xx -
      regions[i].sum_x * regions[i].sum_x;
    
    double B = n * regions[i].sum_xy - 
      regions[i].sum_x * regions[i].sum_y;

    double C = n * regions[i].sum_yy -
      regions[i].sum_y * regions[i].sum_y; 
  
    double D = 0.5*(n * regions[i].sum_xyy -
		    regions[i].sum_x * regions[i].sum_yy +
		    n * regions[i].sum_xxx - 
		    regions[i].sum_x * regions[i].sum_xx);

    double E = 0.5*(n * regions[i].sum_xxy -
		    regions[i].sum_y * regions[i].sum_xx +
		    n * regions[i].sum_yyy - 
		    regions[i].sum_y * regions[i].sum_yy);

    double denom = A*C - B*B;

    if(fabs(denom) > .0000001) {
      regions[i].fit_x = (D*C - B*E)/denom;
      regions[i].fit_y = (A*E - B*D)/denom;
    } else {
      // Our denominator is too close to zero and probably should be
      // zero except for roundoff. This can happen if points are 
      // colinear.
      regions[i].fit_x = regions[i].fit_y = 0;

      // indicate it's a bogus fit so we don't calculate
      // fit_r at a later date.
      regions[i].fit_r = -1; 
    }
  }

  // We need the mean radius of each region.
  // We do this both in terms of the centroid of the image
  // and in terms of (fit_x, fit_y) which are our best fit
  // estimates for the circle that fits the data.
  for(int32 i=0; i<bw_image_width*bw_image_height; i++) {
    if(buf[i] > 0) {
      x = i % bw_image_width;
      y = i/bw_image_width;
      int32 region_offset = buf[i] - 1;
      
      double dx = x - regions[region_offset].mean_x;
      double dy = y - regions[region_offset].mean_y;
      
      regions[region_offset].mean_r += sqrt(dx*dx + dy*dy);

      if(regions[region_offset].fit_r >= 0) {
	
	dx = x - regions[region_offset].fit_x;
	dy = y - regions[region_offset].fit_y;

	regions[region_offset].fit_r += sqrt(dx*dx + dy*dy);
      }
    }
  }
  
  // Again, we need to go through and divide to get the mean
  // radius.
  for(int32 i=0; i<num_regions; i++) {
    regions[i].mean_r /= regions[i].num_pixels;
    
    if(regions[i].fit_r >= 0)
      regions[i].fit_r /= regions[i].num_pixels;
  }
  
  // Calculate the variance of each pixel of the regions from
  // the mean radius of that region.
  for(int32 i=0; i<bw_image_width*bw_image_height; i++) {
    if(buf[i] > 0) {
      x = i % bw_image_width;
      y = i/bw_image_width;
      int32 region_offset = buf[i] - 1;
      
      double dx = x - regions[region_offset].mean_x;
      double dy = y - regions[region_offset].mean_y;
      
      double r = sqrt(dx*dx + dy*dy);

      double dr = r - regions[region_offset].mean_r;

      regions[region_offset].var_r += dr*dr;
    }
  }

  // Divide our variances by the number of pixels in each
  // region.
  for(int32 i=0; i<num_regions; i++) {
    regions[i].var_r /= regions[i].num_pixels;
  }
  
}

void BWBallDetector::calcRegionScores() {

  double temp;

  for(int32 i=0; i<num_regions; i++) {

    if((regions[i].max_x - regions[i].min_x >= 3) &&
       (regions[i].max_y - regions[i].min_y >= 3)){

      // Calculate scores for determining whether a ball is up close to
      // us or not.
      regions[i].up_close_score = 0.0;

      double white_accum = 0.0;
      double black_accum = 0.0;
      int first_offset, second_offset;
      int start_x, end_x, start_y, end_y;
      start_x = (int)(regions[i].fit_x - regions[i].fit_r);
      end_x = (int)(regions[i].fit_x + regions[i].fit_r + 1);
      start_y = (int)(regions[i].fit_y - regions[i].fit_r);
      end_y = (int)(regions[i].fit_y + regions[i].fit_r + 1);
      if(start_x < 0) start_x = 0;
      if(start_y < 0) start_y = 0;
      if(end_x > bw_image_width) end_x = bw_image_width;
      if(end_y > bw_image_height) end_y = bw_image_height;

      first_offset = start_y*bw_image_width;
      second_offset = end_y*bw_image_width;
      for(int x=start_x; x<end_x; x++) { 
	white_accum += white_data[first_offset + x];
	white_accum += white_data[second_offset + x];
      }
      
      first_offset = start_y*bw_image_width + start_x;
      second_offset = start_y*bw_image_width + end_x;
      for(int y=start_y; y<end_y; y++) {
	white_accum += white_data[first_offset];
	white_accum += white_data[second_offset];
	first_offset += bw_image_width;
	second_offset += bw_image_width;
      }

      white_accum /= 255.0*2*((end_x - start_x) + (end_y - start_y));

      first_offset = start_y*bw_image_width + start_x;
      for(int y=start_y; y<end_y; y++) {
	for(int x=start_x; x<end_x; x++) {
	  black_accum += black_data[first_offset];
	  first_offset++;
	}

	first_offset += bw_image_width - (end_x - start_x);
      }

      black_accum /= 255.0*(end_x - start_x)*(end_y - start_y);

      if(regions[i].fit_r > 30 ||
	 regions[i].fit_r < 10 ||
	 (regions[i].max_x - regions[i].min_x) < 10 ||
	 (regions[i].max_y - regions[i].min_y) < 10 ||
	 regions[i].num_pixels < 20)
	regions[i].up_close_score = 0.0;
      else
	regions[i].up_close_score = white_accum * black_accum * 
	  regions[i].ring_score *
	  (1.0 - (bw_image_height - regions[i].fit_y)/((double)bw_image_height));

      // Calc ring score
      temp = sqrt(regions[i].var_r)/regions[i].mean_r;
      temp = 1.0 - temp;
      temp = max(temp, 0.0);
      regions[i].scores[BWRegion::ring_score] = temp;

      // Calc square score
      double width = regions[i].max_x - regions[i].min_x;
      double height = regions[i].max_y - regions[i].min_y;
      if(width > height)
	regions[i].scores[BWRegion::square_score] = height/width;
      else
	regions[i].scores[BWRegion::square_score] = width/height;

      // We penalize regions for having a different fit_r and mean_r -
      // a circle with all points present will have equal values for
      // both. (This hurts lines 'cuz they're missing many points)
      double denom = max(regions[i].fit_r,
			 regions[i].mean_r);
      double numer = min(regions[i].fit_r,
			 regions[i].mean_r);

      regions[i].scores[BWRegion::radius_score] = numer/denom;

      // Give extra weight to small radii, but past a certain
      // point, we don't care.
      regions[i].scores[BWRegion::small_radius_score] =
	1.0/sqrt(max(1.0, regions[i].fit_r - 10.0));
      
      // Ideally we'd have a line of pixels forming a perfect
      // circle with Pi*d points in it.
      regions[i].scores[BWRegion::line_to_radius_score] = 
      	regions[i].num_pixels/(2*M_PI*regions[i].fit_r);

      // Take our smallest bounding box dim and compare it's
      // perimeter (if it were the side of a square) to the
      // perimeter of a square with 2*fit_r side length
      numer = regions[i].max_x - regions[i].min_x + 1;
      numer = min(numer, (double)(regions[i].max_y - regions[i].min_y + 1));
      numer *= numer;
      denom = 4*regions[i].fit_r * regions[i].fit_r;

      if(numer > denom) {
	double temp = numer;
	numer = denom;
	denom = temp;
      }

      regions[i].scores[BWRegion::sq_perim_score] = numer/denom;

      // haven't assigned neighbor score, so set to multiplicative identity.
      regions[i].scores[BWRegion::neighbor_score] = 1.0; 
      
      regions[i].individual_score = regions[i].scores[0];
      for(int k=1; k<BWRegion::num_scores; k++) {
	regions[i].individual_score *= regions[i].scores[k];
      }
      
    } else {
      // too few pixels
      regions[i].individual_score = 0;
    }
  }
  
  calcNeighborScores();
}

void BWBallDetector::calcNeighborScores() {
  // Calculate neighbor scores based on having several similar sized
  // regions nearby.
  
  for(int i=0; i<num_regions; i++)
    regions[i].scores[BWRegion::neighbor_score] = 0.0;
  
  for(int r_id=0; r_id<num_regions - 1; r_id++) {
    
    for(int inner=r_id + 1; inner < num_regions; inner++) {

      double r1 = regions[r_id].fit_r;
      double r2 = regions[inner].fit_r;

      // Make sure we have fit data for both regions and
      // reasonable scores for the candiate regions.
      if(r1 < 0 || r2 < 0)
      	continue;

      double max_r = max(r1, r2);
      //      double min_r = min(r1, r2);
      
      double dx = regions[r_id].fit_x - regions[inner].fit_x;
      double dy = regions[r_id].fit_y - regions[inner].fit_y;
      double dist = sqrt(dx*dx + dy*dy);
      
      // The centers should be 1.25 + 2 + 1.25 cm apart (diam of
      // about 2.5 cm on the ball. So let's figure this out as a
      // proportion. 4.5/2.5 = 1.8diameter = 3.6*r
      double r_dev = dist - 3.6*max_r;
      double r_score = sqrt(r_dev*r_dev)/(3.6*max_r);
      r_score = (2.0 - min(r_score, 2.0))/2.0;

      regions[r_id].scores[BWRegion::neighbor_score] +=
	r_score * 
	regions[r_id].individual_score * 
	regions[inner].individual_score;
      regions[inner].scores[BWRegion::neighbor_score] +=
	r_score *
	regions[r_id].individual_score * 
	regions[inner].individual_score;
    }
  }
}

void BWBallDetector::copyData(const uchar *_y_data,
			      const uchar *_u_data,
			      const uchar *_v_data) {
  // FIXME
  // Do I actually need to do a copy? Do we ever write to
  // y_data? On the other hand, not dealing with the planar crap
  // is nice too even if it does cost a copy.

  // Copy data into a local buffer where we can munge it.
  int offset_in = 0;
  int offset_copy = 0;
  for(int y=0; y<bw_image_height; y++) {
    for(int x=0; x<bw_image_width; x++) {
      y_data[offset_copy] = _y_data[offset_in];
      u_data[offset_copy] = _u_data[offset_in];
      v_data[offset_copy++] = _v_data[offset_in++];
    }
    
    // We need to account for how the data is packed on the robot - our array
    // contains a complete row of y, followed by a complete row of u, followed
    // by a complete row of v, and so on.
    offset_in += 2*bw_image_width;
  }
}

void BWBallDetector::run() {
  
  int32 *temp;

  stripStatus(y_data);
  stripStatus(u_data);
  stripStatus(v_data);

  greenFilter(black_data);

  gradientFilter(y_data, region_data);

  // 80 worked well before contrast filter
  // (Note: You need to square the thresholds now since we
  // got rid of the sqrt in gradientFilter for speed)
  threshold(region_data, 70*70, 0, false);

  int32 n_c = findConnectedComponents(region_data, working);
  
  if(n_c > max_regions) {
    printf("Warning: Too many regions found\n");
    n_c = max_regions;
  }
  
  // Swap buffers
  temp = working; working = region_data; region_data = temp;

  gatherRegionStats(region_data, n_c);

  calcRegionScores();
  
  regionVote();

#if 0
  for(int i=0; i<bw_image_width*bw_image_height; i++) {
    if(region_data[i]!=0) {
      working[i] = regions[region_data[i] - 1].up_close_score*100;
      //working[i] = regions[region_data[i] - 1].fit_r + 100;
      if(working[i] > 255)
	working[i] = 255;
    } else {
      working[i] = 0;
    }
  }
  
  threshold(white_data, 200, 0, false);
  nastyPPM(working, 0, 0, 0, 0, 0);
  return;
#endif

#if 0  
  double c = 1.0, ind = 0.0;
  int x0, x1, y0, y1;
  findBall(&x0, &y0, &x1, &y1, &c);
  //  printf("Ball in (%d, %d)-(%d, %d) with confidence %f gs: %f\n",
  //	 x0, y0, x1, y1, c, greenScore(x0, y0, x1, y1));
  /*  printf("%f ind: %f neigh: %f row_score: %f vote: %f\n",
	 ind,
	 regions[(int)ind].individual_score,
	 regions[(int)ind].scores[BWRegion::neighbor_score],
	 regions[(int)ind].scores[BWRegion::mean_row_score],
	 regions[(int)ind].region_votes);
  
  printf("---------- end run\n");
  */
  nastyPPM(y_data, x0, y0, x1, y1, c);
#endif
}

const double neighbor_thresh = 100000.0;
const double individual_thresh = 300.0;

void BWBallDetector::findBall(int *x0, int *y0, 
			      int *x1, int *y1, 
			      double *conf) {

  
  // Make sure we've actually got regions to consider.
  *x0 = *y0 = *x1 = *y1 = 0;
  *conf = 0.0;
  
  if(num_regions<=0)
    return;

  int num_by_neighbor = 0;
  int max_ind_index = 0;
  double max_ind_score = 0;
  
  int group_indexes[3];
  
  for(int i=0; i<3; i++)
    group_indexes[i] = 0;
  
  for(int i=0; i<num_regions; i++) {
    if(regions[i].individual_score > max_ind_score) {
      max_ind_index = i;
      max_ind_score = regions[i].individual_score;
    }
    
    if(regions[i].scores[BWRegion::neighbor_score] > 
       regions[group_indexes[0]].scores[BWRegion::neighbor_score]) {
      
      group_indexes[2] = group_indexes[1];      
      group_indexes[1] = group_indexes[0];
      group_indexes[0] = i;
    } else if(regions[i].scores[BWRegion::neighbor_score] >
	      regions[group_indexes[1]].scores[BWRegion::neighbor_score]) {
      group_indexes[2] = group_indexes[1];      
      group_indexes[1] = i;
    } else if(regions[i].scores[BWRegion::neighbor_score] >
	      regions[group_indexes[2]].scores[BWRegion::neighbor_score]) {
      group_indexes[2] = i;
    }
  }
  
  // Find out how many regions have high neighbor scores
  for(int i=0; i<3; i++)
    if(regions[group_indexes[i]].scores[BWRegion::neighbor_score] >
       neighbor_thresh &&
       regions[group_indexes[i]].individual_score >
       individual_thresh)
      num_by_neighbor++;
  
  double ball_x, ball_y, ball_r;
  ball_x = ball_y = ball_r = 0.0;
  double min_r = 10000000;
  double max_r = -1;
  double conf_right = 0.0;
  if(num_by_neighbor > 0) {
    for(int i=0; i<num_by_neighbor; i++) {
      double temp_conf = 1.0;

      temp_conf *= 
	min(1.0, 
	    regions[group_indexes[i]].individual_score / 
	    (2.0*individual_thresh));

      temp_conf *= 
	min(1.0, 
	    regions[group_indexes[i]].scores[BWRegion::neighbor_score] / 
	    (2.0*neighbor_thresh));
      
      if(temp_conf > conf_right)
	conf_right = temp_conf;
      /*
	printf("ind: %f neigh %f\n",
	       regions[group_indexes[i]].individual_score,
	       regions[group_indexes[i]].scores[BWRegion::neighbor_score]);
      */
      ball_x += regions[group_indexes[i]].fit_x;
      ball_y += regions[group_indexes[i]].fit_y;
      if(regions[group_indexes[i]].fit_r > max_r)
	max_r = regions[group_indexes[i]].fit_r;
      
      if(regions[group_indexes[i]].fit_r < min_r)
	min_r = regions[group_indexes[i]].fit_r;
    }
    
    ball_x /= num_by_neighbor;
    ball_y /= num_by_neighbor;
    
    switch(num_by_neighbor) {
    case 1:
    case 2:
      ball_r = min_r;
      break;
      
      // Finds the median with 3 and comes closer than the min or max
      // for more than 3 regions.
    case 3:
    default:
      ball_r = min_r; // just in case...
      for(int i=0; i<num_by_neighbor; i++) {
	if(regions[group_indexes[i]].fit_r != min_r &&
	   regions[group_indexes[i]].fit_r != max_r) {
	  ball_r = regions[group_indexes[i]].fit_r;
	}
      }
    }
    
    //    printf("[MULTI] USING FIT_R: %f\n",
    //   ball_r);
    
    *conf = conf_right;
    //    *conf = 0.5 + min(0.5, num_by_neighbor/6.0);
    ball_r *= 3.5; // guestimate of pentagon radius to ball radius ratio.
  } 
#if 0
  else {
    
    ball_x = regions[max_ind_index].fit_x;
    ball_y = regions[max_ind_index].fit_y;
    ball_r = regions[max_ind_index].fit_r;

    *conf = max(regions[max_ind_index].individual_score - 200.0, 0.0)/200.0;
    *conf = min(*conf, 1.0);

    printf("[SINGLE] USING FIT_R: %f ind %f neigh %f\n",
	   ball_r, regions[max_ind_index].individual_score,
	   regions[max_ind_index].scores[BWRegion::neighbor_score]);

    ball_r *= 3.5;
  }
#endif
  
  //  *conf = max_ind_index;
  
  *x0 = (int)(ball_x - ball_r);
  *y0 = (int)(ball_y - ball_r);
  *x1 = (int)(ball_x + ball_r);
  *y1 = (int)(ball_y + ball_r);

  //  nastyPPM(y_data, *x0, *y0, *x1, *y1, *conf);
}

void BWBallDetector::nastyPPM(int32 *buf, 
			      int x0, int y0, 
			      int x1, int y1, double c) {

  static int num = 0;
  char filename[100];

  sprintf(filename, "out_%d.ppm", num++);
  FILE *outfile = fopen(filename, "wb");

  fprintf(outfile, "P6\n%d %d\n%d\n", (int)(2*bw_image_width),
	  (int)bw_image_height, 255);
  
  //  for(int i=0; i<bw_image_width*bw_image_height; i++) {

  for(int y=0; y<bw_image_height; y++)
    for(int x = 0; x<2*bw_image_width; x++) {

      uchar data = 0;
      if(x < bw_image_width) 
	data = y_data[y*bw_image_width + x];
      else
	data = buf[y*bw_image_width + x - bw_image_width];

      fwrite(&data, 1, 1, outfile);
      fwrite(&data, 1, 1, outfile);
      //    int x = i % bw_image_width;
      // int y = i/bw_image_width;
      if(x >= x0 && x<= x1 &&
	 y >= y0 && y<= y1 &&
	 c > 0.1) {
	data = (uchar)((255 - c*255)/2.0);
	//    data = y_data[i];
      }
      fwrite(&data, 1, 1, outfile);
    }

  fclose(outfile);

}

void BWBallDetector::normalize(int32 *buf, int32 max_value) {

  int32 d_min, d_max;
  double scale;

  d_min = d_max = buf[0];

  // Find min and max values
  for(int32 i=1; i<bw_image_width * bw_image_height; i++) {
    if(buf[i] < d_min)
      d_min = buf[i];

    if(buf[i] > d_max)
      d_max = buf[i];
  }

  if(d_max - d_min > 0) {
    scale = ((double)max_value)/(d_max - d_min);
  } else {
    scale = 1;
  }

  for(int32 i=0; i<bw_image_width * bw_image_height; i++) {
    buf[i] = (int)((buf[i] - d_min)*scale);
  }
}

double BWBallDetector::greenScore(int32 x0, int32 y0, 
				int32 x1, int32 y1) {

  double retval = 0;

  // bound our rect to the image dimensions
  x0 = max(x0, (int32)0);
  y0 = max(y0, (int32)0);
  x1 = max(x1, (int32)0);
  y1 = max(y1, (int32)0);
  
  x0 = min(x0, bw_image_width - 1);
  y0 = min(y0, bw_image_height - 1);
  x1 = min(x1, bw_image_width - 1);
  y1 = min(y1, bw_image_height - 1);

  for(int y=y0; y<=y1; y++) {
    int offset = y*bw_image_width + x0;

    for(int x=x0; x<=x1; x++) {
      
      double y = y_data[offset];
      double u = 2*u_data[offset] - 255;
      double v = 2*v_data[offset] - 255;
      
      double r = y + u;
      double g = y - .51*u - .19*v;
      double b = y + v;
      
      r = max(r, 0.0);
      g = max(g, 0.0);
      b = max(b, 0.0);

      if(r > 0 && b > 0 && g > 0) {
	if((g*g) > 4*r*b)
	  retval++;
      }
      
      offset++;
    }
  }

  return retval/((x1 - x0 + 1)*(y1 - y0 + 1));
}

void BWBallDetector::hack() {

  // We want dark regions that aren't too colorful
  for(int yc=0; yc<bw_image_height; yc++) {
    int offset = yc*bw_image_width;
    
    row_score_count[yc] = 0;
    for(int x=0; x<bw_image_width; x++) {
      
      double y = y_data[offset];
      double u = 2*u_data[offset] - 255;
      double v = 2*v_data[offset] - 255;
      
      double r = y + u;
      double g = y - .51*u - .19*v;
      double b = y + v;
      
      r = max(r, 0.0);
      g = max(g, 0.0);
      b = max(b, 0.0);
      
      if(r > 0 && b > 0 && g > 0) {
	if(g*g > 4*r*b)
	  row_score_count[yc]+=4;
	else if(y > 180)
	  row_score_count[yc]++;

	if(g*g > 4*r*b ||
	   b*b > 4*r*g ||
	   r*r > 4*g*b) {
	  working[offset] = 255;
	} else {
	  working[offset] = y_data[offset];
	}
      } else {
	working[offset] = y_data[offset];
      }
      
      // Threshold
      if(working[offset] > 80)
	working[offset] = 0;
      else
	working[offset] = 255 - working[offset];

      offset++;
    }
  }
}

void BWBallDetector::medianFilter(int32 *in_buf, int32 *out_buf) {
  
  int data[9];
  for(int x=1; x<bw_image_width - 1; x++) {
    for(int y=1; y<bw_image_height - 1; y++) {
      int base_offset = y*bw_image_width + x;
      int offset = base_offset - (bw_image_width + 1);
      for(int i=0; i<3; i++) {
	data[i] = in_buf[offset + i];
	data[3 + i] = in_buf[offset + bw_image_width + i];
	data[6 + i] = in_buf[offset + 2*bw_image_width + i];
      }

      for(int i=0; i<2; i++) {
	for(int j = i + 1; j<3; j++) {
	  if(data[i] > data[j]) {
	    int temp = data[i];
	    data[i] = data[j];
	    data[j] = temp;
	  }
	}
      }

      out_buf[base_offset] = data[4];
    }
  }
}

void BWBallDetector::stencil(int32 *in_buf, int32 *out_buf) {

  setBorder(out_buf, 0);

  for(int y=1; y<bw_image_height - 1; y++) {
    for(int x=1; x<bw_image_width - 1; x++) {
      int offset = y*bw_image_width + x;

      int sum = in_buf[offset - bw_image_width];
      sum += in_buf[offset + bw_image_width];
      sum += in_buf[offset - 1];
      sum += in_buf[offset + 1];

      if(sum!=(in_buf[offset] << 2))
	out_buf[offset] = in_buf[offset];
      else
	out_buf[offset] = 0;
    }
  }
}


void BWBallDetector::greenFilter(int32 *black_buf) {

  total_green = 0;

  // We want dark regions that aren't too colorful
  for(int yc=0; yc<bw_image_height; yc++) {
    int offset = yc*bw_image_width;
    
    row_score_count[yc] = 0;
    for(int x=0; x<bw_image_width; x++) {
      
      double y = y_data[offset];
      double u = 2*u_data[offset] - 255;
      double v = 2*v_data[offset] - 255;
      
      double r = y + u;
      double g = y - .51*u - .19*v;
      double b = y + v;
      
      if(r > 0 && b > 0 && g > 0) {
	if(g*g > 4*r*b) {
	  total_green++;
	  row_score_count[yc]+=4;
	} else if(y > 180) {
	  row_score_count[yc]++;
	}
	
	if(g*g > 4*r*b ||
	   b*b > 4*r*g ||
	   r*r > 4*g*b) {
	  black_buf[offset] = 255;
	  white_data[offset] = 0;
	} else {
	  white_data[offset] = y_data[offset];
	  black_buf[offset] = y_data[offset];
	}
      } else {
	black_buf[offset] = y_data[offset];
	white_data[offset] = 0;
      }
      
      black_buf[offset] = 255 - black_buf[offset];

      offset++;
    }
  }
}

void BWBallDetector::regionVote() {
  
  int32 *sort_buf = working; // give working a sane name

  // Zero vote buffer
  for(int32 i=0; i<num_regions; i++)
    regions[i].region_votes = 0;

  int32 num_above_thresh;
  for(int32 score_id=0; score_id<BWRegion::num_scores; score_id++) {
    
    // Find regions with scores[score_id] above their threshold.
    num_above_thresh = 0;
    for(int32 region_id=0; region_id<num_regions; region_id++) {
      if(regions[region_id].scores[score_id] > 
	 score_thresholds[score_id])
	sort_buf[num_above_thresh++] = region_id;
    } 
    
    // Okay, now sort the region ids by their score.
    sortByRegionScore(sort_buf, num_above_thresh, score_id);

    // Now add up the votes from this score attribute
    for(int32 region_id=0; region_id < num_above_thresh; region_id++) {
      regions[sort_buf[region_id]].region_votes += num_regions - region_id;
    }
  }

  // Now we need to normalize the region votes.
  for(int region_id=0; region_id<num_regions; region_id++)
    regions[region_id].region_votes /= (num_regions*BWRegion::num_scores);
}

void BWBallDetector::sortByRegionScore(int32 *buf,
				       int32 count,
				       int32 score_id) {
  int32 temp;

  for(int i=0; i<count - 1; i++) {
    for(int j=i+1; j<count; j++) {
      if(regions[buf[i]].scores[score_id] <
	 regions[buf[j]].scores[score_id]) {
	temp = buf[i];
	buf[i] = buf[j];
	buf[j] = temp;
      }
    }
  }
}

double BWBallDetector::ballUnderNose() {
  return ((double)total_green)/(bw_image_width*bw_image_height);
}