/* 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 INCLUDED_DetectBall_h
#define INCLUDED_DetectBall_h

#include "colors.h"
#include "Vision.h"
#include "../headers/Geometry.h"

static const bool FitEllipseToBall = true;

#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
static bool DrawBallEdges     = false;
#endif

//#define DEBUG_ELLIPSE_FIT

#ifdef DEBUG_ELLIPSE_FIT
static const bool DebugBallScan       = true;
static const bool DebugBallEllipseFit = true;
#else
static const bool DebugBallScan       = false;
static const bool DebugBallEllipseFit = false;
#endif

static const double min_edge_magnitude = 4.5;
static const double l_min_edge_magnitude = 2.5;
static const int cnt=9; // number of points used in line fit
// number of points to side to make sure local maximum is greater than
static const int max_window=5;
static const int NumEllipseFitsToTry = 40;

double CalcDeterminant(bool &ok,int n_rc,double *mat);
double distToEllipse(double *coeffs,vector2f cen,vector2i edge_pt);
double evaluateEllipseError(double *coeffs,vector2f cen,int num_edge_pts,vector2i *edge_pts);
bool IsDuplicate(vector2i new_pt,int num_pts,vector2i *edge_pts);

struct BallEdgeHistStruct
{
  yuv color;
  vector2i loc;
  double der_fit;
  double l_der_fit; // long range der firt
#ifdef DEBUG_ELLIPSE_FIT
  yuvf b; // slope of line fits
  yuvf l_b;
#endif
};

// summary statistics for least squares line fitting
struct LineFit
{
  double xx,xy,x,y;

  void init()
  {
    xx = xy = x = y = 0.0;
  }

  void add(double _x,double _y)
  {
    xx += sq(_x);
    xy += _x*_y;
    x  += _x;
    y  += _y;
  }

  void remove(double _x,double _y)
  {
    xx -= sq(_x);
    xy -= _x*_y;
    x  -= _x;
    y  -= _y;
  }

  double slope(int cnt)
  {
    return (cnt*xy - x*y) / (cnt*xx - sq(x));
  }
};

template<class Image>
struct FindBallEdgeCallbackParams
{
  static const int QueueSize=32;//30;
  // index into the queue
  // idx must be in [-QueueSize,+inf)
  static inline int IdxQueue(int idx)
  {
    return ((unsigned int)(idx + QueueSize)) & (QueueSize-1);
  }

  Image *img;
  BallEdgeHistStruct *histQueue;
  int queueIdx;
  vector2i lastPt;
  vector2i endScanPt;
  bool foundEdge;
  int pixCnt;

  LineFit lfY,lfU,lfV;
  LineFit lLfY,lLfU,lLfV;
  double maxDer;
  double lMaxDer;

  FindBallEdgeCallbackParams();
  ~FindBallEdgeCallbackParams();
  void init(Image &_img);
};

template<class Image>
FindBallEdgeCallbackParams<Image>::FindBallEdgeCallbackParams()
{
  histQueue = NULL;
  img = NULL;
  foundEdge = false;
}

template<class Image>
FindBallEdgeCallbackParams<Image>::~FindBallEdgeCallbackParams()
{
  delete[] histQueue;
}

template<class Image>
void FindBallEdgeCallbackParams<Image>::init(Image &_img)
{
  histQueue = new BallEdgeHistStruct[QueueSize];
  memset(histQueue,0,sizeof(BallEdgeHistStruct)*QueueSize);
  queueIdx = 0;

  img = &_img;

  pixCnt = 0;

  lfY.init();
  lfU.init();
  lfV.init();
  lLfY.init();
  lLfU.init();
  lLfV.init();

  maxDer  = 0.0;
  lMaxDer = 0.0;
}

template<class Image>
struct FindBallEdgeCallback
{
  typedef FindBallEdgeCallbackParams<Image> Params;

  bool operator()(int x, int y, FindBallEdgeCallbackParams<Image> *params)
  {
    yuv color_here;
    int queue_size = FindBallEdgeCallbackParams<Image>::QueueSize;
    BallEdgeHistStruct *hist_queue;
    int queue_idx;
    
    hist_queue = params->histQueue;
    queue_idx = params->queueIdx;

    color_here = params->img->get(x,y);

    bool add=true;

    // if this is first pixel, fill in queue
    if(params->pixCnt < 1){
      // fill queue with copies of this pixel
      BallEdgeHistStruct *hist;
      for(int i=0; i<queue_size; i++){
        hist = &hist_queue[i];
        hist->color = color_here;
        hist->loc.set(x,y);
      }
      // initialize line fits with copies of this pixel
      for(int i=-(cnt-1); i<=0; i++){
        params->lfY.add(i,color_here.y);
        params->lfU.add(i,color_here.u);
        params->lfV.add(i,color_here.v);
      }
      for(int i=-2*(cnt-1); i<=0; i++){
        params->lLfY.add(i,color_here.y);
        params->lLfU.add(i,color_here.u);
        params->lLfV.add(i,color_here.v);
      }
      params->pixCnt = 1;
    }
         
    BallEdgeHistStruct *hist,*l_hist;
    hist = &hist_queue[queue_idx];

    // add to history
    hist->color = color_here;
    hist->loc.set(x,y);

    double b_y=0,b_u=0,b_v=0;
    double l_b_y=0,l_b_u=0,l_b_v=0;

#define CALC_SLOPE(store,lf,dim,cs_cnt) \
    { \
      BallEdgeHistStruct *hist; \
      /* subtract off previous pixel */ \
      hist = &hist_queue[Params::IdxQueue(queue_idx-(cs_cnt))]; \
      lf.remove(params->pixCnt-(cs_cnt),hist->color.dim); \
      /* add in new pixel */         \
      hist = &hist_queue[ queue_idx                                  ]; \
      lf.add   (params->pixCnt,         hist->color.dim); \
       \
      store = lf.slope(cs_cnt); \
    }

    CALC_SLOPE(b_y,params->lfY,y,cnt);
    CALC_SLOPE(b_u,params->lfU,u,cnt);
    CALC_SLOPE(b_v,params->lfV,v,cnt);

    CALC_SLOPE(l_b_y,params->lLfY,y,2*cnt-1);
    CALC_SLOPE(l_b_u,params->lLfU,u,2*cnt-1);
    CALC_SLOPE(l_b_v,params->lLfV,v,2*cnt-1);

    params->pixCnt++;

    hist = &hist_queue[Params::IdxQueue(queue_idx-cnt/2)];
    hist->der_fit = 0.25*(b_y>0 ? b_y : 0) + abs(b_u) + abs(b_v);
#ifdef DEBUG_ELLIPSE_FIT
    if(DebugBallEllipseFit)
      hist->b.set(b_y,b_u,b_v);
