/* 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.
  ========================================================================= */

#ifndef BWBallDetector_h
#define BWBallDetector_h

#include <math.h>

#include "../headers/DogTypes.h"

// I couldn't find these in Vision.h. Weird.
const int32 bw_image_width = 176;
const int32 bw_image_height = 144;

const int max_regions = 10000;

const static double score_thresholds[] =
  { /* l2r */ 0.3,
    /* sq_perim */ 0.3,
    /* small_r */ 0.9,
    /* r_score */ 0.6,
    /* mean_row */ 75.0,
    /* square */ 0.4,
    /* ring */ 0.3,
    /* neighbor */ 5.0
  };

/* Information about each region */
struct BWRegion {
  int32 num_pixels;

  double mean_x;
  double mean_y;
  // The "radius" is the mean distance of the pixels from the
  // centroid of the region
  double mean_r;
  double var_r;
  
  // The bounding box of this region
  int32 min_x, min_y;
  int32 max_x, max_y;

  // Various sums that we track in order to fit a circle to each
  // region. The actual algorithm is in a paper by Dale Umbach and Kerry Jones
  // in IEEE Transactions on Instrumentation and Measurement from 2000.
  double sum_x, sum_y;
  double sum_xx, sum_yy, sum_xy;
  double sum_xxx, sum_xxy, sum_xyy, sum_yyy; 

  double fit_x, fit_y, fit_r;

  enum score_offsets { line_to_radius_score = 0, sq_perim_score, 
		       small_radius_score, radius_score, mean_row_score,
		       square_score, ring_score, neighbor_score, 
		       num_scores  };
  
  double scores[num_scores];
  double region_votes;

  double up_close_score;
#if 0
  // .9 is not unheard of for very good ones. See around .6 typically.
  // .3 is a good threshold.
  double line_to_radius_score;

  // .9 for good ones, falls as low as .5 for reasonable pentagons.
  // threshold around .3 reasonable.
  double sq_perim_score;

  // Always 1.0 for pentagons. Thresholding at .9 is good.
  double small_radius_score;

  // Virtually always .99 for pentagons. Sometimes falls into
  // .80s or .70's. Thresholding at .7 or .6 reasonable
  double radius_score;
  
  // 500 shows up for good regions on the field.
  // Get as low as 100 for regions against the wall
  // Maybe 75 as a threshold (or ditch ones against wall
  // and do 200 (see 300 for close up regions against green 'cuz
  // so much ball is present)
  double mean_row_score;

  // Really good ones will be .95-1.0
  // See as low as .5 for some pentagons
  // .4 is a good threshold.
  double square_score;

  // Really good rings have .8. Weaker rings .4 - .5
  // 0.3 is a good threshold
  double ring_score;

  // When it's good, it's up above 25. When it's bad, it's around .6.
  // 1, 5, or 10 might be reasonable thresholds.
  double neighbor_score;
#endif
  double individual_score;
};

class BWBallDetector {
  
 private:
  // Values from YUV bitmap
  int32 *y_data;
  int32 *u_data;
  int32 *v_data;
  int32 *black_data;
  int32 *white_data;

  // Map pixels to regions
  int32 *region_data;

  /* Many operations require making a copy of the data while you
     operate on it. Let's just statically allocate a buffer of the
     same size as the data array for these operations. Then we'll just swap
     the buffers once we're done instead of copying or allocating 
     new memory.
  */
  int32 *working;

  BWRegion *regions;
  int32 num_regions;

  int32 *row_score_count;
  int32 total_green;
 public:
  BWBallDetector();
  ~BWBallDetector();

  // Copy the data from _y_data and process it to find a black
  //   and white ball (if one is in the image).
  void copyData(const uchar *_y_data,
		const uchar *_u_data,
		const uchar *_v_data);

  void run();

  // Replace all points below (or above) value with replace_with.
  // clear_above should be false to reset points below and true
  // for points above.
  void threshold(int32 *buf, int32 value, 
		 int32 replace_with, bool clear_above);

  // Replace each point in the image with the approximate
  // magnitude of the gradient at that point.
  void gradientFilter(int32 *in_buf, int32 *out_buf);

  // Returns the number of components found. Components are numbered
  // 1 - n, NOT 0->n - 1. The 0 label corresponds to background.
  int32 findConnectedComponents(int32 *in_buf, int32 *out_buf);

  void gatherRegionStats(int32 *buf,
			 int32 _num_regions);
  void calcRegionScores();
  
  void findBall(int *x0, int *y0, int *x1, int *y1, double *conf); //,
  //	int *up_close_x, int *up_close_y, double *up_close_conf);

  double ballUnderNose();

 private:
  
  // Inits the points in buf corresponding to the outermost
  // rectangle of pictures in the image - it's intended as
  // an initialization step for filters that don't set these
  // pixels.
  void setBorder(int32 *buf, int32 value);
  void stripStatus(int32 *buf);
  void nastyPPM(int32 *buf, int x0, int y0, int x1, int y1, double c);
  void normalize(int32 *buf, int32 max_value);
  double greenScore(int32 x0, int32 y0, int32 x1, int32 y1);
  void medianFilter(int32 *in_buf, int32 *out_buf);
  void stencil(int32 *in_buf, int32 *out_buf);
  void hack();
  void greenFilter(int32 *black_buf);
  void regionVote();
  void sortByRegionScore(int32 *buf,
			 int32 count,
			 int32 score_id);
  void calcNeighborScores();
};


#endif