#endif

    l_hist = &hist_queue[Params::IdxQueue(queue_idx-cnt-1)];
    l_hist->l_der_fit = 0.25*(l_b_y>0 ? l_b_y : 0) + abs(l_b_u) + abs(l_b_v);
#ifdef DEBUG_ELLIPSE_FIT
    if(DebugBallEllipseFit)
      l_hist->l_b.set(l_b_y,l_b_u,l_b_v);
#endif

    bool edge=false;
    bool edge_so_far=false;

    bool long_fit=false;

    if(hist->der_fit > params->maxDer)
      params->maxDer = hist->der_fit;
    if(l_hist->l_der_fit > params->lMaxDer)
      params->lMaxDer = l_hist->l_der_fit;

    double der_fit;
    der_fit = hist_queue[Params::IdxQueue(queue_idx-cnt/2-max_window)].der_fit;
    edge = (der_fit > min_edge_magnitude);
    // make sure this local maximum is maximum over at least a little range
    edge = edge && (der_fit >= params->maxDer);
    edge_so_far = edge_so_far || edge;

    if(!edge_so_far){
      der_fit = hist_queue[Params::IdxQueue(queue_idx-cnt-1-max_window)].l_der_fit;
      edge = (der_fit > l_min_edge_magnitude);
      // make sure this local maximum is maximum over at least a little range
      edge = edge && (der_fit >= params->lMaxDer);
      long_fit = edge;
    }
    edge_so_far = edge_so_far || edge;

    //pprintf(TextOutputStream,"%d %d %d %d %d %d %d %d %d %d %d %d %g %g %g\n",x,y,YUVCOMP(color_here),YUVCOMP(hist->der),YUVCOMP(hist_prev->der_med),edge_so_far?1:0,b_y,b_u,b_v);
#ifdef DEBUG_ELLIPSE_FIT
    if(DebugBallEllipseFit)
      pprintf(TextOutputStream,"%d %d %d %d %d %g %g %g %g %d %g %g %g %g\n",
              x,y,YUVCOMP(color_here),
              hist->der_fit,YUVCOMP(hist->b),
              edge_so_far?1:0,
              l_hist->l_der_fit,YUVCOMP(l_hist->l_b));
#endif
    
    //edge_so_far = false;
    if(edge_so_far){
      if(long_fit)
        params->lastPt = hist_queue[Params::IdxQueue(queue_idx-cnt-1-max_window)].loc;
      else
        params->lastPt = hist_queue[Params::IdxQueue(queue_idx-cnt/2-max_window)].loc;
      params->foundEdge = true;
      add = false;
    }else{
      params->queueIdx = (params->queueIdx + 1) % queue_size;
    }
    params->endScanPt.set(x,y);

    return add;
  }
};

template<class Image>
int Vision::scanForBallEdge(vector2i &edge,vector2i &scan_end,vector2i start,vector2i dir,Image &img)
{
  edge = start;

  FindBallEdgeCallback<Image> ball_edge_cb;
  FindBallEdgeCallbackParams<Image> ball_edge_cb_params;

  ball_edge_cb_params.init(img);

  drawLine2(V2COMP(start),V2COMP(dir),ball_edge_cb,&ball_edge_cb_params);

  edge = ball_edge_cb_params.lastPt;
  scan_end.set(V2COMP(ball_edge_cb_params.endScanPt));

  return ball_edge_cb_params.foundEdge ? 1 : 0;
}

struct CHEdge {
  int size; // monotonically increases with length of edge
  int vertex_id1; // vertex id of first vertex of edge
  int vertex_id2; // vertex id of second vertex of edge

  bool operator<(const CHEdge &x) const
  {
    //return size < x.size;
    //return (vertex_id2 - vertex_id1)<(x.vertex_id2 - x.vertex_id1);
    return size*(vertex_id2 - vertex_id1)<size*(x.vertex_id2 - x.vertex_id1);
  }
};

template<class Image>
void Vision::fitBallEllipse(EllipseFit *ellipse,region *or_reg,Image &img)
{
  static const int NumScanDirs   = 32;

  vector2i edge_pts[NumScanDirs+1];
  int num_edge_pts;

  vector2f or_cen;
  vector2i or_cen_i;

  or_cen.set(or_reg->cen_x,or_reg->cen_y);
  or_cen_i.set((int)rint(or_cen.x),(int)rint(or_cen.y));

  if(DebugBallEllipseFit)
    pprintf(TextOutputStream,"cen=(%g,%g) size=%d\n",
            V2COMP(or_cen),or_reg->area);

  std::map<int,vector2i> vertices;
  std::vector<CHEdge> ch_edges; // maintained as heap
  static const int MinVertexId =       0;
  static const int MaxVertexId = 1000000;
  int vertex_id;
  CHEdge ch_edge;
  vector2i left_pt,right_pt;

  // HEY SCOTT,
  // Are these two variables supposed to be static? Why are we clearing
  // data structures that we just created on the stack?
  vertices.clear();
  ch_edges.clear();
  num_edge_pts = 0;

  vector2i edge_pt;
  int edge_type=0;
  vector2i scan_cen_i;
  vector2i scan_start_i;
  vector2i scan_dir_i;
  vector2i scan_end;
  
  scan_cen_i = or_cen_i;
      
#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
  if(DrawBallEdges) {
    rgb c; c.set(255,0,0);
    if(DebugImage) DebugImage->setpixel(V2COMP(scan_cen_i),c);
  }
#endif

  // scan right
  scan_dir_i.set(100,0);
  edge_type = scanForBallEdge(edge_pt,scan_end,scan_cen_i,scan_dir_i,img);
      
  if(DebugBallScan)
    pprintf(TextOutputStream,"scan_cen_i=(%3d,%3d) scan_dir=(%4d,%4d) edge_type=%d edge_pt=(%3d,%3d) scan_end=(%3d,%3d)\n",
            V2COMP(scan_cen_i),V2COMP(scan_dir_i),edge_type,V2COMP(edge_type?edge_pt:vector2i(0,0)),V2COMP(scan_end));
      
  if(edge_type){
    if(!IsDuplicate(edge_pt,num_edge_pts,edge_pts)){
      edge_pts[num_edge_pts] = edge_pt;
      num_edge_pts++;
    }
  }

  right_pt = edge_type?edge_pt:scan_end;
  vertices[MinVertexId] = right_pt;
  vertices[MaxVertexId] = right_pt;

#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
  if(DrawBallEdges) {
    if(edge_type){
      rgb c; c.set(0,255,0);
      if(DebugImage) DebugImage->setpixel(V2COMP(edge_pt),c);
    }
  }
#endif

  // scan left
  scan_dir_i.set(-100,0);
  edge_type = scanForBallEdge(edge_pt,scan_end,scan_cen_i,scan_dir_i,img);
      
  if(DebugBallScan)
    pprintf(TextOutputStream,"scan_cen_i=(%3d,%3d) scan_dir=(%4d,%4d) edge_type=%d edge_pt=(%3d,%3d) scan_end=(%3d,%3d)\n",
            V2COMP(scan_cen_i),V2COMP(scan_dir_i),edge_type,V2COMP(edge_type?edge_pt:vector2i(0,0)),V2COMP(scan_end));
      
  if(edge_type){
    if(!IsDuplicate(edge_pt,num_edge_pts,edge_pts)){
      edge_pts[num_edge_pts] = edge_pt;
      num_edge_pts++;
    }
  }

  vertex_id = (MaxVertexId + MinVertexId)/2;
  left_pt = edge_type?edge_pt:scan_end;
  vertices[vertex_id] = left_pt;

  ch_edge.size = sq(right_pt.x - left_pt.x);
  ch_edge.vertex_id1 = MinVertexId;
  ch_edge.vertex_id2 = vertex_id;
  ch_edges.push_back(ch_edge);
  push_heap(ch_edges.begin(),ch_edges.end());

  ch_edge.vertex_id1 = vertex_id;
  ch_edge.vertex_id2 = MaxVertexId;
  ch_edges.push_back(ch_edge);
  push_heap(ch_edges.begin(),ch_edges.end());

#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
  if(DrawBallEdges) {
    if(edge_type){
      rgb c; c.set(0,255,0);
      if(DebugImage) DebugImage->setpixel(V2COMP(edge_pt),c);
    }
  }
#endif

  while(vertices.size() < (unsigned)NumScanDirs){
    CHEdge edge_to_split;
    int pt1_id,pt2_id;
    vector2i pt1,pt2;
    vector2i mid_pt;
    vector2i new_pt;

    pop_heap(ch_edges.begin(),ch_edges.end());
    edge_to_split = ch_edges.back();
    ch_edges.pop_back();

    pt1_id = edge_to_split.vertex_id1;
    pt2_id = edge_to_split.vertex_id2;

    if(DebugBallScan){
      pprintf(TextOutputStream,"vertices.size()=%u edge_to_split:size=%d id1=%d(%3d,%3d) id2=%d(%3d,%3d)\n",
              vertices.size(),edge_to_split.size,pt1_id,V2COMP(vertices[pt1_id]),pt2_id,V2COMP(vertices[pt2_id]));
      for(uint i=0; i<ch_edges.size(); i++){
        CHEdge *edge = &ch_edges[i];
        pprintf(TextOutputStream,"ch_edges[%d]:size=%d id1=%d(%3d,%3d) id2=%d(%3d,%3d)\n",
                i,edge->size,edge->vertex_id1,V2COMP(vertices[edge->vertex_id1]),edge->vertex_id2,V2COMP(vertices[edge->vertex_id2]));
      }
    }
    
    if(edge_to_split.size < sq(2))
      break;

    pt1 = vertices[pt1_id];
    pt2 = vertices[pt2_id];
    mid_pt = (pt1 + pt2)/2; // mid point (round down .5)

    if(mid_pt.x == 0 ||
       mid_pt.x == img.width-1 ||
       mid_pt.y == 0 ||
       mid_pt.y == img.height-1){
      new_pt = mid_pt;
    }else{
      scan_dir_i = (pt1+pt2) - scan_cen_i*2;
      if(mid_pt.y == scan_cen_i.y)
        scan_dir_i = (pt2 - pt1).perp();

      // FIXME: start scan closer to edge
      static const int off_dist=15;
      if(GVector::sdistance(scan_cen_i,mid_pt) > sq(off_dist)){
        vector2f scan_dir;
        vector2f scan_start;

        scan_dir.set(V2COMP(scan_dir_i));
        scan_dir = scan_dir.norm();

        scan_start.set(V2COMP(mid_pt));
        scan_start -= scan_dir*off_dist;

        scan_start_i.set((int)rint(scan_start.x),(int)rint(scan_start.y));
      }else{
        scan_start_i = scan_cen_i;
      }
      edge_type = scanForBallEdge(edge_pt,scan_end,scan_start_i,scan_dir_i,img);
    
      if(DebugBallScan)
        pprintf(TextOutputStream,"scan_start_i=(%3d,%3d) scan_dir=(%4d,%4d) edge_type=%d edge_pt=(%3d,%3d) scan_end=(%3d,%3d)\n",
                V2COMP(scan_start_i),V2COMP(scan_dir_i),edge_type,V2COMP(edge_type?edge_pt:vector2i(0,0)),V2COMP(scan_end));
    
      if(edge_type){
        if(!IsDuplicate(edge_pt,num_edge_pts,edge_pts)){
          edge_pts[num_edge_pts] = edge_pt;
          num_edge_pts++;
        }
      }

#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
      if(DrawBallEdges) {
        if(edge_type){
          rgb c; c.set(0,255,0);
          if(DebugImage) DebugImage->setpixel(V2COMP(edge_pt),c);
        }
      }
#endif

      new_pt = edge_type?edge_pt:scan_end;
    }

    vertex_id = (pt1_id + pt2_id) / 2;
    vertices[vertex_id] = new_pt;

    ch_edge.size = (new_pt - pt1).sqlength();
    ch_edge.vertex_id1 = pt1_id;
    ch_edge.vertex_id2 = vertex_id;
    ch_edges.push_back(ch_edge);
    push_heap(ch_edges.begin(),ch_edges.end());

    ch_edge.size = (new_pt - pt2).sqlength();
    ch_edge.vertex_id1 = vertex_id;
    ch_edge.vertex_id2 = pt2_id;
    ch_edges.push_back(ch_edge);
    push_heap(ch_edges.begin(),ch_edges.end());
  }

  if(num_edge_pts >= 6){
    double best_error=HUGE_VAL;
    double best_coeffs[6];
    vector2f best_cen(100,100);
    double best_maj_axis=1.0,best_min_axis=1.0;
    double best_angle=0.0;
    int best_pt_idxs[5];

    int edge_pt_idxs[NumScanDirs+1];
    for(int i=0; i<NumScanDirs+1; i++){
      edge_pt_idxs[i] = i;
      if(DebugBallScan || DebugBallEllipseFit)
        if(i<num_edge_pts)
          pprintf(TextOutputStream,"edge_pt[%2d]=(%3d,%3d)\n",
                  i,V2COMP(edge_pts[i]));
    }
    
    for(int fit_count=0; fit_count<NumEllipseFitsToTry; fit_count++){
      double xs[5],xy[5],ys[5],x[5],y[5],ones[5];
      double *cols[6]={xs,xy,ys,x,y,ones};
      int pt_idxs[5];

      // pick a permutation of edge pt idxs
      for(int i=0; i<5; i++){
        int edge_pts_idx = GlobalRandom.uint32() % (num_edge_pts-i);
        swap(edge_pt_idxs[i],edge_pt_idxs[i+edge_pts_idx]);
      }

      for(int i=0; i<5; i++){
        int edge_pts_idx = edge_pt_idxs[i];
        pt_idxs[i] = edge_pts_idx;
        // minus signs convert to right handed coordinate system by working in the +x,-y quadrant
        xs[i] =  sq(edge_pts[edge_pts_idx].x);
        xy[i] = -edge_pts[edge_pts_idx].x*edge_pts[edge_pts_idx].y;
        ys[i] =  sq(edge_pts[edge_pts_idx].y);
        x [i] =  edge_pts[edge_pts_idx].x;
        y [i] = -edge_pts[edge_pts_idx].y;
        ones[i] = 1.0;
        if(DebugBallEllipseFit)
          pprintf(TextOutputStream,"chose i=%d to be edge_pt[%2d]=(%3d,%3d)\n",
                  i,edge_pts_idx,V2COMP(edge_pts[edge_pts_idx]));
      }

      // see http://mathews.ecs.fullerton.edu/n2003/ConicFitMod.html
      // for description of algorithm
      double mat[5*5];
      for(int i=0; i<5*5; i++)
	mat[i] = 0; // paranoid 

      // conic is described by coeff[0]*x^2 + coeff[1]*x*y + coeff[2]*y^2 + coeff[3]*x + coeff[4]*y + coeff[5] = 0
      double coeffs[6];
      for(int i=0; i<6; i++){
        // copy matrix excluding this coefficient
        for(int c=0; c<5; c++){
          int eff_c;
          eff_c = c;
          // don't copy the column corresponding to this coefficient
          if(c >= i)
            eff_c++;
          for(int r=0; r<5; r++){
            mat[r*5+c] = cols[eff_c][r];
          }
        }

        // calculate determinant
        bool ok=false;
        double det;
        det = CalcDeterminant(ok,5,mat);
        //pprintf(TextOutputStream,
        //        "det:[%g,%g,%g,%g,%g;\n"
        //        "     %g,%g,%g,%g,%g;\n"
        //        "     %g,%g,%g,%g,%g;\n"
        //        "     %g,%g,%g,%g,%g;\n"
        //        "     %g,%g,%g,%g,%g]\n"
        //        " = %g\n",
        //        mat[5*0+0],mat[5*0+1],mat[5*0+2],mat[5*0+3],mat[5*0+4],
        //        mat[5*1+0],mat[5*1+1],mat[5*1+2],mat[5*1+3],mat[5*1+4],
        //        mat[5*2+0],mat[5*2+1],mat[5*2+2],mat[5*2+3],mat[5*2+4],
        //        mat[5*3+0],mat[5*3+1],mat[5*3+2],mat[5*3+3],mat[5*3+4],
        //        mat[5*4+0],mat[5*4+1],mat[5*4+2],mat[5*4+3],mat[5*4+4],
        //        det);
        //pprintf(TextOutputStream,
        //        "%g,%g,%g,%g,%g\n"
        //        "%g,%g,%g,%g,%g\n"
        //        "%g,%g,%g,%g,%g\n"
        //        "%g,%g,%g,%g,%g\n"
        //        "%g,%g,%g,%g,%g\n"
        //        " = %g\n",
        //        mat[5*0+0],mat[5*0+1],mat[5*0+2],mat[5*0+3],mat[5*0+4],
        //        mat[5*1+0],mat[5*1+1],mat[5*1+2],mat[5*1+3],mat[5*1+4],
        //        mat[5*2+0],mat[5*2+1],mat[5*2+2],mat[5*2+3],mat[5*2+4],
        //        mat[5*3+0],mat[5*3+1],mat[5*3+2],mat[5*3+3],mat[5*3+4],
        //        mat[5*4+0],mat[5*4+1],mat[5*4+2],mat[5*4+3],mat[5*4+4],
        //        det);
        if(ok){
          coeffs[i] = det;
          if(i%2 == 1)
            coeffs[i] = -coeffs[i];
        }else{
          if(DebugBallEllipseFit)
            pprintf(TextOutputStream,"determinant indeterminable\n");
          coeffs[i] = 0.0;
        }
      }

      if(DebugBallEllipseFit)
        pprintf(TextOutputStream,"conic is: %g*x^2 + %g*x*y + %g*y^2 + %g*x + %g*y + %g = 0\n",
                coeffs[0],coeffs[1],coeffs[2],coeffs[3],coeffs[4],coeffs[5]);

      double det;
      det = sq(coeffs[1]) - 4*coeffs[0]*coeffs[2];
      if(det >= 0){
        if(DebugBallEllipseFit)
          pprintf(TextOutputStream,"conic is not an ellipse, det=%g\n",det);
        continue;
      }

      double angle; // angle of conic
      // HEY SCOTT, do we still use atan2a?
      angle = atan2(coeffs[1],coeffs[0] - coeffs[2]);
      angle /= 2.0;
      if(DebugBallEllipseFit)
        pprintf(TextOutputStream,"conic is an ellipse/circle/point/no curve, angle=%g\n",angle);
    
      double cos_angle,sin_angle;
      double cos2_angle,sin2_angle,cossin_angle;
      // conic is now represented by rot_cs[0]*x'^2 + rot_cs[1]*y'^2 + rot_cs[2]*x' + rot_cs[3]*y' + rot_cs[4] = 0
      // where x' and y' are after the axis are rotated by angle
      double rot_cs[5]; // rotated coefficients
      // FIXME: compute this from coeffs directly instead of taking sin(atan()) and cos(atan())
      cos_angle = cos(angle);
      sin_angle = sin(angle);
      cos2_angle = sq(cos_angle);
      sin2_angle = sq(sin_angle);
      cossin_angle = cos_angle*sin_angle;
      rot_cs[0] = coeffs[0]*cos2_angle + coeffs[1]*cossin_angle + coeffs[2]*sin2_angle;
      rot_cs[1] = coeffs[0]*sin2_angle - coeffs[1]*cossin_angle + coeffs[2]*cos2_angle;
      rot_cs[2] = coeffs[3]*cos_angle + coeffs[4]*sin_angle;
      rot_cs[3] = coeffs[4]*cos_angle - coeffs[3]*sin_angle;
      rot_cs[4] = coeffs[5];

      //double rot_B = 2*(coeffs[2]-coeffs[0])*cossin_angle + coeffs[1]*(cos2_angle - sin2_angle);
      if(DebugBallEllipseFit){
        pprintf(TextOutputStream,"conic is: %g*x'^2 + %g*y'^2 + %g*x' + %g*y' + %g = 0\n",
                rot_cs[0],rot_cs[1],rot_cs[2],rot_cs[3],rot_cs[4]);
        //pprintf(TextOutputStream,"[rot_B=%g]\n",rot_B);
      }

      double rot_cen_x,rot_cen_y;
      vector2f rot_cen,cen;
      // protect against division by zero
      if(rot_cs[0] < 1e-10)
        continue;
      if(rot_cs[1] < 1e-10)
        continue;
      rot_cen_x = -rot_cs[2]/(2*rot_cs[0]);
      rot_cen_y = -rot_cs[3]/(2*rot_cs[1]);
      rot_cen.set(rot_cen_x,rot_cen_y);
      cen = rot_cen.rotate(angle);
    
      if(DebugBallEllipseFit)
        pprintf(TextOutputStream,"rot_cen=(%g,%g) cen=(%g,%g)\n",V2COMP(rot_cen),V2COMP(cen));

      // now represent conic by a*(x'-rot_cen_x)^2 + b*(y'-rot_cen_y) = c
      double a,b,c;
      a = rot_cs[0];
      b = rot_cs[1];
      c = -rot_cs[4] + rot_cs[0]*sq(rot_cen_x) + rot_cs[1]*sq(rot_cen_y);
      if(DebugBallEllipseFit)
        pprintf(TextOutputStream,"conic is: %g*(x'-%g)^2 + %g*(y'-%g)^2 = %g\n",
                a,rot_cen_x,b,rot_cen_y,c);

      double maj_axis,min_axis;
      if(c*a <= 0){
        if(DebugBallEllipseFit)
          pprintf(TextOutputStream,"maj_axis imaginary\n");
        continue;
      }
      if(c*b <= 0){
        if(DebugBallEllipseFit)
          pprintf(TextOutputStream,"min_axis imaginary\n");
        continue;
      }
      maj_axis = sqrt(c/a);
      min_axis = sqrt(c/b);
      if(min_axis > maj_axis){
        swap(min_axis,maj_axis);
        angle += M_PI/2;
      }
      if(DebugBallEllipseFit)
        pprintf(TextOutputStream,"maj_axis=%g min_axis=%g\n",maj_axis,min_axis);

      //// calculate foci
      //vector2f foci1,foci2;
      //double ecc;
      //if(maj_axis >= min_axis)
      //  ecc = sqrt(1-sq(min_axis)/sq(maj_axis));
      //else
      //  ecc = sqrt(1-sq(maj_axis)/sq(min_axis));
      //foci1 = cen + vector2f( ecc,0).rotate(angle);
      //foci2 = cen + vector2f(-ecc,0).rotate(angle);
      //pprintf(TextOutputStream,"foci1=(%g,%g) foci2=(%g,%g)\n",V2COMP(foci1),V2COMP(foci2));
      double ratio_axis_size;
      // HEY SCOTT,
      // Didn't you just swap these if maj wasn't really maj? Doesn't matter tho...
      if(maj_axis >= min_axis)
        ratio_axis_size = maj_axis/min_axis;
      else
        ratio_axis_size = min_axis/maj_axis;
      
      double orig_error,error;
      // + .001 ensures that even zero median error fits are penalized for high eccentricity
      orig_error = evaluateEllipseError(coeffs,cen,num_edge_pts,edge_pts) + .001;
      error = orig_error * ratio_axis_size;
      if(DebugBallEllipseFit)
        pprintf(TextOutputStream,"orig_error=%g ratio=%g error=%g\n",orig_error,ratio_axis_size,error);

      if(error < best_error){
        if(DebugBallEllipseFit)
          pprintf(TextOutputStream,"new best fit\n");
        best_error = error;
        best_cen = cen;
        best_maj_axis = maj_axis;
        best_min_axis = min_axis;
        best_angle = angle;
        for(int i=0; i<6; i++)
          best_coeffs[i] = coeffs[i];
        for(int i=0; i<5; i++)
          best_pt_idxs[i] = pt_idxs[i];
      }
    }

    // convert back to image coordinates
    double best_error_frac;
    best_error_frac = best_error/((best_maj_axis+best_min_axis)/2.0);
    if(best_error < 3.0){
      best_cen.y = -best_cen.y;
      best_angle = -best_angle;
      if(DebugBallEllipseFit)
        pprintf(TextOutputStream,"best: error=%g error_frac=%g cen=(%g,%g) axis=(%g,%g) at angle %g\n",
                best_error,best_error_frac,V2COMP(best_cen),best_maj_axis,best_min_axis,best_angle);

      // fill in ellipse fit structure with best ellipse
      ellipse->cen            = best_cen;
      ellipse->maj_axis       = best_maj_axis;
      ellipse->min_axis       = best_min_axis;
      ellipse->maj_axis_angle = best_angle;
      ellipse->fit_error      = best_error;
      ellipse->num_error_pts  = num_edge_pts-5;
      
#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
      if(DrawBallEdges) {
        rgb c;
        c.set(255,255,0);
        if(DebugImage) 
          for(int i=0; i<5; i++)
            DebugImage->setpixel(V2COMP(edge_pts[best_pt_idxs[i]]),c);
      }
      if(DrawBallEdges) {
        rgb c;
        vector2f major_add,minor_add;
        vector2f s,e;
        vector2i si,ei;
        c.set(0,0,255);
        int num_pts;
        num_pts=32;
        for(int i=0; i<num_pts; i++){
          vector2f pt1,pt2;
          vector2i pt1_i,pt2_i;
          double angle1,angle2;

          angle1 = (i  )*2*M_PI/num_pts;
          angle2 = (i+1)*2*M_PI/num_pts;
          pt1.set(cos(angle1)*best_maj_axis,sin(angle1)*best_min_axis);
          pt2.set(cos(angle2)*best_maj_axis,sin(angle2)*best_min_axis);

          pt1 = pt1.rotate(-best_angle);
          pt2 = pt2.rotate(-best_angle);

          pt1.x += best_cen.x; pt1.y += -best_cen.y;
          pt2.x += best_cen.x; pt2.y += -best_cen.y;

          pt1_i.set((int)rint(pt1.x),-(int)rint(pt1.y));
          pt2_i.set((int)rint(pt2.x),-(int)rint(pt2.y));

          if(DebugImage) DebugImage->draw_line(V2COMP(pt1_i),V2COMP(pt2_i),c);
        }
        c.set(255,0,255);
        major_add.set(best_maj_axis,0.0);
        major_add = major_add.rotate(best_angle);
        minor_add.set(0.0,best_min_axis);
        minor_add = minor_add.rotate(best_angle);
        s = best_cen + major_add;
        e = best_cen - major_add;
        si.set((int)rint(s.x),(int)rint(s.y));
        ei.set((int)rint(e.x),(int)rint(e.y));
        if(DebugImage) DebugImage->draw_line(V2COMP(si),V2COMP(ei),c);
        s = best_cen + minor_add;
        e = best_cen - minor_add;
        si.set((int)rint(s.x),(int)rint(s.y));
        ei.set((int)rint(e.x),(int)rint(e.y));
        if(DebugImage) DebugImage->draw_line(V2COMP(si),V2COMP(ei),c);
      }
#endif
    }
  }
}

template<class image>
void Vision::findBall(VObject *ball,VisionObjectInfo *ball_info,image &img)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findBall",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  static const bool debug_ball = false;
  static const bool debug_conf = false;
  static const bool debug_sort = false;

  static int frame_cnt=0;
#ifdef PLATFORM_LINUX
  static const int print_period=1;
#else
  static const int print_period=90;
#endif
  if(debug_ball)
    frame_cnt = (frame_cnt + 1) % print_period;

  bool print_now = (frame_cnt == 0);

  if(debug_sort){
    DebugClass = 0;
    DebugInfo[0] = '\0';
  }

  color_class_state *orange;
  int n;
  int w,h;
  double conf;
  double d;
  double green_f = 1.0;

  orange=&color[COLOR_ORANGE];

  region *or_reg; // orange region

  or_reg=orange->list;

  ball->confidence = 0.0;
  //if(debug_sort)
  //  sprintf(DebugInfo,"0 0 0");

  if(ballDisabled) return;

  if(!or_reg) return;

  for(n=0; or_reg && n<10; or_reg = or_reg->next, n++) {
    double conf0,conf_square_bbox,conf_area,conf_green,conf_area_bonus;
    double conf_shape;
    double conf_red_v_area;
    double conf_dist_project_error;
    double ellipse_conf_shape;
    int edge;

    w = or_reg->x2 - or_reg->x1 + 1;
    h = or_reg->y2 - or_reg->y1 + 1;
    
    edge = calcEdgeMask(or_reg);

    EllipseFit ellipse;
    bool good_ellipse_fit=false;

    // clear out ellipse fit
    mzero(ellipse);
    ellipse.fit_error = HUGE_VAL;
    
    // possibly fit ellipse to ball
    if(FitEllipseToBall)
      if(n<1 && or_reg->area>=25)
        fitBallEllipse(&ellipse,or_reg,img);

    // check if got good ellipse fit
    good_ellipse_fit = 
      (ellipse.fit_error < 3.0) && 
      (ellipse.maj_axis/ellipse.min_axis < 2.0) && 
      (ellipse.num_error_pts >= 3);
    if(good_ellipse_fit){
      ellipse_conf_shape = ramp_dn(ellipse.fit_error,1.5,1.7);
      if(debug_conf && print_now)
        pprintf(TextOutputStream,"ellipse_conf_shape=%g\n",ellipse_conf_shape);
      ellipse_conf_shape *= ramp_dn(2*ellipse.fit_error / (ellipse.maj_axis + ellipse.min_axis), .15, .20);
      if(debug_conf && print_now)
        pprintf(TextOutputStream,"rel error=%g\n",2*ellipse.fit_error / (ellipse.maj_axis + ellipse.min_axis));
      if(debug_conf && print_now)
        pprintf(TextOutputStream,"ellipse_conf_shape mult=%g\n",ramp_dn(2*ellipse.fit_error / (ellipse.maj_axis + ellipse.min_axis), .15, .20));
      if(debug_conf && print_now)
        pprintf(TextOutputStream,"ellipse_conf_shape=%g\n",ellipse_conf_shape);
      if(ellipse_conf_shape < .5){
        good_ellipse_fit = false;
        if(debug_ball && print_now)
          pprintf(TextOutputStream,"aborting use of ellipse\n",ellipse_conf_shape);
      }
    }
    if(debug_ball && print_now) {
      if(ellipse.fit_error < 100)
        pprintf(TextOutputStream,"ellipse: error=%g num_error_pts=%d cen=(%g,%g) axis=(%g,%g) angle %g\n",
                ellipse.fit_error,ellipse.num_error_pts,V2COMP(ellipse.cen),ellipse.maj_axis,ellipse.min_axis,ellipse.maj_axis_angle);
      if(good_ellipse_fit)
        pprintf(TextOutputStream,"using ellipse fit for region:cen=(%g,%g) area=%d w=%d h=%d\n",
                or_reg->cen_x,or_reg->cen_y,or_reg->area,w,h);
    }
        
    conf0 = (((w >= 3) && (h >= 3) && (or_reg->area >= 7)) ? 1.0 : 0.0);
    conf_square_bbox = 
      edge ?
      gaussian_with_min(pct_from_mean(w,h) / .6, 1e-3) :
      gaussian_with_min(pct_from_mean(w,h) / .2, 1e-3);
    conf_area =
      edge ?
      gaussian_with_min(pct_from_mean(M_PI*w*h/4.0,or_reg->area) / .6, 1e-3) :
      gaussian_with_min(pct_from_mean(M_PI*w*h/4.0,or_reg->area) / .2, 1e-3);
    conf_area_bonus = ((double)or_reg->area / 1000);
    conf_shape = conf_square_bbox * conf_area + conf_area_bonus;
    conf_red_v_area = 1.0;
    conf_green = 1.0;

    if(good_ellipse_fit){
      if(ellipse.num_error_pts >= 5)
        conf0 = (((ellipse.min_axis >= 1.5) && (M_PI * ellipse.min_axis * ellipse.maj_axis >= 7) && (or_reg->area>=7)) ? 1.0 : 0.0);
      conf_shape = 1.0;
    }

    conf =
      conf0 *
      conf_shape *
      conf_red_v_area *
      conf_green;

    if(conf > 1.0) conf = 1.0;
    
    if(debug_conf && print_now) {
      pprintf(TextOutputStream,"conf0 %g conf_square_bbox %g conf_area %f conf_area_bonus %f conf_shape %f (conf=%f)\n",
              conf0,conf_square_bbox,conf_area,conf_area_bonus,conf_shape,conf);
    }

    if(conf <= ball->confidence)
      continue;

    vector3d ball_dir; // direction of ball from camera in robot coordinates
    if(good_ellipse_fit){
      ball_dir = getPixelDirection(V2COMP(ellipse.cen));
    }else{
      ball_dir = getPixelDirection(or_reg->cen_x,or_reg->cen_y);
    }
    
    if(debug_ball && print_now) {
      pprintf(TextOutputStream,"candidate ball n%d cen (%f,%f) area %d bbox (%d,%d)-(%d,%d) conf %f\n",
              n,or_reg->cen_x,or_reg->cen_y,or_reg->area,
              or_reg->x1,or_reg->y1,or_reg->x2,or_reg->y2,
              conf);
    }

    float area;
    if(good_ellipse_fit){
      area = M_PI * ellipse.maj_axis * ellipse.min_axis;
    }else{
      area = or_reg->area;
    }

    d = sqrt((FocalDist * FocalDistV) * (M_PI * BallRadius * BallRadius) / area);
    
    vector3d intersect_ball_loc(0.0,0.0,0.0),pixel_size_ball_loc;
    vector2d pixel_size_ball_loc_2d;

    bool intersects=false;
    vector3d intersection_pt(0.0,0.0,0.0);

    double ground_dist,ground_dist_sq;
    double ball_to_cam_height;
    ball_to_cam_height = camera_loc.y - BallRadius;
    ground_dist_sq = d * d  -  ball_to_cam_height * ball_to_cam_height;
    ground_dist = (ground_dist_sq > 0.0 ? sqrt(ground_dist_sq) : 0.0);
    vector2d ball_dir_2d(ball_dir.x,ball_dir.y);
    pixel_size_ball_loc_2d = vector2d(camera_loc.x,camera_loc.y) + ball_dir_2d * ground_dist;
    pixel_size_ball_loc.set(pixel_size_ball_loc_2d.x,pixel_size_ball_loc_2d.y,BallRadius);
    pixel_size_ball_loc.set(pixel_size_ball_loc_2d.x,pixel_size_ball_loc_2d.y,BallRadius);
    //pixel_size_ball_loc = camera_loc + ball_dir * d;

    intersects=GVector::intersect_ray_plane(camera_loc,ball_dir,
                                            vector3d(0.0,0.0,BallRadius),vector3d(0.0,0.0,1.0),
                                            intersection_pt);
    if(intersects) {
      intersect_ball_loc = intersection_pt;
    }

    double ang_diff=0.0;
    conf_dist_project_error = 0.5;
    double ang_diff_cos=0.0;
    vector3d cam_to_ball;
    
    cam_to_ball = pixel_size_ball_loc-camera_loc;
    ang_diff_cos = cam_to_ball.norm().dot(ball_dir);
    ang_diff = acos(ang_diff_cos);
    //if(ball_dir.z > cam_to_ball.z)
    //  ang_diff = -ang_diff;
      
    double allowed_slop=1.5;
    double angle_down; // angle away from down
    angle_down = acos(-ball_dir.z);
    if(debug_conf && print_now)
      pprintf(TextOutputStream,"angle down=%g\n",angle_down);
    if(edge){
      allowed_slop = 2.0 + 1.0*ramp_dn(angle_down,1.0,1.5);
    }else{
      allowed_slop = 1.5 + 1.5*ramp_dn(angle_down,1.0,1.5);
    }
    if(good_ellipse_fit){
      allowed_slop *= 1.5;
    }
    conf_dist_project_error = uniform_with_gaussian_tails(ang_diff / .1, allowed_slop, .6);
    // we don't care about angle error if the location of the ball is well determined
    double dist_error,scaled_proj_error1,scaled_proj_error2,scaled_proj_errorf;
    dist_error = GVector::distance(pixel_size_ball_loc,intersect_ball_loc);
    scaled_proj_error1 = 1.0 - (1.0 - conf_dist_project_error)*ramp_up(dist_error,50.0,80.0);
    if(good_ellipse_fit)
      scaled_proj_error2 = 1.0 - (1.0 - scaled_proj_error1)*ramp_dn(or_reg->area,600,1000);
    else
      scaled_proj_error2 = scaled_proj_error1;
    scaled_proj_errorf = scaled_proj_error2;
    if(debug_conf && print_now) {
      pprintf(TextOutputStream,"ang_diff_cos=%g ang_diff=%g cam_to_ball=(%g,%g,%g) cam_to_ball_norm=(%g,%g,%g) ball_dir=(%g,%g,%g)\n",
              ang_diff_cos,ang_diff,
              cam_to_ball.x,cam_to_ball.y,cam_to_ball.z,
              cam_to_ball.norm().x,cam_to_ball.norm().y,cam_to_ball.norm().z,
              ball_dir.x,ball_dir.y,ball_dir.z);
      pprintf(TextOutputStream,"proj_error=%g scaled_proj_error:1=%g 2=%g f=%g\n",
              conf_dist_project_error,scaled_proj_error1,scaled_proj_error2,scaled_proj_errorf);
      pprintf(TextOutputStream,"pixel_size_ball_loc=(%g,%g,%g) intersect_ball_loc=(%g,%g,%g)\n",
              V3COMP(pixel_size_ball_loc),V3COMP(intersect_ball_loc));
    }
    conf_dist_project_error = scaled_proj_errorf;
    
    conf *= conf_dist_project_error;
    
    if(conf > 1.0) conf = 1.0;
    
    if(debug_ball && print_now) {
      pprintf(TextOutputStream,"|conf ang_diff %f (conf=%f)",
              conf_dist_project_error,conf);
    }
    
    if(conf <= ball->confidence)
      continue;
    
    int sx[2],sy[2]; // scan bounding coordinates;
    bool edge_x[2],edge_y[2]; // true if scan coordinate went off edge
    static const int scan_distance = 3;
    static int color_cnts[MAX_COLORS];
    int scan_pixels;
    int good_value;

    if(good_ellipse_fit){
      vector2f ellipse_pt;
      vector2f axis;
      vector2f axis_dir;
      float x_dist,y_dist;

      axis.set(ellipse.maj_axis,ellipse.min_axis);

      axis_dir.set(1.0,0.0);
      // rotate by -maj_axis_angle after negating maj_axis_angle to convert to a right handed coordinate system
      axis_dir.rotate(ellipse.maj_axis_angle);

      x_dist = fabs(axis_dir.dot(axis));

      axis_dir = axis_dir.perp();

      y_dist = fabs(axis_dir.dot(axis));

      sx[0] = (int)rint(ellipse.cen.x - x_dist) - scan_distance;
      sx[1] = (int)rint(ellipse.cen.x + x_dist) + scan_distance;

      sy[0] = (int)rint(ellipse.cen.y - y_dist) - scan_distance;
      sy[1] = (int)rint(ellipse.cen.y + y_dist) + scan_distance;
    }else{
      sx[0] = or_reg->x1 - scan_distance;
      sx[1] = or_reg->x2 + scan_distance;
      sy[0] = or_reg->y1 - scan_distance;
      sy[1] = or_reg->y2 + scan_distance;
    }

    edge_x[0] = (sx[0] < 0);
    if(edge_x[0]) sx[0] = 0;

    edge_x[1] = (sx[1] > width-1);
    if(edge_x[1]) sx[1] = width-1;

    edge_y[0] = (sy[0] < 0);
    if(edge_y[0]) sy[0] = 0;

    edge_y[1] = (sy[1] > height-1);
    if(edge_y[1]) sy[1] = height-1;
    
    scan_pixels = 0;
    for(int color_idx=0; color_idx<MAX_COLORS; color_idx++)
      color_cnts[color_idx]=0;
    
    // do horizontal strips
    for(int side=0; side<2; side++) {
      if(!edge_y[side]) {
        scan_pixels+=addToHistHorizStrip(sy[side],sx[0],sx[1],color_cnts);
      }
    }
    
    // do vertical strips
    for(int side=0; side<2; side++) {
      if(!edge_x[side]) {
        scan_pixels+=addToHistVertStrip(sx[side],sy[0],sy[1],color_cnts);
      }
    }

    int non_robot_fringe=0;
    non_robot_fringe += 3*color_cnts[COLOR_GREEN ];
    //non_robot_fringe += 3*color_cnts[COLOR_WHITE ];
    non_robot_fringe += 3*color_cnts[COLOR_BLUE  ];
    non_robot_fringe -=   color_cnts[COLOR_RED   ];
    non_robot_fringe -= 3*color_cnts[COLOR_YELLOW]/2;

    if(!ballGreenFiltersDisabled) {
      conf_red_v_area = 1 + (((double)non_robot_fringe) / or_reg->area);
      conf_red_v_area = bound(conf_red_v_area,0.0,1.0);
    }
      
    good_value = 0;
    good_value += 2*color_cnts[COLOR_GREEN ];
    good_value += 3*color_cnts[COLOR_WHITE ]/2;
    good_value +=   color_cnts[COLOR_RED   ];
    good_value +=   color_cnts[COLOR_BLUE  ];
    good_value -=   color_cnts[COLOR_YELLOW]/2;

    green_f = max(good_value+1,1) / (scan_pixels + 1.0);
    green_f = bound(green_f,0.0,1.0);
    conf_area_bonus = ((double)or_reg->area / 1000);
    green_f += conf_area_bonus;
    if(green_f > 1.0)
      green_f = 1.0;
    if(!ballGreenFiltersDisabled)
      conf_green = green_f;
    
    conf *= conf_green;
    conf *= conf_red_v_area;

    if(conf > 1.0) conf = 1.0;

    if(debug_conf && print_now){
      pprintf(TextOutputStream,"scanned x=[%d(%d),%d(%d)] y=[%d(%d),%d(%d)]\n",
              sx[0],!edge_x[0],sx[1],!edge_x[1],sy[0],!edge_y[0],sy[1],!edge_y[1]);
    }
    if(debug_ball && print_now){
      pprintf(TextOutputStream,"|conf red_v_area %f green_f %f (conf=%f) good_value %d g %d w %d r %d b %d y %d scan_pixels %d\n",
              conf_red_v_area,green_f,conf,good_value,
              color_cnts[COLOR_GREEN],color_cnts[COLOR_WHITE],color_cnts[COLOR_RED],color_cnts[COLOR_BLUE],color_cnts[COLOR_YELLOW],
              scan_pixels);
    }

    if(conf <= ball->confidence)
      continue;

    uchar ball_edges;
    double ball_distance;

    ball_edges = calcEdgeMask(or_reg);

    vector3d ball_loc,alt_ball_loc;
    if(good_ellipse_fit){
      if(intersects && intersect_ball_loc.length() < 500.0){
        ball_loc     = intersect_ball_loc;
        alt_ball_loc = pixel_size_ball_loc;
      }else{
        alt_ball_loc = intersect_ball_loc;
        ball_loc     = pixel_size_ball_loc;
      }
    }else if(ball_edges!=0 && intersects){
      ball_loc     = intersect_ball_loc;
      alt_ball_loc = pixel_size_ball_loc;
    }else{
      alt_ball_loc = intersect_ball_loc;
      ball_loc     = pixel_size_ball_loc;
    } 


    ball_distance = hypot(ball_loc.x,ball_loc.y);

    // filter based upon average color if possible, ball is small, and ball is far
    if(red_v_or_valid && 
       ((or_reg->area < 128 && ball_distance >= 1500.0))){
      vector3d ball_color_dir;
      yuv ball_color;

      ball_color = AverageColor(img,rmap,or_reg->run_start);
      ball_color_dir.x = ball_color.y;
      ball_color_dir.y = ball_color.u;
      ball_color_dir.z = ball_color.v;

      double redness; // 0 = orange, 1 = red (can take on valus outside of [0,1])
      redness = normalizedColorLocation(ball_color_dir,red_v_or_dir,red_v_or_base,red_v_or_scale);

      if(debug_conf && print_now)
        pprintf(TextOutputStream,"ball_color (%3d,%3d,%3d) redness %g\n",
                ball_color.y,ball_color.u,ball_color.v,redness);

      if(redness > 0.75){
        if(debug_ball && print_now){
          pprintf(TextOutputStream,"throwing out ball because it is too red, redness=%g\n",redness);
        }
        continue;
      }
    }

    // Decided this is in fact a better ball than we had before so calc and save all the stats for it.
    ball->edge = ball_edges;    

    ball->confidence = conf;
        
    ball->loc = ball_loc;
        
    ball->distance = ball_distance;
        
    findSpan(ball->left,ball->right,or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2);

    ball_info->reg = or_reg;

    if(debug_ball && print_now) {
      pprintf(TextOutputStream,"###found ball, conf %g loc (%g,%g,%g) alt (%g,%g,%g) dist %g left %g right %g edge %d\n",
              ball->confidence,
              ball->loc.x,ball->loc.y,ball->loc.z,
              alt_ball_loc.x,alt_ball_loc.y,alt_ball_loc.z,
              ball->distance,ball->left,ball->right,ball->edge);
    }
    if(debug_sort){
      if(ball->confidence > .4)
        DebugClass = 1;
      //sprintf(DebugInfo,"%8.4f",ball->confidence);
      //if(good_ellipse_fit)
      //  sprintf(DebugInfo,"%.4f %.4f",ellipse.fit_error,2*ellipse.fit_error/(ellipse.maj_axis+ellipse.min_axis));
      //else
      //  sprintf(DebugInfo,"0.0 0.0");
      //sprintf(DebugInfo,"%d %d %d",or_reg->area,w,h);
    }
  }
}

#endif