/* 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 <stdio.h>
#include <iostream>
#include <algorithm>

using std::cout;
using std::endl;

#ifdef PLATFORM_APERIOS
#include <MCOOP.h>
#endif

#include "../headers/CircBufPacket.h"
#include "../headers/Config.h"
#include "../headers/Dictionary.h"
#include "../headers/FileSystem.h"
#include "../headers/gvector.h"
#include "../headers/random.h"
#include "../headers/Reporting.h"
#include "../headers/Sensors.h"
#include "../headers/Utility.h"
#include "../headers/Util.h"
#include "../Motion/Kinematics.h"
#include "../Motion/RobotConstants.h"
#include "../Motion/MotionInterface.h"
#include "../Motion/Motion.h"
#include "../Main/Events.h"
#include "../Main/globals.h"
#include "../Main/SystemEvent.h"

#include "cmv_region.h"
#include "DetectBall.h"
#include "Vision.h"

#ifdef PLATFORM_LINUX
#ifndef VIS_NO_LIBS
#include "../../util/shared/image.h"

extern SImage *DebugImage;
static bool DrawUpVector      = true;
#endif
#endif

double AvgImgColor[3]={-1.0,-1.0,-1.0};

static const int MaxCorners = 100; // maximum number of corners for pattern analysis challenge

int DebugClass = 0;
char DebugInfo[1024];
RegionLabelMapT *RegionLabelsOld = NULL;
RegionLabelMapT *RegionLabelsNew = NULL;

bool ShouldThresholdImage = true;

char const * const Vision::name = "Vision";

Vision::Vision()
{
  // HACK for variable lighting challenge
  score_images = false;
  for(int i=0; i<MAX_TMAPS; i++)
    tmap_scores[i] = 0.0;
  // /HACK for variable lighting challenge

  cam_frame_src = NULL;
  available_time = 0UL;
  last_update_time = 0UL;
  use_correction_mask = false;

  object_info = new VisionInterface::ObjectInfo;
  memset(object_info,0,sizeof(VisionInterface::ObjectInfo));

#ifdef PLATFORM_APERIOS

  char design[256];
  int x_res, y_res;
  OPENR::GetRobotDesign(design);
  if(strcmp(design,"ERS-210")==0){
    x_res = 176;
    y_res = 144;
    kin = Kinematics(Kinematics::ERS210);
    HorzFOV = ers210_HorzFOV;
    VertFOV = ers210_VertFOV;
    initialize("/MS/config/ers210/vision.cfg",x_res,y_res);
  }else{
    x_res = 208;
    y_res = 160;
    kin = Kinematics(Kinematics::ERS7);
    HorzFOV = ers7_HorzFOV;
    VertFOV = ers7_VertFOV;
    initialize("/MS/config/ers7/vision.cfg",x_res,y_res);
  }

#else
  pprintf(TextOutputStream, 
	  "\n\nWARNING: ASSUMING ERS-210 KINEMATICS in Vision::Vision\n");
  pprintf(TextOutputStream, "(skipping Vision::initialize too - remember to call it)\n\n");
  kin = Kinematics(Kinematics::ERS210);
  HorzFOV = ers210_HorzFOV;
  VertFOV = ers210_VertFOV;
  
#endif
}

/* Hey now, if we're not running on the robot we need to tell
   Vision which set of kinematics it should be using.
*/
Vision::Vision(bool use_ers7_kinematics) {
  cam_frame_src = NULL;
  available_time = 0UL;
  last_update_time = 0UL;
  use_correction_mask = false;
  
  object_info = new VisionInterface::ObjectInfo;
  memset(object_info,0,sizeof(VisionInterface::ObjectInfo));
  
  if(!use_ers7_kinematics) {
    kin = Kinematics(Kinematics::ERS210);
    HorzFOV = ers210_HorzFOV;
    VertFOV = ers210_VertFOV;
  }else{
    kin = Kinematics(Kinematics::ERS7);
    HorzFOV = ers7_HorzFOV;
    VertFOV = ers7_VertFOV;
  }
  
  pprintf(TextOutputStream, "Vision::Vision(bool): Skipping Vision::initialize in constructor\n");
}

Vision::~Vision()
{
}

bool Vision::initConnections()
{
#ifdef PLATFORM_APERIOS
  EventManager *event_mgr;
  EventProcessor *ep;

  event_mgr = EventManager::getManager();

  cam_frame_src_id = event_mgr->getEventProcessorId("CameraFrameSource");
  ep = event_mgr->listenEventProcessor(name,cam_frame_src_id);
  cam_frame_src = dynamic_cast<CameraFrameSource *>(ep);

  sensor_data_id = event_mgr->getEventProcessorId("SensorData");
  ep = event_mgr->listenEventProcessor(name,sensor_data_id);
  sensor_data = dynamic_cast<SensorData *>(ep);
#endif

  return true;
}

void Vision::forceUpdate(ulong time)
{
#ifdef PLATFORM_APERIOS
  // update vision
  const CameraFrameSource::CameraFrameInfo *info;
  info = cam_frame_src->get(time);
  processFrame(object_info,info->bytes_y,info->bytes_u,info->bytes_v,info->width,info->height,info->skip);

  last_update_time = time;
#endif
}

bool Vision::update(ulong time, const EventList *events)
{
#ifdef PLATFORM_APERIOS
  if(events->present(cam_frame_src_id)){
    if(time > last_update_time){
      // update vision
      const CameraFrameSource::CameraFrameInfo *info;
      info = cam_frame_src->get(time);
      processFrame(object_info,info->bytes_y,info->bytes_u,info->bytes_v,info->width,info->height,info->skip);

      last_update_time = time;
    }

    return true;
  }
#endif

  return false;
}

void Vision::get(ulong time)
{
}

bool Vision::initialize(char *vision_cfg_file,int width_param,int height_param)
{
#ifdef PLATFORM_APERIOS
  sError result;
#endif
  pprintf(TextOutputStream,"initialized vision\n");

  // We'll create a buffer for our correction mask. If they config
  // files point us at a file to use, we'll load that. Otherwise
  // we'll use an identity mask. Keep in mind that the mask is twice
  // as wide as the image.
  int mask_width = 2*width_param;
  correction_mask.width = mask_width;
  correction_mask.height = height_param;
  correction_mask.row_stride= 2*mask_width + mask_width;

  uchar *correction_mask_ptr = NewLarge(&correction_mask_ptr, 3*mask_width*height_param);
  correction_mask.buf_y = correction_mask_ptr;
  correction_mask.buf_u = correction_mask.buf_y + mask_width;
  correction_mask.buf_v = correction_mask.buf_u + mask_width;

  // A multiplication by 128 is the identity. Similarly adding zero
  // yields the identity. The image is: a1,b1,a2,b2,... over and
  // over again. So if we fill even columns with 128 and odd
  // columns with 0, we get the identity mask.
  for(int i=0; i<3*mask_width*height_param; i++) {
    if(i % 2)
      correction_mask_ptr[i] = 0;
    else
      correction_mask_ptr[i] = 1 << mask_shift;
  }


  // Our focal distances changes depending on which model of robot we
  // are using.

  /* focal dist of camera:
     fh = (w/2) / tan(h/2) = (176/2) / (100 / (2*109)) = 88 / (100/109)
     = 95.92
     fv = (h/2) / tan(v/2) = (144/2) / (100 / (2*150)) = 60 / (100/150)
     = 90
  */
  FocalDist =(width_param/2.0)/tan(HorzFOV/2); // 158.76
  FocalDistV=(height_param/2.0)/tan(VertFOV/2); // 161.71
  // size of vertical pixel in terms of size of horizontal pixel
  YPixelSize=(FocalDist/FocalDistV);
  
  memset(DebugInfo,0,sizeof(DebugInfo)/sizeof(DebugInfo[0]));

  int num_path_chars=0;
  char colors_file_buf[256],tmapfile_base_buf[256],red_v_or_filename_buf[256],correction_mask_buf[256];
  const char *colors_file,*tmapfile_base,*red_v_or_filename,*correction_mask_filename;
  const char *path_sep;
  path_sep = strrchr(vision_cfg_file,'/');
  if(path_sep){
    num_path_chars = (path_sep - vision_cfg_file) + 1;
  }
  { // scope to make dictionary auto clean up
    Dictionary vis_config;
    vis_config.read(vision_cfg_file);

    if(vis_config.getValueString("colors_file",colors_file)){
      strncpy(colors_file_buf,vision_cfg_file,num_path_chars);
      colors_file_buf[num_path_chars] = 0;
      strcat(colors_file_buf,colors_file);
    }else{
      if(kin.model == kin.ERS210){
	colors_file = "/MS/config/ers210/colors.txt";
      }else{
	colors_file = "/MS/config/ers7/colors.txt";
      }
      // copy out before memory goes away
      strcpy(colors_file_buf,colors_file);
    }
    if(vis_config.getValueString("thresh_base",tmapfile_base)){
      strncpy(tmapfile_base_buf,vision_cfg_file,num_path_chars);
      tmapfile_base_buf[num_path_chars] = 0;
      strcat(tmapfile_base_buf,tmapfile_base);
    }else{
      if(kin.model == kin.ERS210){
	tmapfile_base = "/MS/config/ers210/thresh";
      }else{
	tmapfile_base = "/MS/config/ers7/thresh";
      }
      // copy out before memory goes away
      strcpy(tmapfile_base_buf,tmapfile_base);
    }
    if(vis_config.getValueString("red_v_or_file",red_v_or_filename)){
      strncpy(red_v_or_filename_buf,vision_cfg_file,num_path_chars);
      red_v_or_filename_buf[num_path_chars] = 0;
      strcat(red_v_or_filename_buf,red_v_or_filename);
    }else{
      if(kin.model == kin.ERS210){
	red_v_or_filename = "/MS/config/ers210/red_v_or.prm";
      }else{
	red_v_or_filename = "/MS/config/ers7/red_v_or.prm";
      }
      // copy out before memory goes away
      strcpy(red_v_or_filename_buf,red_v_or_filename);
    }
    if(!vis_config.getValueInt("num_thresholds",num_tmaps))
      num_tmaps = 1;
    int val=0;
    if(!vis_config.getValueInt("color_area",val) || val!=1)
      calcTotalArea = false;
    else
      calcTotalArea = true;
    if(!vis_config.getValueDouble("marker_color_offset",marker_color_offset))
      marker_color_offset = 0.0;
    ballGreenFiltersDisabled = (vis_config.getValueInt("green_filters_disabled",val) && val==1);

    if(vis_config.getValueInt("use_correction_mask", val) && val==1) {
      use_correction_mask = true;
    } else {
      use_correction_mask = false;
    }

    if(!vis_config.getValueString("correction_mask_file", correction_mask_filename)) {
      if(kin.model == kin.ERS210) 
	correction_mask_filename = "/MS/config/ers210/mask_img.raw";
      else
	correction_mask_filename = "/MS/config/ers7/mask_img.raw";
    }

    // copy out before memory goes away
    strcpy(correction_mask_buf, correction_mask_filename);
    colors_file = colors_file_buf;
    tmapfile_base = tmapfile_base_buf;
  }

  int num_y,num_u,num_v;
  int size;

  width  = width_param;
  height = height_param;
  max_width  = width;
  max_height = height;

  mzero(radial_map);

  // Load the correction mask
  if(use_correction_mask) {
    HFS::FILE *mask_file = HFS::fopen(correction_mask_buf, "rb");
    if(mask_file) {
      if(HFS::fread(correction_mask_ptr, 3*mask_width, height, mask_file)!=(unsigned int)height) {
	pprintf(TextOutputStream, "Error reading correction mask file\n");
	use_correction_mask = false; // who knows what state it's in
      }
      HFS::fclose(mask_file);
    }
  }

  // Load color information
  num_colors = CMVision::LoadColorInformation(color,MAX_COLORS,colors_file);
  if(num_colors <= 0){
    pprintf(TextOutputStream,"  ERROR: Could not load color information.\n");
  }
  else {
    pprintf(TextOutputStream,"  Loaded %d colors.\n",num_colors);
  }

  // Set up threshold
  size = 1 << (bits_y + bits_u + bits_v);
  num_y = 1 << bits_y;
  num_u = 1 << bits_u;
  num_v = 1 << bits_v;

  cur_tmap = 0;
  memset(tmap,0,sizeof(tmap));
  for(int i=0; i<num_tmaps; i++) {
#ifdef PLATFORM_APERIOS
    result = NewRegion(size, reinterpret_cast<void**>(&tmap[i])); 
    if (result != sSUCCESS)
      pprintf(TextOutputStream,"Unable to allocate tmap buffer %d of size %d\n",i,size);
#else
    tmap[i] = new cmap_t[size];
#endif
    memset(tmap[i],0,size*sizeof(cmap_t));
  }

  for(int i=0; i<num_tmaps; i++) {
    char tmapfile[256];
    if(i==0)
      sprintf(tmapfile,"%s.tm",tmapfile_base);
    else 
      sprintf(tmapfile,"%s%d.tm",tmapfile_base,i);
    
    pprintf(TextOutputStream, "Loading threshold file %s\n", tmapfile);

    if(!CMVision::LoadThresholdFile(tmap[i],num_y,num_u,num_v,tmapfile)) {
      if(i==0)
	pprintf(TextOutputStream,"  ERROR: Could not load threshold file '%s'.\n",tmapfile);
    }
    int remapped=0;
    CMVision::CheckTMapColors(tmap[i],num_y,num_u,num_v,num_colors,0);
    if(remapped>0)
      pprintf(TextOutputStream,"remapped %d colors in threshold file '%s'\n",remapped,tmapfile);
  }

  HFS::FILE *red_v_or_file;
  bool error=false;
  red_v_or_valid = false;
  red_v_or_file = HFS::fopen(red_v_or_filename_buf,"rb");
  if(red_v_or_file){
    if(HFS::fread(&red_v_or_dir,  sizeof(red_v_or_dir  ),1,red_v_or_file)!=1)
      error = true;
    if(HFS::fread(&red_v_or_base, sizeof(red_v_or_base ),1,red_v_or_file)!=1)
      error = true;
    if(HFS::fread(&red_v_or_scale,sizeof(red_v_or_scale),1,red_v_or_file)!=1)
      error = true;
  }else{
    error = true;
  }
  if(error){
#ifdef PLATFORM_APERIOS
    *((long *)0xDEAD07ED) = 0L;
#endif
  }else{
    red_v_or_valid = true;
  }

  // Allocate map structures
  max_width  = width;
  max_height = height;
  size = width * height;
  // max_runs = size / MIN_EXP_RUN_LENGTH;
  max_runs = 5000;
  max_regions = size / MIN_EXP_REGION_SIZE;
  size = max_width * max_height;
  NewLarge(&cmap,size+1); // +1 is for extra terminator
  if(!cmap) {
    cout << "Couldn't allocate cmap\n\xFF" << endl;
    *((long *)0xDEADCC44)=1;
  }
  NewLarge(&rmap,max_runs);
  if(!rmap) {
    cout << "Couldn't allocate rmap\n\xFF" << endl;
    *((long *)0xDEAD7744)=1;
  }
  NewLarge(&rmap2,max_runs);
  if(!rmap) {
    cout << "Couldn't allocate rmap2\n\xFF" << endl;
    *((long *)0xDEAD7745)=1;
  }
  NewLarge(&radial_runs,MAX_RADIAL_RUNS);
  if(!radial_runs) {
    cout << "Couldn't allocate radial runs\n" << endl;
    *((long *)0xDEAD7A75)=1;
  }
  NewLarge(&reg,max_regions);
  if(!reg) {
    cout << "Couldn't allocate reg\n" << endl;
    *((long *)0xDEAD7E66)=1;
  }

  body_angle=0.0;
  body_height=100.0;
  head_angles[0]=head_angles[1]=head_angles[2]=0.0;
  mzero(head_vel);

  // initialize output stuff
  encodeBuf = NULL;
  encoder = NULL;
  // Always init
  //if(config.spoutConfig.dumpVisionRLE!=0) {
  //if(config.spoutConfig.dumpVisionRLE) {
  // body position (5 floats) + image size*3 chars(value, x, length) + stop chars
  // body position (5 floats) + image size*3 chars(y, u, v) + stop chars
#ifdef PLATFORM_APERIOS
  NewRegion(5*4+2*2+width*height*3+2,(void **)&encodeBuf);
  if(encodeBuf==NULL)
    pprintf(TextOutputStream,"Null vision rle encode buf\n");
  else
    encoder=new SPOutVisionEncoder;
  //}
#endif

  visionAvgColorStream  = NULL;
  visionColorAreaStream = NULL;
  visionRadialMapStream = NULL;
  visionRawStream       = NULL;
  visionRLEStream       = NULL;
  visionObjStream       = NULL;

  outCountAvgColor  = 0;
  outCountColorArea = 0;
  outCountRadialMap = 0;
  outCountRaw       = 0;
  outCountRLE       = 0;

  //pprintf(TextOutputStream,"Done initializing vision.\n");

  ballDisabled=false;

  frameTimestamp = 0UL;

  return(true);
}

void Vision::initializeStreams(PacketStreamCollection *out_psc) {
  static const int normal_raw_size=width*height*3+5*4+2*2; // 76056
  int raw_id;
  raw_id    =out_psc->allocateStream(normal_raw_size*3,3);
  visionRawStream=out_psc->getStream(raw_id);
  visionRawStream->type = SPOutEncoder::VisionRaw2Lead;
  visionRawStream->binary = true;

  static const int normal_rle_size=5*1024;
  int rle_id;
  rle_id    =out_psc->allocateStream(normal_rle_size*10,10);
  visionRLEStream=out_psc->getStream(rle_id);
  visionRLEStream->type = SPOutEncoder::VisionRLE2Lead;
  visionRLEStream->binary = true;

  int vobj_id;
  vobj_id         = out_psc->allocateStream(sizeof(ObjectInfo)*10,10);
  visionObjStream = out_psc->getStream(vobj_id);
  visionObjStream->type = SPOutEncoder::VisionObjLead;
  visionObjStream->binary = true;

  int vrad_map_id;
  vrad_map_id = out_psc->allocateStream(sizeof(float)*2*NUM_ANGLES*NUM_ROBJECTS*3,10);
  visionRadialMapStream = out_psc->getStream(vrad_map_id);
  visionRadialMapStream->type = SPOutEncoder::VisionRadialMapLead;
  visionRadialMapStream->binary = true;

  int vavg_color_id;
  vavg_color_id   = out_psc->allocateStream(3*10,10);
  visionAvgColorStream = out_psc->getStream(vavg_color_id);
  visionAvgColorStream->type = SPOutEncoder::VisionAvgColorLead;
  visionAvgColorStream->binary = true;

  int vcolor_area_id;

  vcolor_area_id   = out_psc->allocateStream(3*(MAX_COLORS*sizeof(ulong)+1),10);
  visionColorAreaStream = out_psc->getStream(vcolor_area_id);
  visionColorAreaStream->type = SPOutEncoder::VisionColorAreaLead;
  visionColorAreaStream->binary = true;
}

bool Vision::close()
{
  for(int i=0; i<num_tmaps; i++) {
#ifdef PLATFORM_APERIOS
    DeleteRegion(tmap[i]);
#else
    delete[] tmap[i];
#endif
  }
#ifdef PLATFORM_APERIOS
  DeleteRegion(cmap);
  DeleteRegion(rmap);
  DeleteRegion(reg);
#else
  delete[] cmap;
  delete[] rmap;
  delete[] reg;
#endif

  memset(tmap,0,sizeof(tmap));
  cmap = NULL;
  rmap = NULL;
  reg  = NULL;

  max_width  = 0;
  max_height = 0;

#ifdef PLATFORM_APERIOS
  DeleteRegion(encodeBuf); // don't care about error
#else
  delete[] encodeBuf;
#endif
  delete encoder;

  return(true);
}

bool Vision::getBodyParameters() {
#ifdef PLATFORM_APERIOS
  static ulong last_timestamp=1UL;

  static int last_idx=0;

  ulong time=GetTime();

  bool done=false;
  if(LocUpdateQueueBuffer!=NULL) {
    while(!done) {
      ulong buffer_timestamp=LocUpdateQueueBuffer->buffer[last_idx].timestamp;
      if(last_timestamp < buffer_timestamp && buffer_timestamp < time) {
        body_angle =LocUpdateQueueBuffer->buffer[last_idx].body.angle;
        body_height=LocUpdateQueueBuffer->buffer[last_idx].body.height;
        last_timestamp=buffer_timestamp;
        last_idx = (last_idx+1) % Motion::MotionLocalizationUpdateQueueBufferSize;

        //pprintf(TextOutputStream,"got ba %g bh %g time=%lu\n",body_angle,body_height,buffer_timestamp);
      }
      else {
        done=true;

        //pprintf(TextOutputStream,"no buffers\n");
      }
    }
  }
#endif

  //pprintf(TextOutputStream,"body_angle %g body_height %g\n",body_angle,body_height);

  return true;
}

void Vision::setHeadVelocities(const double *angles) {
  double tilt = angles[0]-head_angles[0];
  double pan = angles[1]-head_angles[1]; 
  double frames_per_sec = 22.0;//approximate number of vision frames per second

  //multiply by the number of 
  
  pan *= frames_per_sec;
  tilt *= frames_per_sec;

  memcpy(&head_vel.tilt,&tilt,sizeof(double));
  memcpy(&head_vel.pan, &pan, sizeof(double));
}

void Vision::setHeadAngles(const double *angles) {
  memcpy(head_angles,angles,sizeof(double)*3);
}


inline double similar(double a,double b) {
  double s = (a - b) / (a + b);
  return(1 - fabs(s));
}

inline double pct_from_mean(double a,double b) {
  double s = (a - b) / (a + b);
  return fabs(s);
}

inline double uniform_with_gaussian_tails(double v, double uni_width, double gauss_stddev) {
  double eff_v;

  eff_v = max(fabs(v)-uni_width,0.0);
  return gaussian_with_min(eff_v/gauss_stddev,1e-3);
}

inline double sqrdist(double x1, double y1, double x2, double y2) {
  double xd,yd;

  xd = x1 - x2;
  yd = y1 - y2;

  return xd*xd + yd*yd;
}

vector3d Vision::getPixelDirection(double x, double y) {
  vector3d img_pixel_dir; // direction vector of pixel in image coordinates

  img_pixel_dir.set((x+.5-width/2.0),(height/2.0-(y+.5))*YPixelSize,FocalDist);

  vector3d robot_pixel_dir; // direction vector of pixel in robot coordinates
  robot_pixel_dir = 
    camera_dir  *img_pixel_dir.z +
    camera_up   *img_pixel_dir.y +
    camera_right*img_pixel_dir.x;

  robot_pixel_dir = robot_pixel_dir.norm();

  return robot_pixel_dir;
}

bool Vision::intersectPixelZPlane(double x, double y, double z_value, vector3d &loc) {
  vector3d pix_dir;
  bool intersects;

  pix_dir = getPixelDirection(x,y);
  intersects=GVector::intersect_ray_plane(camera_loc,pix_dir,
                                          vector3d(0.0,0.0,z_value),vector3d(0.0,0.0,1.0),
                                          loc);
  return intersects;
}

vector3d Vision::convertToCamera(vector3d ego) {
  vector3d cam;

  ego -= camera_loc;
  cam.set(ego.dot(camera_right),-(ego.dot(camera_up)),ego.dot(camera_dir));

  return cam;
}

vector2d Vision::cameraToImg(vector3d camera) {
  vector2d img;

  img.set(camera.x,camera.y / YPixelSize);
  img *= FocalDist / camera.z;
  img += vector2d(width/2.0 - .5,height/2.0 - .5);

  return img;
}

double Vision::alignVertical(vector3d v1, vector3d v2, vector2d &plane_dir, vector2d &v1_dz, vector2d &v2_dz) {
  vector3d avg_dir;
  avg_dir = (v1+v2).norm();
  
  plane_dir = vector2d(avg_dir.x,avg_dir.y).norm();

  double a1,a2;
  a1 = acos(avg_dir.dot(v1)/v1.length());
  a2 = acos(avg_dir.dot(v2)/v2.length());
  
  vector2d in_plane_dir;
  in_plane_dir.set(hypot(avg_dir.x,avg_dir.y),avg_dir.z);
  
  v1_dz = v2_dz = in_plane_dir;
  
  if(v1.z > avg_dir.z) {
    v1_dz = v1_dz.rotate( a1);
  } else {
    v1_dz = v1_dz.rotate(-a1);
  }
  
  if(v2.z > avg_dir.z) {
    v2_dz = v2_dz.rotate( a2);
  } else {
    v2_dz = v2_dz.rotate(-a2);
  }
  
  vector2d v1_xy_dir,v2_xy_dir;
  
  v1_xy_dir.set(v1.x,v1.y);
  v2_xy_dir.set(v2.x,v2.y);

  //plane_dir = v1_xy_dir + v2_xy_dir;
  //plane_dir.normalize();
  //
  //v1_dz.set(hypot(v1_xy_dir.x,v1_xy_dir.y),v1.z);
  //v2_dz.set(hypot(v2_xy_dir.x,v2_xy_dir.y),v2.z);

  return v1_xy_dir.dot(v2_xy_dir);
}

bool Vision::findLocVerticalSeperation(vector2d top_dz, vector2d bot_dz, double seperation, vector2d &top_loc, vector2d &bot_loc) {
  if(bot_dz.x < 1e-3)
    return false;

  if(top_dz == bot_dz)
    return false;

  double top_t,bot_t;

  // this should be safe because it blows up when
  // a) bottom distance component is zero (filtered out above)
  // b) top and bottom vectors are colinear (filtered out above)
  top_t = seperation / (top_dz.y - bot_dz.y * top_dz.x / bot_dz.x);
  bot_t = top_t * top_dz.x / bot_dz.x;

  if(top_t < 0.0)
    return false;

  top_loc = top_dz * top_t;
  bot_loc = bot_dz * bot_t;

  return true;
}

double Vision::groundAngle(double x, double y) {
  vector3d pix_dir;

  pix_dir = getPixelDirection(x,y);
  return atan2a(pix_dir.y,pix_dir.x);
}

void Vision::findSpan(double &left, double &right, double x1, double x2, double y1, double y2) {
  double angle;

  vector3d bbox_corner_dir; // direction vector through bounding box corner
  
  bbox_corner_dir = getPixelDirection(x1,y1);
  angle = atan2a(bbox_corner_dir.y,bbox_corner_dir.x);
  right = left = angle;

  bbox_corner_dir = getPixelDirection(x1,y2);
  angle = atan2a(bbox_corner_dir.y,bbox_corner_dir.x);
  left  = max(left , angle);
  right = min(right, angle);

  bbox_corner_dir = getPixelDirection(x2,y1);
  angle = atan2a(bbox_corner_dir.y,bbox_corner_dir.x);
  left  = max(left , angle);
  right = min(right, angle);

  bbox_corner_dir = getPixelDirection(x2,y2);
  angle = atan2a(bbox_corner_dir.y,bbox_corner_dir.x);
  left  = max(left , angle);
  right = min(right, angle);
}

int Vision::calcEdgeMask(int x1,int x2,int y1,int y2)
{
  static const int boundary_pixel_size=1;

  int edge;

  edge = 0;
  if(x1 <= 0       +boundary_pixel_size) edge |= OFF_EDGE_LEFT  ;
  if(x2 >= width -1-boundary_pixel_size) edge |= OFF_EDGE_RIGHT ;
  if(y1 <= 0       +boundary_pixel_size) edge |= OFF_EDGE_TOP   ;
  if(y2 >= height-1-boundary_pixel_size) edge |= OFF_EDGE_BOTTOM;

  return edge;
}

int Vision::calcEdgeMask(int x,int y,int slop)
{
  int edge;

  edge = 0;
  if(x <= 0       +slop) edge |= OFF_EDGE_LEFT  ;
  if(x >= width -1-slop) edge |= OFF_EDGE_RIGHT ;
  if(y <= 0       +slop) edge |= OFF_EDGE_TOP   ;
  if(y >= height-1-slop) edge |= OFF_EDGE_BOTTOM;

  return edge;
}

int Vision::addToHistHorizStrip(int y, int x1, int x2, int *color_cnt)
{
  int x;

  y  = bound(y ,0,height-1);
  x1 = bound(x1,0,width -1);
  x2 = bound(x2,0,width -1);

  for(x=x1; x<=x2; x++) {
    color_cnt[getColorUnsafe(x,y)]++;
  }

  return x2-x1+1;
}

int Vision::addToHistVertStrip(int x, int y1, int y2, int *color_cnt)
{
  int y;

  x  = bound(x ,0,width -1);
  y1 = bound(y1,0,height-1);
  y2 = bound(y2,0,height-1);

  for(y=y1; y<=y2; y++) {
    color_cnt[getColorUnsafe(x,y)]++;
  }

  return y2-y1+1;
}

double Vision::normalizedColorLocation(vector3d loc,vector3d dir,double base,double scale)
{
  double val;

  val = dir.dot(loc);
  val -= base;
  val *= scale;

  return val;
}

void Vision::findMarkers(VObject *markers,VisionObjectInfo *marker_infos) {
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findMarkers",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  const bool debug_markers    = false;
  const bool debug_verbose    = false;
  const bool debug_confidence = false;
  static int frame_cnt=0;
#ifdef PLATFORM_APERIOS
  static const int print_period=30;
#else
  static const int print_period=1;
#endif

  if(debug_markers)
    frame_cnt = (frame_cnt + 1) % print_period;

  static const int MaxMarkerRegions=10;

  // color (along with pink) for markers 0-1, 2-3, and 4-5, respectively
  static const int marker_color_array[3] = {COLOR_CYAN, COLOR_GREEN, COLOR_YELLOW};

  //pprintf(TextOutputStream,"Trying to find markers\n");

  for(int marker_idx=0; marker_idx<6; marker_idx++)
    markers[marker_idx].confidence=0.0;

  region *pk_reg; // pink regions
  int pk_num; // number of pink regions processed

  pk_reg = color[COLOR_PINK].list;
  pk_num=0;

  while(pk_reg && pk_num<MaxMarkerRegions) {
    if(debug_markers && debug_verbose &&
       frame_cnt == 0)
      pprintf(TextOutputStream,"pk %d img (%g,%g) area %d\n",pk_num,pk_reg->cen_x,pk_reg->cen_y,pk_reg->area);

    if(pk_reg->area > 8) {
      for(int color_idx=0; color_idx<3; color_idx++) {
        int marker_color;
        
	if(marker_color_array[color_idx] == COLOR_GREEN && !USE_MIDDLE_MARKERS)
	  continue;

        marker_color = marker_color_array[color_idx];
        
        region *ot_reg; // the other region
        int ot_num;

        ot_reg = color[marker_color].list;
        ot_num = 0;

        while(ot_reg && ot_num<MaxMarkerRegions) {
          double s,conf1,conf2,conf3;
          double conf;
          int marker_idx;

          if(debug_markers && debug_verbose &&
             frame_cnt == 0)
            pprintf(TextOutputStream,"  ot %d img (%g,%g) area %d\n",ot_num,ot_reg->cen_x,ot_reg->cen_y,ot_reg->area);

          vector3d pk_dir,ot_dir;
          vector2d pk_ctr,ot_ctr;
          bool pink_top;
          vector3d top_dir,bot_dir;
          vector2d xy_dir;
          vector2d top_dz_dir,bot_dz_dir; // vector in distance (radial direction) and elevation
          vector2d marker_dz_loc;
          vector2d marker_xy_loc;
          vector3d marker_loc;
          double top_t,bot_t;
          bool bad_marker=false;

          if(ot_reg->area <= 8)
            bad_marker = true;

          // The color classification we use tends to not pick up colors blended with pink but
          // does pick up colors blended with white.  The causes a .5 pixel bias in the centroid
          // of the color region which then makes distance readings be too small.  The
          // marker_color_offset parameter corrects for this bias by moving the centroid of the
          // colored region.  This parameter should be -.5 in order to make the colored region move
          // the .5 pixels towards the pink region.

          pk_ctr.set(pk_reg->cen_x,pk_reg->cen_y);
          pk_dir = getPixelDirection(pk_ctr.x,pk_ctr.y);
          ot_ctr.set(ot_reg->cen_x,ot_reg->cen_y);
          ot_ctr += (ot_ctr-pk_ctr).norm() * marker_color_offset;
          ot_dir = getPixelDirection(ot_ctr.x,ot_ctr.y);

          // protect against numerical instabilities (we'd end up throwing this out anyway)
          if(fabs(atan2(pk_dir.z,hypot(pk_dir.x,pk_dir.y))) > 25*M_PI/180.0)
            bad_marker=true;

          // protect against numerical instabilities (we'd end up throwing this out anyway)
          if(fabs(atan2(ot_dir.z,hypot(ot_dir.x,ot_dir.y))) > 25*M_PI/180.0)
            bad_marker=true;

          if(!bad_marker) {
            if(debug_markers && debug_verbose &&
               frame_cnt == 0) {
              pprintf(TextOutputStream,"   pk_dir (%g,%g,%g) ot_dir (%g,%g,%g)\n",
                      pk_dir.x,pk_dir.y,pk_dir.z,
                      ot_dir.x,ot_dir.y,ot_dir.z);
            }

            pink_top = pk_dir.z > ot_dir.z;
            if(pink_top) {
              top_dir = pk_dir;
              bot_dir = ot_dir;
            }
            else {
              top_dir = ot_dir;
              bot_dir = pk_dir;
            }

            alignVertical(top_dir,bot_dir,xy_dir,top_dz_dir,bot_dz_dir);

            vector2d pk_to_ot_img;
            pk_to_ot_img = vector2d(pk_reg->cen_x,pk_reg->cen_y) - vector2d(ot_reg->cen_x,ot_reg->cen_y);
            pk_to_ot_img.normalize();
            conf1 = fabs(pk_to_ot_img.dot(img_up));
            conf1 = (conf1 - .966) / (1 - .966); // fall to zero in 15 degrees
            conf1 = bound(conf1,0.0,1.0);
            if(debug_markers && debug_verbose &&
               frame_cnt == 0)
              pprintf(TextOutputStream,"    pk_to_ot_img (%g,%g) img_up (%g,%g) dot %g\n",
                      pk_to_ot_img.x,pk_to_ot_img.y,
                      img_up.x,img_up.y,
                      pk_to_ot_img.dot(img_up));
            
            s = sqrdist(pk_reg->cen_x, pk_reg->cen_y, ot_reg->cen_x, ot_reg->cen_y);
            //conf2 = gaussian_with_min(pct_from_mean(pk_reg->area,ot_reg->area) / .2, 1e-3);
            conf2 = gaussian_with_min(pct_from_mean(pk_reg->area,ot_reg->area) / .3, 1e-3);
            //conf3 = gaussian_with_min(pct_from_mean((pk_reg->area+ot_reg->area)/2,s) / .3, 1e-3);
            conf3 = gaussian_with_min(pct_from_mean((pk_reg->area+ot_reg->area)/2,s) / .5, 1e-3);

            conf = conf1 * conf2 * conf3;
            marker_idx = 2*color_idx + pink_top;

	    if(debug_markers && debug_confidence &&
	       frame_cnt == 0)
	      pprintf(TextOutputStream,"    marker %d conf 1:%g 2:%g 3:%g final:%g\n",marker_idx,conf1,conf2,conf3,conf);

	    if(conf > markers[marker_idx].confidence) {
	      vector2d top_dz_loc,bot_dz_loc;
	      top_dz_loc.set(0,0); bot_dz_loc.set(0,0);
	      // FIXME: Use return value
	      findLocVerticalSeperation(top_dz_dir,bot_dz_dir,MarkerColorSeperation,top_dz_loc,bot_dz_loc);
          
	      if(debug_markers && debug_verbose &&
		 frame_cnt == 0) {
		pprintf(TextOutputStream,"top_t %g bot_t %g ",
			top_t,bot_t);
	      }

	      marker_dz_loc = (top_dz_loc + bot_dz_loc) / 2.0;
	      marker_xy_loc = xy_dir * marker_dz_loc.x;
	      marker_loc.set(marker_xy_loc.x,marker_xy_loc.y,marker_dz_loc.y);
	      marker_loc += camera_loc;
        
	      if(debug_markers && debug_verbose &&
		 frame_cnt == 0) {
		pprintf(TextOutputStream,"mk dz_loc (%g,%g) xy_loc (%g,%g)\n",
			top_t,bot_t);
	      }


	      //          d = sqrt(s);
	      //
	      if(debug_markers && debug_verbose &&
		 frame_cnt == 0)
		pprintf(TextOutputStream,"    pk %d c %d ot %d marker %d conf %g img_d %g ",
			pk_num,color_idx,ot_num,marker_idx,conf,sqrt(s));

	      markers[marker_idx].confidence = conf;

	      markers[marker_idx].loc = marker_loc;
	      markers[marker_idx].distance = hypot(marker_loc.x,marker_loc.y);

	      int x1,x2,y1,y2; // x1=min x, x2=max x, y1=min y, y2=max y

	      x1 = min(pk_reg->x1,ot_reg->x1);
	      x2 = max(pk_reg->x2,ot_reg->x2);
	      y1 = min(pk_reg->y1,ot_reg->y1);
	      y2 = max(pk_reg->y2,ot_reg->y2);

	      findSpan(markers[marker_idx].left,markers[marker_idx].right,x1,x2,y1,y2);

	      markers[marker_idx].edge = calcEdgeMask(x1,x2,y1,y2);

	      marker_infos[marker_idx].reg  = pk_reg;
	      marker_infos[marker_idx].reg2 = ot_reg;

	      if(debug_markers && debug_verbose &&
		 (frame_cnt == 0))
		pprintf(TextOutputStream,"dist %g img (%g,%g) edge %d\n",
			marker_dz_loc.x,
			(pk_reg->cen_x+ot_reg->cen_x)/2.0,(pk_reg->cen_y+ot_reg->cen_y)/2.0,
			markers[marker_idx].edge);
	    }
	    
          }

          ot_num++;
          ot_reg = ot_reg->next;
        }
      }
    }

    pk_num++;
    pk_reg = pk_reg->next;
  }

  if(debug_markers && 
     frame_cnt == 0) {
    for(int marker_idx=0; marker_idx<6; marker_idx++) {
      VObject *marker;
      
      marker=&markers[marker_idx];
      if(marker->confidence > 0.02)
        pprintf(TextOutputStream,"marker %d conf %g loc (%g,%g,%g) dist %g left %g right %g edge %d\n",
                marker_idx,marker->confidence,marker->loc.x,marker->loc.y,marker->loc.z,marker->distance,marker->left,marker->right,marker->edge);  
    }
  }
  

  //pprintf(TextOutputStream,"Done finding markers\n");
}

struct ScanGoalHeightCallbackParams {
  Vision *vision;
  int colorToFind;
  int heightSoFar;
  int greenSoFar;
  int otherSoFar;
  bool up,endLocSet;
  GVector::vector2d<int> endLoc;

  void init(Vision *vision_param, int color_idx, bool up_param) {
    vision = vision_param;
    colorToFind = color_idx;
    heightSoFar = 0;
    greenSoFar = 0;
    otherSoFar = 0;
    up = up_param;
    endLoc.set(0,0);
    endLocSet = false;
  }
};

struct ScanGoalHeightCallback {
  bool operator()(int x,int y,ScanGoalHeightCallbackParams *params) {
    int color = params->vision->getNearColor(x,y);

    if(!params->endLocSet) {
      params->endLocSet = true;
      params->endLoc.set(x,y);
    }

    if(color == params->colorToFind) {
      params->heightSoFar++;
      params->endLoc.set(x,y);
      params->otherSoFar = 0;
    } else if(color == COLOR_GREEN) {
      params->greenSoFar++;
    } else {
      params->otherSoFar++;
    }

    //pprintf(TextOutputStream,"ht:pt=(%3d,%3d) color=%d cnts:c=%2d g=%2d o=%2d\n",x,y,color,
    //        params->heightSoFar,params->greenSoFar,params->otherSoFar);

    //return (params->heightSoFar > 2*(params->greenSoFar + params->otherSoFar));
    if(params->up){
      return
        ((params->heightSoFar*params->heightSoFar > 
          (params->endLoc-GVector::vector2d<int>(x,y)).sqlength()) ||
         (params->heightSoFar < 5));
    }else{
      bool cont;

      cont = (params->greenSoFar < 5);
      cont = cont && !(color == COLOR_ORANGE && params->otherSoFar >= 5);

      return cont;
    }
  }
};

struct ScanGoalWidthCallbackParams {
  Vision *vision;
  int colorToFind;
  int widthSoFar;
  int otherSoFar;
  int x1,x2,y1,y2;
  GVector::vector2d<int> endLoc;

  void init(Vision *vision_param, int color_idx, int x1_in, int y1_in, int x2_in, int y2_in) {
    vision = vision_param;
    colorToFind = color_idx;
    x1 = x1_in;
    y1 = y1_in;
    x2 = x2_in;
    y2 = y2_in;
    widthSoFar = 0;
    otherSoFar = 0;
    endLoc.set(-1,-1);
  }
};

struct ScanGoalWidthCallback {
  bool operator()(int x,int y,ScanGoalWidthCallbackParams *params) {
    int color = params->vision->getNearColor(x,y);

    if(color == params->colorToFind) {
      params->widthSoFar++;
      params->endLoc.set(x,y);
    } else {
      params->otherSoFar++;
    }

    //pprintf(TextOutputStream,"wd:pt=(%3d,%3d) color=%d cnts:c=%2d o=%2d\n",x,y,color,
    //        params->widthSoFar,params->otherSoFar);

    return Vision::inBox(x,y,params->x1,params->y1,params->x2,params->y2);
  }
};

struct FindColorCallbackParams {
  Vision *vision;
  int colorIdx;
  int atX,atY;

  void init(Vision *vision_param,int color_idx) {
    vision = vision_param;
    colorIdx = color_idx;
    atX = atY = 0;
  }    
};

struct FindColorCallback {
  bool operator()(int x, int y, FindColorCallbackParams *params) {
    int color_at=0;

    color_at = params->vision->cmap[y*params->vision->width + x];
    if(color_at == params->colorIdx) {
      params->atX = x;
      params->atY = y;
      return false;
    } else {
      return true;
    }

    return true;
  }
};

struct FindLastColorCallbackParams {
  Vision *vision;
  int colorIdx;
  int atX,atY;

  void init(Vision *vision_param,int color_idx) {
    vision = vision_param;
    colorIdx = color_idx;
    atX = atY = 0;
  }    
};

struct FindLastColorCallback {
  bool operator()(int x, int y, FindLastColorCallbackParams *params) {
    int color_at=0;

    color_at = params->vision->cmap[y*params->vision->width + x];
    if(color_at == params->colorIdx) {
      params->atX = x;
      params->atY = y;
      return true;
    } else {
      return false;
    }

    return true;
  }
};

static const GVector::vector2d<int> Dirs[8] = {
  GVector::vector2d<int>( 1, 0),
  GVector::vector2d<int>( 1, 1),
  GVector::vector2d<int>( 0, 1),
  GVector::vector2d<int>(-1, 1),
  GVector::vector2d<int>(-1, 0),
  GVector::vector2d<int>(-1,-1),
  GVector::vector2d<int>( 0,-1),
  GVector::vector2d<int>( 1,-1)
};

bool Vision::findCorner(int color_idx, int dir_x, int dir_y, int *x, int *y) {
  static const bool debug=false;

  GVector::vector2d<int> cur_loc;

  cur_loc.set(*x,*y);

  // first find a pixel that is of color color_idx
  // use drawLine2

  if(debug)
    pprintf(TextOutputStream,"start (%d,%d)\n",cur_loc.x,cur_loc.y);

  if(!onImage(cur_loc.x,cur_loc.y)) {
    if(debug)
      pprintf(TextOutputStream,"cur_loc (%d,%d) not on image\n",cur_loc.x,cur_loc.y);

    vector2d loc_r,dir_r,intersect_pt;

    loc_r.set(cur_loc.x,cur_loc.y);
    dir_r.set(-dir_x,-dir_y);
    dir_r.normalize();
    if(debug)
      pprintf(TextOutputStream,"dir_r (%g,%g)\n",dir_r.x,dir_r.y);
    if(loc_r.x < 0) {
      if(!GVector::intersect_ray_plane(loc_r,dir_r,vector2d(0,0),vector2d(-1,0),intersect_pt)) {
        if(debug)
          pprintf(TextOutputStream,"x<0 intersection failed\n");
        return false;
      }
      loc_r = intersect_pt;
    }
    if(debug)
      pprintf(TextOutputStream,"1 loc_r (%g,%g)\n",loc_r.x,loc_r.y);
    if(loc_r.x > width-1) {
      if(!GVector::intersect_ray_plane(loc_r,dir_r,vector2d(width-1,0),vector2d(1,0),intersect_pt)) {
        if(debug)
          pprintf(TextOutputStream,"x>width-1 intersection failed\n");
        return false;
      }
      loc_r = intersect_pt;
    }
    if(debug)
      pprintf(TextOutputStream,"2 loc_r (%g,%g)\n",loc_r.x,loc_r.y);
    if(loc_r.y < 0) {
      if(!GVector::intersect_ray_plane(loc_r,dir_r,vector2d(0,0),vector2d(0,-1),intersect_pt)) {
        if(debug)
          pprintf(TextOutputStream,"y<0 intersection failed\n");
        return false;
      }
      if(intersect_pt.x < 0 || intersect_pt.x > width-1) {
        if(debug)
          pprintf(TextOutputStream,"y<0 intersection disturbed x\n");
        return false;
      }
      loc_r = intersect_pt;
    }
    if(debug)
      pprintf(TextOutputStream,"3 loc_r (%g,%g)\n",loc_r.x,loc_r.y);
    if(loc_r.y > height-1) {
      if(!GVector::intersect_ray_plane(loc_r,dir_r,vector2d(0,height-1),vector2d(0,1),intersect_pt)) {
        if(debug)
          pprintf(TextOutputStream,"y>hieght intersection failed\n");
        return false;
      }
      if(intersect_pt.x < 0 || intersect_pt.x > width-1) {
        if(debug)
          pprintf(TextOutputStream,"y>hieght intersection disturbed x\n");
        return false;
      }
      loc_r = intersect_pt;
    }
    if(debug)
      pprintf(TextOutputStream,"4 loc_r (%g,%g)\n",loc_r.x,loc_r.y);
    if(loc_r.y < 0 || loc_r.y > height-1)
      return false;
    if(debug)
      pprintf(TextOutputStream,"5 loc_r (%g,%g)\n",loc_r.x,loc_r.y);
    cur_loc.set((int)loc_r.x,(int)loc_r.y);

    if(debug)
      pprintf(TextOutputStream,"cur_loc moved to (%d,%d)\n",cur_loc.x,cur_loc.y);
  }

  if(cmap[cur_loc.y*width + cur_loc.x]==color_idx) {
    FindLastColorCallbackParams flc_params;
    FindLastColorCallback flc;
    flc_params.init(this,color_idx);
    drawLine2(cur_loc.x,cur_loc.y,dir_x,dir_y,flc,&flc_params);
    cur_loc.set(flc_params.atX,flc_params.atY);
  } else {
    FindColorCallbackParams fc_params;
    FindColorCallback find_color;
    fc_params.init(this,color_idx);
    if(drawLine2(cur_loc.x,cur_loc.y,-dir_x,-dir_y,find_color,&fc_params))
      return false;
    cur_loc.set(fc_params.atX,fc_params.atY);
  }

  if(debug)
    pprintf(TextOutputStream,"found (%d,%d)\n",cur_loc.x,cur_loc.y);

  // now greedily try to move to pixels in dir_x,dir_y direction
  int dir_idxs[4];
  int dir16; // direction quantized down to 16 directions

  dir16 = 0;
  if(abs(dir_x) < abs(dir_y)) {
    if(2*abs(dir_x) < abs(dir_y)) {
      dir16 = 3;
    } else {
      dir16 = 2;
    }
  } else {
    if(abs(dir_x) > 2*abs(dir_y)) {
      dir16 = 0;
    } else {
      dir16 = 1;
    }
  }
  if(dir_y < 0)
    dir16 = 15-dir16; // flip vertically
  if(dir_x < 0)
    dir16 = (dir16 & 0x8) + (0x7 - (dir16 & 0x7)); // flip horizontally

  if(dir16 & 1) {
    dir_idxs[0] = (((dir16+ 1)/2) + 8) % 8;
    dir_idxs[2] = (((dir16+ 3)/2) + 8) % 8;
    dir_idxs[1] = (((dir16+15)/2) + 8) % 8;
    dir_idxs[3] = (((dir16+13)/2) + 8) % 8;
  } else {
    dir_idxs[0] = (((dir16+ 0)/2) + 8) % 8;
    dir_idxs[2] = (((dir16+14)/2) + 8) % 8;
    dir_idxs[1] = (((dir16+ 2)/2) + 8) % 8;
    dir_idxs[3] = (((dir16+ 4)/2) + 8) % 8;
  }

  if(debug)
    pprintf(TextOutputStream,"dir (%4d,%4d) dir_idxs=[%d,%d,%d,%d] dirs=[(%2d,%2d) (%2d,%2d) (%2d,%2d) (%2d,%2d)]\n",
            dir_x,dir_y,
            dir_idxs[0],dir_idxs[1],dir_idxs[2],dir_idxs[3],
            Dirs[dir_idxs[0]].x,Dirs[dir_idxs[0]].y,Dirs[dir_idxs[1]].x,Dirs[dir_idxs[1]].y,
            Dirs[dir_idxs[2]].x,Dirs[dir_idxs[2]].y,Dirs[dir_idxs[3]].x,Dirs[dir_idxs[3]].y
            );

  bool done=false;
  while(!done) {
    bool found_move;
    found_move = false;
    for(int dir_to_try=0; !found_move && dir_to_try<4; dir_to_try++) {
      GVector::vector2d<int> new_loc;
      new_loc = cur_loc + Dirs[dir_idxs[dir_to_try]];
      if(onImage(new_loc.x,new_loc.y)) {
        if(cmap[new_loc.y*width + new_loc.x]==color_idx) {
          found_move = true;
          cur_loc = new_loc;
        }
      }
    }
    done = !found_move;
  }

  if(debug)
    pprintf(TextOutputStream,"climb (%d,%d)\n",cur_loc.x,cur_loc.y);

  *x = cur_loc.x;
  *y = cur_loc.y;

  return true;
}

void Vision::findGoal(int color_idx, VObject *goal, VObject *goal_edges,VisionObjectInfo *goal_info) {
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findGoal",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  static const bool debug_goals  =false;
  static const bool debug_verbose=false;
  static const bool debug_conf   =false;
  static const bool debug_sort=false;
  static const int debug_sort_color=4;
  static const int goal_outside_offset=4; // offset to get outside of the goal on image

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

  bool print_now = debug_goals && (frame_cnt == 0);

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

  region *goal_reg;

  //pprintf(TextOutputStream,"Trying to find goal of color %d\n",color_idx);

  goal_reg = color[color_idx].list;

  goal->confidence=0.0;
  goal_edges[0].confidence = 0.0;
  goal_edges[1].confidence = 0.0;

  if(!goal_reg) return;

  if(goal_reg->area < 10) return;

  if(print_now)
    pprintf(TextOutputStream,"region color %d ll (%d,%d) ur (%d,%d) cen (%g,%g) area %d\n",
            color_idx,
            goal_reg->x1,goal_reg->y1,goal_reg->x2,goal_reg->y2,
            goal_reg->cen_x,goal_reg->cen_y,goal_reg->area);

  GVector::vector2d<int> left_loc,right_loc,top_loc,bottom_loc;
  int x_size,y_size;
  
  int start_xh,start_yh;
  start_xh = (int)goal_reg->cen_x;
  start_yh = (int)goal_reg->cen_y;

  int dir_x,dir_y;
  dir_x = (int)(img_up.x*1000);
  dir_y = (int)(img_up.y*1000);

  ScanGoalHeightCallback ht_callback;
  ScanGoalHeightCallbackParams ht_params;

  ht_params.init(this,color_idx,false);
  drawLine2(start_xh,start_yh,-dir_x,-dir_y,ht_callback,&ht_params);
  bottom_loc = ht_params.endLoc;

  ht_params.init(this,color_idx,true);
  drawLine2(start_xh,start_yh,dir_x,dir_y,ht_callback,&ht_params);
  top_loc = ht_params.endLoc;

  if(top_loc == bottom_loc) return;

  GVector::vector2d<int> diff_loc;
  diff_loc = top_loc - bottom_loc;
  diff_loc.set(abs(diff_loc.x),abs(diff_loc.y));
  diff_loc += GVector::vector2d<int>(1,1);
  y_size = (int)(sqrt((double)diff_loc.sqlength()));

  if(y_size < 3) return;

  int start_xw,start_yw;
  start_xw = (start_xh + top_loc.x) / 2;
  start_yw = (start_yh + top_loc.y) / 2;
  dir_x = (int)(img_hor.x*1000);
  dir_y = (int)(img_hor.y*1000);

  ScanGoalWidthCallback wd_callback;
  ScanGoalWidthCallbackParams wd_params;

  wd_params.init(this,color_idx,goal_reg->x1,goal_reg->y1,goal_reg->x2,goal_reg->y2);
  drawLine2(start_xw,start_yw,dir_x,dir_y,wd_callback,&wd_params);
  right_loc = wd_params.endLoc;
  if(right_loc.x == -1)
    return;

  wd_params.init(this,color_idx,goal_reg->x1,goal_reg->y1,goal_reg->x2,goal_reg->y2);
  drawLine2(start_xw,start_yw,-dir_x,-dir_y,wd_callback,&wd_params);
  left_loc = wd_params.endLoc;
  if(left_loc.x == -1)
    return;

  if(left_loc == right_loc) return;

  if(print_now && debug_verbose) {
    pprintf(TextOutputStream,"top_loc=(%d,%d) bot_loc=(%d,%d) right_loc=(%d,%d) left_loc=(%d,%d)\n",
            V2COMP(top_loc),
            V2COMP(bottom_loc),
            V2COMP(right_loc),
            V2COMP(left_loc));
  }

  diff_loc = right_loc - left_loc;
  diff_loc.set(abs(diff_loc.x),abs(diff_loc.y));
  diff_loc += GVector::vector2d<int>(1,1);
  x_size = (int)(sqrt((double)diff_loc.sqlength()));

  if(x_size < 3) return;

  // bottom/top left/right corner (w/ respect to world) (relative to centroid)
  GVector::vector2d<int> blc_cen,brc_cen,tlc_cen,trc_cen;
  // bottom/top left/right corner (w/ respect to world) (img coordinates)
  GVector::vector2d<int> blc_img,brc_img,tlc_img,trc_img;
  GVector::vector2d<int> centroid;

  centroid.set(start_xh,start_yh);

  tlc_cen =
    GVector::vector2d<int>(top_loc.x    - start_xh, top_loc.y    - start_yh) +
    GVector::vector2d<int>(left_loc.x   - start_xw, left_loc.y   - start_yw);
  trc_cen =
    GVector::vector2d<int>(top_loc.x    - start_xh, top_loc.y    - start_yh) +
    GVector::vector2d<int>(right_loc.x  - start_xw, right_loc.y  - start_yw);
  blc_cen =
    GVector::vector2d<int>(bottom_loc.x - start_xh, bottom_loc.y - start_yh) +
    GVector::vector2d<int>(left_loc.x   - start_xw, left_loc.y   - start_yw);
  brc_cen =
    GVector::vector2d<int>(bottom_loc.x - start_xh, bottom_loc.y - start_yh) +
    GVector::vector2d<int>(right_loc.x  - start_xw, right_loc.y  - start_yw);

  tlc_img = centroid + tlc_cen;
  trc_img = centroid + trc_cen;
  blc_img = centroid + blc_cen;
  brc_img = centroid + brc_cen;

  if(print_now && debug_verbose) {
    pprintf(TextOutputStream,"befor tl=(%d,%d) tr=(%d,%d) bl=(%d,%d) br=(%d,%d)\n",
            tlc_img.x,tlc_img.y,
            trc_img.x,trc_img.y,
            blc_img.x,blc_img.y,
            brc_img.x,brc_img.y);
  }

  bool found_tlc,found_trc,found_blc,found_brc;

  found_tlc=findCorner(color_idx,(int)(1000*( img_up.x-img_hor.x)),(int)(1000*( img_up.y-img_hor.y)),&tlc_img.x,&tlc_img.y);
  found_trc=findCorner(color_idx,(int)(1000*( img_up.x+img_hor.x)),(int)(1000*( img_up.y+img_hor.y)),&trc_img.x,&trc_img.y);
  found_blc=findCorner(color_idx,(int)(1000*(-img_up.x-img_hor.x)),(int)(1000*(-img_up.y-img_hor.y)),&blc_img.x,&blc_img.y);
  found_brc=findCorner(color_idx,(int)(1000*(-img_up.x+img_hor.x)),(int)(1000*(-img_up.y+img_hor.y)),&brc_img.x,&brc_img.y);

  if(print_now && debug_verbose) {
    pprintf(TextOutputStream,"after tl=(%d,%d) tr=(%d,%d) bl=(%d,%d) br=(%d,%d)\n",
            (found_tlc ? tlc_img.x : -1),(found_tlc ? tlc_img.y : -1),
            (found_trc ? trc_img.x : -1),(found_trc ? trc_img.y : -1),
            (found_blc ? blc_img.x : -1),(found_blc ? blc_img.y : -1),
            (found_brc ? brc_img.x : -1),(found_brc ? brc_img.y : -1));
  }

  bool good_xsize = false;
  if(found_tlc && found_trc) {
    diff_loc = trc_img - tlc_img;
    diff_loc.set(abs(diff_loc.x),abs(diff_loc.y));
    diff_loc += GVector::vector2d<int>(1,1);
    x_size = (int)(sqrt((double)diff_loc.sqlength()));
    good_xsize = (calcEdgeMask(tlc_img.x,tlc_img.y)==0 && calcEdgeMask(trc_img.x,trc_img.y)==0);
  }
  if(found_blc && found_brc && !good_xsize && 
     (calcEdgeMask(blc_img.x,blc_img.y)==0 && calcEdgeMask(brc_img.x,brc_img.y)==0)) {
    diff_loc = trc_img - tlc_img;
    diff_loc.set(abs(diff_loc.x),abs(diff_loc.y));
    diff_loc += GVector::vector2d<int>(1,1);
    x_size = (int)(sqrt((double)diff_loc.sqlength()));
  }

  bool good_ysize = false;
  if(found_tlc && found_blc) {
    diff_loc = tlc_img - blc_img;
    diff_loc.set(abs(diff_loc.x),abs(diff_loc.y));
    diff_loc += GVector::vector2d<int>(1,1);
    y_size = (int)(sqrt((double)diff_loc.sqlength()));
    good_ysize = (calcEdgeMask(tlc_img.x,tlc_img.y)==0 && 
                  (calcEdgeMask(blc_img.x,blc_img.y) & (OFF_EDGE_TOP | OFF_EDGE_BOTTOM))==0);
  }
  if(found_trc && found_brc && !good_ysize) {
    diff_loc = trc_img - brc_img;
    diff_loc.set(abs(diff_loc.x),abs(diff_loc.y));
    diff_loc += GVector::vector2d<int>(1,1);
    y_size = (int)(sqrt((double)diff_loc.sqlength()));
  }

  if(x_size <= 2 || y_size <=2)
    return;

  double g_conf,g_conf0,g_conf1,g_conf2,g_conf3,g_conf4,g_conf5; // goal confidence values
  double ge_conf,ge_conf0,ge_conf1,ge_conf2,ge_conf3; // goal edge confidence values

  // ******** process central part of goal **********
  double ratio_wh; // ratio of width to height
  int area;

  area = goal_reg->area;
  if(area < 16)
    return;

  ratio_wh = (double)x_size / y_size;

  bool found_left_edge,found_right_edge;
  found_left_edge = found_right_edge = false;

  vector2d eff_tlc_img(0.0,0.0),eff_blc_img(0.0,0.0),eff_trc_img(0.0,0.0),eff_brc_img(0.0,0.0);
  if(found_tlc){
    eff_tlc_img.set(V2COMP(tlc_img));
    eff_tlc_img +=  img_up  * .5;
    eff_tlc_img += -img_hor * .5; 
  }
  if(found_trc){
    eff_trc_img.set(V2COMP(trc_img));
    eff_trc_img +=  img_up  * .5;
    eff_trc_img +=  img_hor * .5; 
  }
  if(found_blc){
    eff_blc_img.set(V2COMP(blc_img));
    eff_blc_img += -img_up  * .5;
    eff_blc_img += -img_hor * .5; 
  }
  if(found_brc){
    eff_brc_img.set(V2COMP(brc_img));
    eff_brc_img += -img_up  * .5;
    eff_brc_img +=  img_hor * .5; 
  }

  // ********** process edges **********
  int sides[2]={LEFT_SIDE, RIGHT_SIDE};
  for(int side_idx=0; side_idx<2; side_idx++) {
    bool found_tc=false,found_bc=false;
    GVector::vector2d<int> tc_img,bc_img;
    // offset a half pixel to get real corner
    vector2d eff_bc,eff_tc;

    switch(sides[side_idx]) {
    case LEFT_SIDE:
      found_tc = found_tlc;
      found_bc = found_blc;
      tc_img = tlc_img;
      bc_img = blc_img;
      eff_tc = eff_tlc_img;
      eff_bc = eff_blc_img;
      break;
    case RIGHT_SIDE:
      found_tc = found_trc;
      found_bc = found_brc;
      tc_img = trc_img;
      bc_img = brc_img;
      eff_tc = eff_trc_img;
      eff_bc = eff_brc_img;
      break;
    }

    if(found_tc && found_bc && calcEdgeMask(bc_img.x,bc_img.y)==0) {
      VObject *cur_edge;
      cur_edge = &goal_edges[sides[side_idx]];

      found_left_edge = true;
      cur_edge->confidence = 1.0;

      vector3d triang_loc,sep_loc;
      triang_loc.set(0,0,0);
      sep_loc.set(0,0,0);

      vector3d bc_pix_dir;
      bool intersects=false;

      if(print_now && debug_verbose) {
        pprintf(TextOutputStream,"eff bc=(%g,%g) tc=(%g,%g)\n",eff_bc.x,eff_bc.y,eff_tc.x,eff_tc.y);
      }

      bc_pix_dir = getPixelDirection(eff_bc.x,eff_bc.y);

      // FIXME: Change this to a more optimized version
      intersects = GVector::intersect_ray_plane(camera_loc,bc_pix_dir,vector3d(0,0,0),vector3d(0,0,1.0),triang_loc);

      // Calc filters for green below goal, white to left/right of bottom
      vector2d b2t,green_loc_r,white_loc_r;
      b2t = eff_tc - eff_bc;

      vector2d b2t_dir;
      b2t_dir = b2t.norm();

      green_loc_r = eff_tc - b2t_dir*(b2t.length() + goal_outside_offset);
      white_loc_r = eff_tc - b2t_dir*(b2t.length() * (1.0 - (WallHeight*.67)/GoalHeight));
      white_loc_r += (sides[side_idx]==LEFT_SIDE ? -img_hor : img_hor) * 5;

      GVector::vector2d<int> green_loc,white_loc;
      green_loc.set((int)(green_loc_r.x+.5),(int)(green_loc_r.y+.5));
      white_loc.set((int)(white_loc_r.x+.5),(int)(white_loc_r.y+.5));

      clipToImage(green_loc.x,green_loc.y);
      clipToImage(white_loc.x,white_loc.y);

      int green_rle_idx,white_rle_idx;
      run *green_run,*white_run;
      int green_cnt=0,white_cnt=0;

      green_rle_idx = CMVision::FindStart(rmap,0,num_runs-1,green_loc.x,green_loc.y);
      white_rle_idx = CMVision::FindStart(rmap,0,num_runs-1,white_loc.x,white_loc.y);
      green_run = &rmap[green_rle_idx];
      white_run = &rmap[white_rle_idx];
      if(green_run->x <= green_loc.x && green_run->color == COLOR_GREEN) {
        green_cnt = reg[green_run->parent].area;
      }
      if(white_run->x <= white_loc.x && white_run->color == COLOR_WHITE) {
        white_cnt = reg[white_run->parent].area;
      }

      if(print_now && debug_verbose) {
        pprintf(TextOutputStream,"green loc (%d,%d), white loc (%d,%d)\n",
                green_loc.x,green_loc.y,
                white_loc.x,white_loc.y);
        pprintf(TextOutputStream,"green cnt %d, white cnt %d\n",
                green_cnt,white_cnt);
      }

      ge_conf0 = bound(img_up.dot(b2t.norm()),0.0,1.0);
      ge_conf0 = (ge_conf0 - .966) / (1 - .966); // fall to zero in 15 degrees
      ge_conf0 = bound(ge_conf0,0.0,1.0);
      
      ge_conf1 = 1.0;
      
      //ge_conf2 = uniform_with_gaussian_tails(pct_from_mean(x_size,y_size*2),.2,.2);
      ge_conf2 = (x_size >= 3 && y_size >= 8 && .25 <= ratio_wh && ratio_wh <= 8) ? 1.0 : 0.0;

      ge_conf3 = (green_cnt >= 20 && white_cnt >= 15) ? 1.0 : 0.0;

      if(calcEdgeMask(tc_img.x,tc_img.y)==0) {
        // calculate position using both top and bottom pixels
        vector3d top_dir,bot_dir;

        top_dir = getPixelDirection(eff_tc.x,eff_tc.y);
        bot_dir = getPixelDirection(eff_bc.x,eff_bc.y);

        vector2d plane_dir,top_dz_dir,bot_dz_dir;
        alignVertical(top_dir,bot_dir,plane_dir,top_dz_dir,bot_dz_dir);

        vector2d top_dz_loc,bot_dz_loc;
        if(findLocVerticalSeperation(top_dz_dir,bot_dz_dir,GoalHeight,top_dz_loc,bot_dz_loc)) {
          vector2d edge_dz_loc,edge_xy_loc;
          edge_dz_loc = bot_dz_loc;
          edge_xy_loc = plane_dir * edge_dz_loc.x;
          sep_loc.set(edge_xy_loc.x,edge_xy_loc.y,edge_dz_loc.y);
          sep_loc += camera_loc;

          cur_edge->loc = sep_loc;
        }
      } else {
        // no useful top left edge, have to use triangulation
        if(intersects) {
          cur_edge->loc = triang_loc;
          // don't do triangulation for goal posts that are far away because it is not accurate
          if(triang_loc.length() > 1000.0)
            ge_conf1 = 0.0;
        } else {
          ge_conf1 = 0.0;
        }
      }

      ge_conf = ge_conf0 * ge_conf1 * ge_conf2 * ge_conf3;

      cur_edge->confidence = ge_conf;

      if(print_now && debug_conf) {
        pprintf(TextOutputStream,"conf %g (0=%g 1=%g 2=%g 3=%g)\n",ge_conf,ge_conf0,ge_conf1,ge_conf2,ge_conf3);
      }

      if(cur_edge->confidence > 0.0) {
        vector2d ground_loc(cur_edge->loc.x,cur_edge->loc.y);
        cur_edge->left     = ground_loc.angle();
        cur_edge->right    = ground_loc.angle();
        cur_edge->distance = ground_loc.length();
        cur_edge->confInFrontOfIR = 0.0;
        cur_edge->edge = calcEdgeMask(bc_img.x,bc_img.y) | calcEdgeMask(tc_img.x,tc_img.y);
      }

      if(print_now && debug_verbose)
        pprintf(TextOutputStream,"side %d tri_loc=(%g,%g,%g) sep_loc=(%g,%g,%g)\n",sides[side_idx],
                triang_loc.x,triang_loc.y,triang_loc.z,
                sep_loc.x,sep_loc.y,sep_loc.z);
    }
  }

  // ******** process central part of goal **********
  vector2d main_goal_green_probe_pt_r;
  GVector::vector2d<int> main_goal_green_probe_pt;
  int main_goal_green_cnt;

  //cout << "bl (" << bottom_loc.x << "," << bottom_loc.y << ") tl (" << top_loc.x << "," << top_loc.y 
  //     << ") goo " << goal_outside_offset << endl;

  if(!found_tlc || !found_blc || !found_trc || !found_brc){
    if(print_now)
      pprintf(TextOutputStream,"Couldn't find a goal corner, assuming no main goal to find, corners=(%d,%d,%d,%d)\n",
              found_tlc?1:0,found_blc?1:0,found_trc?1:0,found_brc?1:0);
    return;
  }

  main_goal_green_probe_pt_r.set(bottom_loc.x,bottom_loc.y);
  main_goal_green_probe_pt_r += (vector2d(bottom_loc.x - top_loc.x, bottom_loc.y - top_loc.y)).norm() * goal_outside_offset;
  main_goal_green_probe_pt.set((int)(main_goal_green_probe_pt_r.x + .5),(int)(main_goal_green_probe_pt_r.y + .5));
  
  if(print_now && debug_verbose)
    pprintf(TextOutputStream,"main green probe = (%d,%d)\n",
            main_goal_green_probe_pt.x,main_goal_green_probe_pt.y);

  main_goal_green_cnt = 0;
  if(onImage(main_goal_green_probe_pt.x,main_goal_green_probe_pt.y)){
    if(getColorUnsafe(main_goal_green_probe_pt.x,main_goal_green_probe_pt.y)==COLOR_GREEN){
      int green_rle_idx;
      run *green_run;
      
      green_rle_idx = CMVision::FindStart(rmap,0,num_runs-1,main_goal_green_probe_pt.x,main_goal_green_probe_pt.y);
      green_run = &rmap[green_rle_idx];
      if(green_run->x <= main_goal_green_probe_pt.x && green_run->color == COLOR_GREEN){
        main_goal_green_cnt = reg[green_run->parent].area;
      }
    }
  }else{
    main_goal_green_cnt = width*height;
  }

  g_conf4 = 1.0;

  {
    float sp; // semiperimeter
    float len_l,len_r,len_t,len_b;
    float angle; // average angle of two opposite corners
    float sq_expected_area;
    float expected_area;
    float error;
    len_l = GVector::distance(eff_tlc_img,eff_blc_img);
    len_r = GVector::distance(eff_trc_img,eff_brc_img);
    len_t = GVector::distance(eff_tlc_img,eff_trc_img);
    len_b = GVector::distance(eff_blc_img,eff_brc_img);
    sp = (len_l + len_r + len_t + len_b)/2.0;
    angle = (acos((eff_tlc_img-eff_blc_img).norm().dot((eff_tlc_img-eff_trc_img).norm())) +
             acos((eff_brc_img-eff_blc_img).norm().dot((eff_brc_img-eff_trc_img).norm()))) / 2.0;
    // area of the quadrilateral defined by the four corner points
    sq_expected_area = (sp-len_l)*(sp-len_r)*(sp-len_t)*(sp-len_b) - len_l*len_r*len_t*len_b*cos(angle);
    if(sq_expected_area < 0.0)
      sq_expected_area = 0.0;
    expected_area = sqrt(sq_expected_area);
    error = pct_from_mean(area,expected_area);
    g_conf4 = ramp_dn(error,.2,.6);
    if(print_now && debug_verbose){
      pprintf(TextOutputStream,"len:l=%g r=%g t=%g b=%g, sp=%g, angle=%g\n",len_l,len_r,len_t,len_b,sp,angle);
      pprintf(TextOutputStream,"area=%d expected_area=%g error=%g conf=%g\n",area,expected_area,error,g_conf4);
    }
  }

  {
    g_conf5 = 1.0;

    int cnts=0;
    int total_bad_cnt=0;
    for(int side=-1; side<2; side+=2){
      for(int i=0; i<3; i++){
        vector2d samp_loc_r;
        
        if(side == -1){
          samp_loc_r = vector2d(V2COMP(tlc_img + blc_img))/2.0 - img_hor*(i*5.0 + 5.0);
        }else{
          samp_loc_r = vector2d(V2COMP(trc_img + brc_img))/2.0 + img_hor*(i*5.0 + 5.0);
        }
        
        GVector::vector2d<int> samp_loc;
        samp_loc.set((int)(samp_loc_r.x+.5),(int)(samp_loc_r.y+.5));
        
        if(!onImage(samp_loc.x,samp_loc.y))
          continue;
        
        int rle_idx;
        run *run;
        int bad_cnt=0;
        
        rle_idx = CMVision::FindStart(rmap,0,num_runs-1,samp_loc.x,samp_loc.y);
        run = &rmap[rle_idx];
        if(run->x <= samp_loc.x && samp_loc.x <= run->x + run->width - 1){
          switch(run->color){
            case COLOR_GREEN:
            case COLOR_YELLOW:
              bad_cnt = reg[run->parent].area;
              break;
            default:
              break;
          }
        }
        
        cnts++;
        total_bad_cnt += bad_cnt;

        if(print_now && debug_verbose) {
          pprintf(TextOutputStream,"samp loc (%d,%d)\n",
                  samp_loc.x,samp_loc.y);
          pprintf(TextOutputStream,"bad cnt %d\n",
                  bad_cnt);
        }
      }
    }

    float avg_bad_cnt=0.0;
    if(cnts != 0){
      avg_bad_cnt = (float)total_bad_cnt / cnts;
      
      g_conf5 = ramp_dn(avg_bad_cnt,500.0,1000.0);
    }else{
      g_conf5 = 1.0;
    }

    if(print_now && debug_verbose) {
      pprintf(TextOutputStream,"cnts=%d avg_bad_cnt=%g conf=%g\n",
              cnts,avg_bad_cnt,g_conf5);
    }
  }

  g_conf0 = (area > 2000) ? 1.0 : similar(ratio_wh,2.0);
  
  g_conf1 = ((area > 2000) || 
             (.866 <= ratio_wh && ratio_wh <= 3.0  &&  area > 20 )) ? 1.0 : 0.0;
  g_conf2 = (x_size >= 3 && y_size >=3) ? 1.0 : 0.0;

  g_conf3 = (main_goal_green_cnt >= 30) ? 1.0 : 0.0;

  g_conf = g_conf0 * g_conf1 * g_conf2 * g_conf3 * g_conf4 * g_conf5;
  if(print_now && debug_conf)
    pprintf(TextOutputStream,"area %d x %d y %d gc %d conf %g (0=%g 1=%g 2=%g 3=%g 4=%g 5=%g)\n",
            area,x_size,y_size,main_goal_green_cnt,g_conf,g_conf0,g_conf1,g_conf2,g_conf3,g_conf4,g_conf5);

  if(g_conf < goal_edges[0].confidence)
    g_conf = goal_edges[0].confidence;
  if(g_conf < goal_edges[1].confidence)
    g_conf = goal_edges[1].confidence;

  if(g_conf > 0.0) {
    double dist;
    
    dist = FocalDistV * GoalHeight / y_size;

    vector3d goal_loc,goal_dir;

    goal_dir = getPixelDirection(goal_reg->cen_x, goal_reg->cen_y);

    //sprintf(DebugInfo,"%g ",atan2a(goal_dir.z,hypot(goal_dir.x,goal_dir.y)));

    if(atan2(goal_dir.z,hypot(goal_dir.x,goal_dir.y)) <= .2){
      if(onImage(top_loc.x,top_loc.y) && calcEdgeMask(top_loc.x,top_loc.y)==0){
        goal_loc = camera_loc + goal_dir * dist;
      }else{
        // no useful top edge, fall back on triangulation

        bool intersects = false;
        vector2d eff_bot_loc;
        vector3d triang_loc;
        vector3d bot_pix_dir;

        eff_bot_loc.set((double)bottom_loc.x,(double)bottom_loc.y);
        eff_bot_loc += img_up * -.5;
        bot_pix_dir = getPixelDirection(eff_bot_loc.x,eff_bot_loc.y);

        intersects = GVector::intersect_ray_plane(camera_loc,bot_pix_dir,vector3d(0,0,0),vector3d(0,0,1.0),triang_loc);
        if(intersects){
          goal_loc = triang_loc;
        }else{
          if(print_now && debug_conf)
            pprintf(TextOutputStream,"squashing goal because no useful distance estimate\n");
          g_conf = 0.0;
        }
      }
      
      if(g_conf > 0.0){
        goal->confidence = g_conf;
        
        goal->loc = goal_loc;
        goal->distance = hypot(goal_loc.x,goal_loc.y);
        
        findSpan(goal->left,goal->right,goal_reg->x1,goal_reg->x2,goal_reg->y1,goal_reg->y2);
        if(goal_edges[LEFT_SIDE].confidence > 0.0){
          goal->left = goal_edges[LEFT_SIDE].left;
        }else{
          bool set_one = false;
          if(found_blc){
            goal->left = groundAngle(blc_img.x,blc_img.y); // FIXME: add half pixel offset
            set_one = true;
          }
          if(found_tlc &&
             (!set_one || calcEdgeMask(tlc_img.x,tlc_img.y)==0))
            goal->left = groundAngle(tlc_img.x,tlc_img.y); // FIXME: add half pixel offset
        }
        if(goal_edges[RIGHT_SIDE].confidence > 0.0){
          goal->right = goal_edges[RIGHT_SIDE].right;
        }else{
          bool set_one = false;
          if(found_brc){
            goal->right = groundAngle(brc_img.x,brc_img.y); // FIXME: add half pixel offset
            set_one = true;
          }
          if(found_trc &&
             (!set_one || calcEdgeMask(trc_img.x,trc_img.y)==0))
            goal->right = groundAngle(trc_img.x,trc_img.y); // FIXME: add half pixel offset
        }
        
        goal->edge = calcEdgeMask(goal_reg);
        
        goal_info->reg = goal_reg;
      }
    }else{
      if(debug_conf)
        pprintf(TextOutputStream,"Nej Angel!  angle=%g\n",atan2(goal_dir.z,hypot(goal_dir.x,goal_dir.y)));
    }
  }

  if(print_now) {
    pprintf(TextOutputStream,"goal of color %d conf %g loc (%g,%g,%g) dist %g left %g right %g edge %d\n",
            color_idx,goal->confidence,goal->loc.x,goal->loc.y,goal->loc.z,goal->distance,goal->left,goal->right,goal->edge);
    pprintf(TextOutputStream,"goal left  edge of color %d conf %g loc (%g,%g,%g) dist %g left %g right %g edge %d\n",
            color_idx,goal_edges[LEFT_SIDE].confidence,goal_edges[LEFT_SIDE].loc.x,goal_edges[LEFT_SIDE].loc.y,goal_edges[LEFT_SIDE].loc.z,goal_edges[LEFT_SIDE].distance,goal_edges[LEFT_SIDE].left,goal_edges[LEFT_SIDE].right,goal_edges[LEFT_SIDE].edge);
    pprintf(TextOutputStream,"goal right edge of color %d conf %g loc (%g,%g,%g) dist %g left %g right %g edge %d\n",
            color_idx,goal_edges[RIGHT_SIDE].confidence,goal_edges[RIGHT_SIDE].loc.x,goal_edges[RIGHT_SIDE].loc.y,goal_edges[RIGHT_SIDE].loc.z,goal_edges[RIGHT_SIDE].distance,goal_edges[RIGHT_SIDE].right,goal_edges[RIGHT_SIDE].right,goal_edges[RIGHT_SIDE].edge);
  }

  if(debug_sort && color_idx==debug_sort_color){
    if(goal        ->confidence > .4 ||
       goal_edges[0].confidence > .4 ||
       goal_edges[1].confidence > .4){
      DebugClass = 1;
      sprintf(&DebugInfo[strlen(DebugInfo)],"%g %g %g %g %g %g",
              goal->confidence,goal_edges[0].confidence,goal_edges[1].confidence,
              goal->distance,goal_edges[0].distance,goal_edges[1].distance);
    }
  }
}

// var(X) = E
// var(AX) = A E A^T
void Vision::findRobots(int color_idx,VObject *robots,VisionObjectInfo *robot_infos,int max_robots)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findRobots",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  static const bool debug_robots = false;
  static const bool send_shape_train = false;
  static const bool send_dist_train = false;

  region *robot_reg;
  region *or_reg;
  vector3d dir;
  int i,n,red_reg_cnt;

  static region *robot_regions[3]; // FIXME: assumes max_robots<3

  // clear out current values
  for(i=0; i<max_robots; i++) {
    memset(&robots[i],0,sizeof(VObject));
    robots[i].confidence = 0.0;
    robot_regions[i] = NULL;
  }

  if(debug_robots){
    pprintf(TextOutputStream,"img_up.x: %f img_up.y: %f\n",img_up.x,img_up.y);
  }

  n = 0;
  red_reg_cnt = 0;
  robot_reg = color[color_idx].list;
  while(robot_reg && n<3 && robot_reg->area>10 && red_reg_cnt<20){
    red_reg_cnt++;

    // make sure that robot is not underneath orange since that would probably just be a ball shadow
    if(color_idx == COLOR_RED){
      bool skip=false;
      or_reg = color[COLOR_ORANGE].list;
      for(int or_cnt=0; !skip && or_reg!=NULL && or_reg->area>=8 && or_cnt<10; or_reg=or_reg->next, or_cnt++){
        double x,y;
        x = robot_reg->cen_x;
        y = robot_reg->cen_y;
        //if(debug_robots)
        //  pprintf(TextOutputStream,"testing red region and orange region, cen=(%8.4g,%8.4g), ball=([%3d,%3d],[%3d,%3d])\n",
        //          x,y,
        //          or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2);
        if(!skip && or_reg->x1-0.5 <= x && x <= or_reg->x2+0.5 &&
           y > or_reg->y1){
          if(debug_robots)
            pprintf(TextOutputStream,"rejecting red region because underneath orange, cen=(%8.4g,%8.4g), ball=([%3d,%3d],[%3d,%3d])\n",
                    x,y,
                    or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2);
          robot_reg = robot_reg->next;
          skip=true;
        }
        if(!skip && or_reg->x1 <= robot_reg->x2 && robot_reg->x1 <= or_reg->x2 &&
           y > or_reg->cen_y){
          if(debug_robots)
            pprintf(TextOutputStream,"rejecting red region because underneath orange on side, cen=(%8.4g,%8.4g), ball=([%3d,%3d],[%3d,%3d])\n",
                    x,y,
                    or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2);
          robot_reg = robot_reg->next;
          skip=true;
        }
      }
      if(skip)
        continue;
    }

    int run_idx = robot_reg->run_start;
    int run_x,run_y,run_width;
    double xtemp, xmoment;
    double ytemp, ymoment;
    double xymoment;
    double run_cen_x;
    xmoment = 0;
    ymoment = 0;
    xymoment = 0;
    while(run_idx != 0){
      run_x = rmap[run_idx].x;
      run_y = rmap[run_idx].y;
      run_width = rmap[run_idx].width;
      run_cen_x = (run_x + (run_width-1)/2.0);

      ytemp = run_y - robot_reg->cen_y;
      ymoment += run_width * sq(ytemp);
      
      // + sum of squared x positions
      // sum_i=1^n (i^2) = n*(n+1)*(2*n+1)/6;
      {
	int before_x,end_x;
	ulong sum_sq_to_before,sum_sq_to_end;
	before_x = run_x-1;
	end_x    = run_x+run_width-1;
	if(before_x > 0)
	  sum_sq_to_before = before_x*(before_x+1)*(2*before_x+1)/6;
	else
	  sum_sq_to_before = 0;
	sum_sq_to_end    =    end_x*(   end_x+1)*(2*   end_x+1)/6;
	xtemp = sum_sq_to_end - sum_sq_to_before;
      }
      xmoment += xtemp;
      // - 2*sum of x positions*centroid
      xmoment += -2*run_cen_x*run_width*robot_reg->cen_x;
      // + sum of centroid squared
      xmoment += run_width*sq(robot_reg->cen_x);
      
      xymoment += ytemp*run_cen_x*rmap[run_idx].width;
      run_idx=rmap[run_idx].next;
    }
    if(false){
      xmoment  /= robot_reg->area;
      ymoment  /= robot_reg->area;
      xymoment /= robot_reg->area;
    }else{
      xmoment  /= sq(robot_reg->area);
      ymoment  /= sq(robot_reg->area);
      xymoment /= sq(robot_reg->area);
    }
    if(debug_robots){
      pprintf(TextOutputStream,
	      "cen=(%g,%g) size=%d xmoment:%f ymoment:%f xymoment:%f\n",
	      robot_reg->cen_x,robot_reg->cen_y,
	      robot_reg->area,
	      xmoment,ymoment,xymoment);
    }
    
    if(send_shape_train){
      //if(n<3){
      //  robots[n].loc.x = (double)robot_reg->x1;
      //  robots[n].loc.y = (double)robot_reg->y1;
      //  robots[n].loc.z = (double)robot_reg->x2;
      //  robots[n].orientation = (double)robot_reg->y2;
      //  robots[n].left = xmoment;
      //  robots[n].right = ymoment;
      //  robots[n].confidence = xymoment;
      //  robots[n].distance = (double)robot_reg->area;
      //}
      if(RegionLabelsNew && RegionLabelsOld){
        char buf[256];
        ulong reg_id;
        RegionLabelMapT::iterator reg_iter;
        reg_id = calcRegionId(robot_reg);
        reg_iter = RegionLabelsOld->find(reg_id);
        if(reg_iter == RegionLabelsOld->end()){
          sprintf(buf,"%d %d %d %d %g %g %g %d none",
                  robot_reg->x1,robot_reg->y1,robot_reg->x2,robot_reg->y2,
                  xmoment,ymoment,xymoment,
                  robot_reg->area);
          (*RegionLabelsNew)[reg_id] = strdup(buf);
        }else{
          sprintf(buf,"%d %d %d %d %g %g %g %d %s",
                  robot_reg->x1,robot_reg->y1,robot_reg->x2,robot_reg->y2,
                  xmoment,ymoment,xymoment,
                  robot_reg->area,
                  (*RegionLabelsOld)[reg_id]);
          (*RegionLabelsNew)[reg_id] = strdup(buf);
        }
        pprintf(TextOutputStream,"added label, RegionLabels now has %u label(s)\n",RegionLabelsNew->size());
      }
    }

    if(!send_shape_train && (n < 3)){
      if(robot_reg->area < 50){
      }
      else if(robot_reg->area > 2000){
	if(debug_robots)
	  pprintf(TextOutputStream,"REGION[%d]: DOG IN FACE!\n",n);
	robots[n].confidence = 1.0;
	robots[n].distance = 250.0;
	robots[n].orientation = 0.0;
	dir = getPixelDirection(robot_reg->cen_x, robot_reg->cen_y);
	robots[n].loc = camera_loc + dir*robots[n].distance;
        robot_regions[n] = robot_reg;
      }
      else if(xmoment <= 0.182475){
	if(robot_reg->area <= 546){
	  if(debug_robots)
	    pprintf(TextOutputStream,"REGION[%d]: nothing!\n",n);
	}
	else{
	  if(ymoment > 0.119776){
	    if(debug_robots)
	      pprintf(TextOutputStream,"REGION[%d]: nothing!\n",n);
	  }
	  else{
	    if(debug_robots)
	      pprintf(TextOutputStream,"REGION[%d]: dog REAR!\n",n);
	    robots[n].confidence = 1.0;
	    robots[n].distance = 500.0;
	    robots[n].orientation = 0.0;
	    dir = getPixelDirection(robot_reg->cen_x, robot_reg->cen_y);
	    robots[n].loc = camera_loc + dir*robots[n].distance;
            robot_regions[n] = robot_reg;
	  }
	}
      }
      else{
	if(ymoment <= 0.084961){
	  if(xmoment > 0.294195){
	    if(debug_robots)
	      pprintf(TextOutputStream,"REGION[%d]: nothing!\n",n);
	  }
	  else{
	    if(ymoment <= 0.061037){
	      if(debug_robots)
		pprintf(TextOutputStream,"REGION[%d]: dog REAR!\n",n);
	      robots[n].confidence = 1.0;
	      robots[n].distance = 500.0;
	      robots[n].orientation = 0.0;
	      dir = getPixelDirection(robot_reg->cen_x, robot_reg->cen_y);
	      robots[n].loc = camera_loc + dir*robots[n].distance;
              robot_regions[n] = robot_reg;
	    }
	    else{
	      if(debug_robots)
		pprintf(TextOutputStream,"REGION[%d]: nothing!\n",n);
	    }
	  }
	}
	else{
	  if(ymoment > 0.133734){
	    if(debug_robots)
	      pprintf(TextOutputStream,"REGION[%d]: nothing!\n",n);
	  }
	  else{
	    if(xmoment <= 0.395579){
	      if(debug_robots)
		pprintf(TextOutputStream,"REGION[%d]: dog SIDE!\n",n);
	      robots[n].confidence = 1.0;
	      robots[n].distance = 500.0;
	      robots[n].orientation = 1.5;
	      dir = getPixelDirection(robot_reg->cen_x, robot_reg->cen_y);
	      robots[n].loc = camera_loc + dir*robots[n].distance;
              robot_regions[n] = robot_reg;
	    }
	    else{
	      if(xmoment <= 0.487548){
		if(debug_robots)
		  pprintf(TextOutputStream,"REGION[%d]: dog FRONT!\n",n);
		robots[n].confidence = 1.0;
		robots[n].distance = 500.0;
		robots[n].orientation = 3.1;
		dir = getPixelDirection(robot_reg->cen_x, robot_reg->cen_y);
		robots[n].loc = camera_loc + dir*robots[n].distance;
                robot_regions[n] = robot_reg;
	      }
	      else{
		if(debug_robots)
		  pprintf(TextOutputStream,"REGION[%d]: nothing!\n",n);
	      }
	    }
	  }  
	}
      }
      //end of orientation tree
    
      //dist tree
      //side
      int robot_h,robot_w;
      robot_h = robot_reg->y2 - robot_reg->y1 + 1;
      robot_w = robot_reg->x2 - robot_reg->x1 + 1;

      //side
      if(robots[n].confidence == 1.0 && robots[n].orientation == 1.5){
	if(debug_robots){
	  pprintf(TextOutputStream,"doing robot side distance\n");
	}
	if(robot_h <= 30){
	  if(robot_h > 20){
	    robots[n].distance = 750;
	  }
	  else{
	    robots[n].distance = 1000;
	  }
	}
	else{
	  if(robot_w <= 87){
	    robots[n].distance = 500;
	  }
	  else{
	    robots[n].distance = 250;
	  }
	}
      }//end side

      if(robots[n].confidence == 1.0 && robots[n].orientation == 0.0){
	if(robot_h <= 22){
	  if(robot_h <= 11){
	    robots[n].distance = 1000;
	  }
	  else{
	    robots[n].distance = 750;
	  }
	}
	else{
	  if(robot_w <= 53){
	    robots[n].distance = 500;
	  }
	  else{
	    robots[n].distance = 250;
	  }
	}
      }//end rear

      if(robots[n].confidence == 1.0 && robots[n].orientation == 3.1){
	if(robot_w <= 33){
	  if(robot_w <= 22){
	    robots[n].distance = 1000;
	  }
	  else{
	    robots[n].distance = 750;
	  }
	}
	else{
	  if(robot_h <= 39){
	    robots[n].distance = 500;
	  }
	  else{
	    robots[n].distance = 250;
	  }
	}
      }

      
      //end dist tree
    }

    if(send_dist_train){
      if(n<3){
	robots[n].loc.x = (double)robot_reg->x1;
	robots[n].loc.y = (double)robot_reg->y1;
	robots[n].loc.z = (double)robot_reg->x2;
	robots[n].orientation = (double)robot_reg->y2;
	//robots[n].left = xmoment;
	//robots[n].right = ymoment;
	//robots[n].confidence = xymoment;
	robots[n].distance = (double)robot_reg->area;
      }
    }

    if(debug_robots){
      if(robots[n].confidence == 1.0){
	pprintf(TextOutputStream,"robot[%d] dist:[%f] orientation[%f]\n",n,robots[n].distance,robots[n].orientation);
      }
    }

    robot_reg = robot_reg->next;
    if(robots[n].confidence > 0.0){
      n++;
    }
  }
  
  //FIXME:  if head is tilted, data will be wrong, so we won't report it
  if(!(img_up.x > -0.2 && img_up.x < 0.2)){
    if(debug_robots){
      pprintf(TextOutputStream,"head angle not ok, disregarding robots!\n");
    }
    robots[0].confidence = 0.0;
    robots[1].confidence = 0.0;
    robots[2].confidence = 0.0;
  } 

  if(head_angles[0] < RAD(-60.0) &&
     fabs(head_angles[1]) < RAD(80.0) &&
     head_angles[2] < RAD(10.0)){
    robots[0].confidence = 0.0;
    robots[1].confidence = 0.0;
    robots[2].confidence = 0.0;
  }

  //fill in robot_infos from robot_regions
  for(int robot_idx=0; robot_idx<max_robots; robot_idx++) {
    robot_infos[robot_idx].reg = robot_regions[robot_idx];
  }
}

void Vision::findChargingTower(VObject *tower)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findChargingTower",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  static const bool debug_tower = true;
  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_tower)
    frame_cnt = (frame_cnt + 1) % print_period;

  bool print_now = (frame_cnt == 0);

  color_class_state *pink, *cyan;
  region *pink_reg, *cyan_reg;

  pink = &color[COLOR_PINK];
  cyan = &color[COLOR_CYAN];

  pink_reg = pink->list;
  cyan_reg = cyan->list;

  if(!pink_reg || !cyan_reg)
    return;

  tower->confidence = 0.0;

  region *best_pink = NULL;
  region *best_cyan = NULL;
  double best_confidence = -1.0;

  // Step through the 3 largest pink regions (if they
  // exist)
  for(int ip=0; ip<3 && pink_reg!=NULL; ip++) {
    
    // approximate the radius of the pink region
    // (assuming it is a circle)
    double pink_r = sqrt(pink_reg->area/M_PI);

    // Step through the 3 largest cyan regions (if they
    // exist)
    cyan_reg = cyan->list;
    for(int ic=0; ic<3 && cyan_reg!=NULL; ic++) {

      // Find the distance from the center of this cyan region
      // to the center of the current pink region.
      double c_to_p = (pink_reg->cen_x - cyan_reg->cen_x) * 
	(pink_reg->cen_x - cyan_reg->cen_x) +
	(pink_reg->cen_y - cyan_reg->cen_y) *
	(pink_reg->cen_y - cyan_reg->cen_y);
      c_to_p = sqrt(c_to_p);

      // Find the ration of the distance from cyan to pink over
      // the radius of the pink region.
      double ratio = c_to_p/pink_r;

      // We will take the dot product of the vector from
      // cyan to pink with the up vector (after normalizing).
      // This will give us cos(theta) which should be 1
      // in a perfect world 'cuz the tower is vertical.
      vector2d v_cp = vector2d(pink_reg->cen_x, pink_reg->cen_y) -
	vector2d(cyan_reg->cen_x, cyan_reg->cen_y);

      vector2d v_cp_norm = v_cp.norm();

      double dot_prod = v_cp_norm.dot(img_up);

      // We need a third filter to make sure we're not looking
      // at a marker. Let's look at 9 pixels -v_cp pixels
      // from the cyan regions centroid and make sure they're
      // green.
      int start_x = (int)(cyan_reg->cen_x - v_cp.x);
      int start_y = (int)(cyan_reg->cen_y - v_cp.y);
      int num_green = 0;
      for(int x=-1; x<=1; x++) {
	for(int y=-1; y<=1; y++) {
	  if(getNearColor(start_x + x, start_y + y)==COLOR_GREEN)
	    num_green++;
	}
      }

      // What is our confidence according to finding green
      // pixels below our pink over cyan blobs?
      double color_conf = num_green/9.0;

      // What is our confidence according to the ratio?
      // Our test data suggests it will be 1.74 or there abouts.
      // We've seen deviations of up to .1. Let's be generous and
      // say you need to be within .3 of our mean.
      double rat_conf = max(0.0, 1.0 - fabs(ratio - 1.74)/0.3);

      // What is our confidence according to our dot product?
      double dot_conf = max(0.0, 1.0 - acos(dot_prod)/15);

      // If our confidence in our current pair of pink and cyan
      // regions is the best we've seen so far, record the
      // current two regions.
      if(dot_conf*rat_conf*color_conf > best_confidence) {
	best_confidence = dot_conf*rat_conf*color_conf;
	best_pink = pink_reg;
	best_cyan = cyan_reg;
      }
      
      cyan_reg = cyan_reg->next;
    }
    
    pink_reg = pink_reg->next;
  }
    
  // The unit vector from the camera to each region
  vector3d pink_dir,cyan_dir;

  // The centroid of each region
  vector2d pink_ctr,cyan_ctr;

  // The vector in the x-y plane that contains the vertical plane that passes
  // through the tower.
  vector2d xy_dir;
  vector2d top_dz_dir,bot_dz_dir; // vector in distance (radial direction) and elevation

  // This will be the location of the tower projected onto a vertical plane.
  // The x coordinate is distance from the origin in the plane and y is
  // distance above the x-y plane.
  vector2d tower_dz_loc;

  // This vector points along the plane referenced above.
  vector2d tower_xy_loc;
    
  // 3-space location of the tower in ego-centric coords
  vector3d tower_loc;

  // Retrieve the location of our centroids and find the unit vector
  // that points from the camera to those pixels in the image.
  pink_ctr.set(best_pink->cen_x, best_pink->cen_y);
  pink_dir = getPixelDirection(pink_ctr.x, pink_ctr.y);

  cyan_ctr.set(best_cyan->cen_x, best_cyan->cen_y);
  cyan_dir = getPixelDirection(cyan_ctr.x, cyan_ctr.y);

  // The last 3 parameters are buffers to hold the return value. We're
  // projecting pink_dir and cyan_dir onto a vertical plane. xy_dir will
  // describe the plane. It passes through the origin and extends in the
  // xy_dir direction (z varies 'cuz it's vertical). top_dz_dir is the
  // projection of the pink_dir vector onto the plane. bot_dz_dir is the
  // projection of the cyan_dir vector.
  alignVertical(pink_dir, cyan_dir,
		xy_dir, top_dz_dir, bot_dz_dir);
    
  // vector between the two centroids
  vector2d pink_to_cyan_img;
  pink_to_cyan_img = pink_ctr - cyan_ctr;
  pink_to_cyan_img.normalize();
    
  vector2d pink_dz_loc, cyan_dz_loc;
  pink_dz_loc.set(0,0); cyan_dz_loc.set(0,0);

  // This will fill in the pink_dz_loc and cyan_dz_loc vectors with
  // the locations of those blobs in the vertical plane. The 61.5
  // is the real life separation between the centroids of the two
  // blobs in millimeters.
  findLocVerticalSeperation(top_dz_dir, bot_dz_dir, 61.5,
			    pink_dz_loc, cyan_dz_loc);
      
  // Average the two vectors to get the center point between
  // the pink blob and the cyan blob.
  tower_dz_loc = (pink_dz_loc + cyan_dz_loc) / 2.0;

  // Multiply the actual distance to the tower by the correct
  // direction vector.
  tower_xy_loc = xy_dir * tower_dz_loc.x;
    
  // Go back to a normal 3-space representation of the tower location.
  // We know our xy position from t_xy_loc. We have our height in
  // t_dz_loc.
  tower_loc.set(tower_xy_loc.x, tower_xy_loc.y, tower_dz_loc.y);
    
  // Our origin for all of this was the camera's location. Correct for
  // that.
  tower_loc += camera_loc;
    
  tower->confidence = best_confidence;
  tower->loc = tower_loc;
  tower->distance = sqrt(tower_loc.x*tower_loc.x + tower_loc.y*tower_loc.y);
    
  if(debug_sort) {
    if(tower->confidence > .5){
      DebugClass = 1;
      sprintf(DebugInfo,"(%g,%g,%g)",V3COMP(tower->loc));
    }
  }

  if(tower->confidence > .5 && print_now){
    pprintf(TextOutputStream,"conf=%g loc=(%g,%g,%g)",tower->confidence,V3COMP(tower->loc));
  }
}

// What percentage of the inner region is inside of the outer region?
double Vision::percRegionInside(region *outer, region *inner) {
  double left, right, top, bottom, width, height;

  left = max(outer->x1, inner->x1);
  right = min(outer->x2, inner->x2);
  top = max(outer->y1, inner->y1);
  bottom = min(outer->y2, inner->y2);

  width = right - left + 1;
  width = max(width, 0.0);

  height = bottom - top + 1;
  height = max(height, 0.0);

  double bb_area = (inner->x2 - inner->x1 + 1) * (inner->y2 - inner->y1 + 1);

  return width*height/bb_area;
}

void Vision::findChargingBullseye(VObject *bullseye)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findChargingBullseye",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  static const bool debug_bullseye = 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_bullseye)
    frame_cnt = (frame_cnt + 1) % print_period;

  bool print_now = (frame_cnt == 0);

  bullseye->confidence = 0.0;

  color_class_state *cyan, *green;
  region *cyan_reg, *green_reg;

  cyan=&color[COLOR_CYAN];
  cyan_reg = cyan->list;

  green = &color[COLOR_GREEN];
  green_reg = green->list;

  if(!cyan_reg || !green_reg)
    return;

  region *best_green = NULL, *best_cyan = NULL;
  double best_conf = -1.0;

  // We'll step through our 3 largest cyan regions
  for(int ic=0; ic<3 && cyan_reg!=NULL; ic++) {
    
    // Step through our 10 largest green regions
    green_reg = green->list;
    for(int ig=0; ig<10 && green_reg!=NULL; ig++) {
      
      // Get our centroids in an easy to work with form
      vector2d cyan_ctr = vector2d(cyan_reg->cen_x, cyan_reg->cen_y);
      vector2d green_ctr = vector2d(green_reg->cen_x, green_reg->cen_y);

      // Calculate the pixel distance between the two centroids.
      // This should be darn close to zero. In practice we see as
      // high as 6 or 7 pixels difference.
      double distance = (cyan_ctr - green_ctr).length();

      // What's the ration of our two areas? From our training data
      // it should be about 3.553 and vary by about .4
      double ratio = ((double)cyan_reg->area)/((double)green_reg->area);

      // This should be 1.0 - all of the bounding box for the green region
      // should be inside of the cyan region.
      double perc = percRegionInside(cyan_reg, green_reg);

      // The 3.553 is the expected ratio. 0.6 is meant to be the standard
      // deviation. It is artificially high. - .4 is more realistic.
      double rat_conf = gaussian_with_min((ratio - 3.553)/0.6, 1.0e-4); 

      // The distance should be 0. Standard dev of 10 (way high - 4 more reasonable).
      double dist_conf = gaussian_with_min(distance/10.0, 1.0e-4);

      // If this pair of regions is our best candidate, save the two
      // regions and record our new best confidence estimate.
      double conf = perc*dist_conf*rat_conf;
      if(conf > best_conf) {
	best_green = green_reg;
	best_cyan = cyan_reg;
	best_conf = conf;
      }
      
      green_reg = green_reg->next;
    }

    cyan_reg = cyan_reg->next;
  }

  // Save our best confidence
  bullseye->confidence = best_conf;

  // Calculate the direction to the center of the green region
  vector3d bullseye_dir;
  bullseye_dir = getPixelDirection(best_green->cen_x, best_green->cen_y);
  
  // Find the point on the ground plane (0,0,0) with normal (0,0,1)
  // that the ray from the camera in bullseye_dir intersects. Store
  // it in intersect_pt.
  vector3d intersect_pt;
  if(intersect_ray_plane(camera_loc, bullseye_dir, 
			 vector3d(0, 0, 0),  vector3d(0, 0, 1), 
			 intersect_pt)) {

    // We're done - that point is the center of the bullseye.
    bullseye->loc = intersect_pt;
    
  } else {
    // We didn't intersect the ground plane. :....O(
    // Technically, we could use area here. But if you can't see it well,
    // you should just move closer using the tower when finding the
    // charging station.
    bullseye->confidence = 0.0;
    bullseye->loc = vector3d(0.0, 0.0, 0.0);
  }
  
  bullseye->distance = sqrt(intersect_pt.x*intersect_pt.x + intersect_pt.y*intersect_pt.y);
 
  if(debug_bullseye && print_now) {
    pprintf(TextOutputStream,"bullseye conf=%g position=(%g,%g,%g)\n",bullseye->confidence,V3COMP(bullseye->loc));
  }
  
  if(debug_sort) {
    if(bullseye->confidence > .5){
      DebugClass = 2;
      sprintf(DebugInfo,"(%g,%g,%g)",V3COMP(bullseye->loc));
    }
  }
}

void Vision::findSkateboardWheels(VObject *wheels)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findSkateboardWheels",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  for(int i=0; i<4; i++){
    wheels[i].confidence = 0.0;
  }

  static const bool debug_wheels = 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_wheels)
    frame_cnt = (frame_cnt + 1) % print_period;

  bool print_now = (frame_cnt == 0);

  color_class_state *orange;
  int n;
  region *or_reg; // cyan region

  orange=&color[COLOR_ORANGE];
  or_reg = orange->list;

  if(!or_reg)
    return;

  int k=0;
  for(n=0; or_reg && n<10 && k<4; or_reg = or_reg->next, n++) {
    vector3d wheels_dir;
    wheels_dir = getPixelDirection(or_reg->cen_x,or_reg->cen_y);
    wheels_dir = wheels_dir * 200.0;
    wheels_dir.z = 0.0;
    
    wheels[k].confidence = 1.0;
    wheels[k].loc = wheels_dir;
    
    if(debug_wheels && print_now) {
      pprintf(TextOutputStream,"wheels[%d] conf=%g position=(%g,%g,%g)\n",k,wheels[k].confidence,V3COMP(wheels[k].loc));
    }

    k++;
  }

  if(debug_sort) {
    if(wheels[0].confidence > .5)
      DebugInfo[0]='\0';
    for(int k=0; k<4; k++){
      if(wheels[k].confidence > .5){
        DebugClass = 3;
        sprintf(DebugInfo,"%s(%g,%g,%g) ",DebugInfo,V3COMP(wheels[k].loc));
      }
    }
  }
}

void Vision::findSkateboard(VObject *skateboard)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::findSkateboard",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  static const bool debug_skateboard = 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_skateboard)
    frame_cnt = (frame_cnt + 1) % print_period;

  bool print_now = (frame_cnt == 0);

  skateboard->confidence = 0.0;

  color_class_state *orange;
  region *or_reg; // orange region

  orange=&color[COLOR_ORANGE];
  or_reg = orange->list;

  if(!or_reg)
    return;

  vector3d skateboard_dir;
  skateboard_dir = getPixelDirection(or_reg->cen_x,or_reg->cen_y);
  skateboard_dir = skateboard_dir * 200.0;
  skateboard_dir.z = 0.0;

  skateboard->confidence = 1.0;
  skateboard->loc = skateboard_dir;
  skateboard->orientation = M_PI/2.0;

  if(debug_skateboard && print_now) {
    pprintf(TextOutputStream,"skateboard conf=%g position=(%g,%g,%g)\n",skateboard->confidence,V3COMP(skateboard->loc));
  }

  if(debug_sort) {
    if(skateboard->confidence > .5){
      DebugClass = 4;
      sprintf(DebugInfo,"(%g,%g,%g) %g",V3COMP(skateboard->loc),skateboard->orientation);
    }
  }
}

void Vision::findObjectInFrontOfIR(ObjectInfo *obj_info) {
  static const int window_size = 6;

  int window_x1,window_x2;
  int window_y1,window_y2;

  window_x1 =  width/2 - window_size/2;
  window_y1 = height/2 - window_size/2;
  window_x2 =  width/2 + window_size/2 - 1;
  window_y2 = height/2 + window_size/2 - 1;

  int run_idx;
  run *rle;

  run_idx=CMVision::FindStart<run>(rmap,0,num_runs-1,window_x1,window_y1);
  rle = &rmap[run_idx];

  int black_pixels;
  int y;
  int last_object = 0;

  black_pixels = window_size * window_size;
  y = rle->y;
  while(rle->y <= window_y2) {
    while(rle->x + rle->width < window_x1 && rle->y <= y) {
      rle++;
    }

    while(rle->x <= window_x2 && rle->y <= y) {
      region *this_reg;
      VObject *obj_to_use=NULL;

      this_reg = &reg[rle->parent];
      if(vobj_info[last_object].reg!=NULL &&
         (this_reg == vobj_info[last_object].reg ||
          (MARKER <= last_object && last_object < MARKER+6 && this_reg == vobj_info[last_object].reg2))) {
        obj_to_use = &((VObject *)obj_info)[last_object];
      } else {
        for(int obj_idx=0; obj_to_use==NULL && obj_idx<NUM_VISION_OBJECTS; obj_idx++) {
          if(vobj_info[obj_idx].reg==NULL)
            continue;
          if(this_reg == vobj_info[obj_idx].reg ||
             (MARKER <= obj_idx && obj_idx < MARKER+6 && this_reg == vobj_info[obj_idx].reg2)) {
            obj_to_use = &((VObject *)obj_info)[obj_idx];
            last_object = obj_idx;
          }
        }
      }

      int x1_add,x2_add;
      int add;
      
      x1_add = max(window_x1,(int)rle->x);
      x2_add = min(window_x2,(int)rle->x+rle->width-1);
      
      add = x2_add - x1_add + 1;
      black_pixels -= add;

      if(obj_to_use!=NULL) {
        obj_to_use->confInFrontOfIR += add;
      }

      rle++;
    }

    while(rle->y <= y)
      rle++;

    y = rle->y;
  }

  for(int obj_idx=0; obj_idx<NUM_VISION_OBJECTS; obj_idx++) {
    int denominator;

    denominator = window_size * window_size;
    if(ROBOT <= obj_idx && obj_idx < ROBOT+NUM_ROBOTS_PER_COLOR*2) {
      denominator -= black_pixels;
      denominator = max(denominator, 1);
    }
    ((VObject *)obj_info)[obj_idx].confInFrontOfIR /= (double)denominator;
  }
}

float Vision::findVisualServoingError()
{
  static const float not_found_error = -1.0;

#if 0
  color_class_state *orange;
  region *or_reg; // orange region

  // get information about orange regions
  orange=&color[COLOR_ORANGE];
  // get the head of the orange region list
  // this is also the largest region of orange
  // this is NULL if there are no orange regions
  or_reg=orange->list;

  if(!or_reg)
    return not_found_error;

  // make sure the orange region isn't really small or narrow
  // this ensures that the robot doesn't react to small amounts of noise
  //   in the segmentation

  int w,h;

  w = or_reg->x2 - or_reg->x1 + 1;
  h = or_reg->y2 - or_reg->y1 + 1;

  if(w<3 || h<3)
    return not_found_error;

  // found an orange region, calculate x coordinate of the centroid

  // the x coordinate of the centroid, during the calculation this is the sum
  //   of the x coordinates of every pixel in the region
  float cen_x;
  // the number of pixels summed up so far in cen_x
  int pixel_cnt;
  // the index number of the next run we should process
  int cur_idx;

  // initialize things to zero
  cen_x = 0;
  pixel_cnt = 0;
  // get the index of the first run that is a part of the region
  // this will be the leftmost of the uppermost runs in the region
  cur_idx = or_reg->run_start;
  do {
    run *cur_run;

    // get the actual data structure for this run
    cur_run = &rmap[cur_idx];
    // use the fields of the run to update the centroid calculation
    cen_x += ((2*cur_run->x + cur_run->width-1) / 2.0) * cur_run->width;
    pixel_cnt += cur_run->width;

    // get the index of the next run in the region
    cur_idx = cur_run->next;
  } while(cur_idx!=0); // keep going until out of runs in the region
  // convert the sum in cen_x into an average
  cen_x /= pixel_cnt;

  // error on the image is distance from center of image
  float img_error;
  img_error = cen_x - (width-1)/2.0;

  // returned error is in [-1.0,1.0]
  // convert error to be more similar to coordinate frame of robot
  // positive means object is too far left
  // normalize error to be within desired range
  return -img_error/(width/2.0);
#else
  color_class_state *green;
  region *green_reg; // green region

  green=&color[COLOR_GREEN];
  green_reg=green->list;

  if(!green_reg)
    return not_found_error;

  int w,h;

  w = green_reg->x2 - green_reg->x1 + 1;
  h = green_reg->y2 - green_reg->y1 + 1;

  if(w<3 || h<3)
    return not_found_error;


  float img_error;
  int run_cnt;
  int cur_idx;
  int cur_y;

  img_error = 0;
  run_cnt = 0;
  cur_idx = green_reg->run_start;
  cur_y = -1;
  do {
    run *cur_run;

    cur_run = &rmap[cur_idx];
    if(cur_run->y != cur_y){
      // new line
      img_error += cur_run->x - (width-1)/2.0;
      run_cnt++;
      cur_y = cur_run->y;
    }

    cur_idx = cur_run->next;
  } while(cur_idx!=0);
  img_error /= run_cnt;

  // returned error is in [-1.0,1.0]
  // positive means object is too far left
  return -img_error/(width/2.0);
#endif
}

void Vision::calcImgVectors() {
  vector3d img_ego_pz_3d;
  vector3d pz_3d(0.0,0.0,1.0);

  img_ego_pz_3d.set(pz_3d.dot(camera_right),
                    pz_3d.dot(camera_up   ),
                    pz_3d.dot(camera_dir  ));
    
  img_up.set(img_ego_pz_3d.x,-img_ego_pz_3d.y);
  img_up.normalize();

  img_hor.set(-img_up.y,img_up.x);

#ifdef PLATFORM_LINUX
#ifndef VIS_NO_LIBS
  {
    if(DrawUpVector) {
      vector2d s_r,e_r;
      GVector::vector2d<int> s,e;
      s_r.set(width/2.0-.5,height/2.0-.5);
      e_r.set(width/2.0-.5,height/2.0-.5);
      e_r +=  img_up*width; 
      s.set((int)(s_r.x+.5),(int)(s_r.y+.5));
      e.set((int)(e_r.x+.5),(int)(e_r.y+.5));
      rgb c;
      c.set(128,128,255);
      if(DebugImage) DebugImage->draw_line(s.x,s.y,e.x,e.y,c);
    }
 }
#endif
#endif

  //pprintf(TextOutputStream,"cam: dir=(%10g,%10g,%10g) up=(%10g,%10g,%10g) right=(%10g,%10g,%10g)\n",
  //        camera_dir.x,  camera_dir.y,  camera_dir.z,
  //        camera_up.x,   camera_up.y,   camera_up.z,
  //        camera_right.x,camera_right.y,camera_right.z
  //        );
  //
  //pprintf(TextOutputStream,"img: pz=(%10g,%10g,%10g)\n",
  //        img_ego_pz_3d.x, img_ego_pz_3d.y, img_ego_pz_3d.z
  //        );
  //
  //pprintf(TextOutputStream,"img_2d: up=(%10g,%10g) hor=(%10g,%10g)\n",
  //        img_up.x,  img_up.y,
  //        img_hor.x, img_hor.y
  //        );
}

template<class image>
bool Vision::runHighLevelVision(ObjectInfo *obj_info,image &img) {
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::runHighLevelVision",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  //pprintf(TextOutputStream,"obj info is %p\n",obj_info);

  DebugClass = 0;
  DebugInfo[0] = '\0';

  kin.getHeadPosition(camera_loc,camera_dir,camera_up,camera_right,
			      head_angles,body_angle,body_height);
  calcImgVectors();

  for(int obj_idx=0; obj_idx<NUM_OBJECTINFO_OBJECTS; obj_idx++) {
    memset(&(((VObject *)obj_info)[obj_idx]),0,sizeof(VObject));
    ((VObject *)obj_info)[obj_idx].confInFrontOfIR = 0.0;
    vobj_info[obj_idx].reg  = NULL;
    vobj_info[obj_idx].reg2 = NULL;
  }

  //pprintf(TextOutputStream,"camera loc (%g,%g,%g) dir (%g,%g,%g) up (%g,%g,%g) right (%g,%g,%g)\n",
  //        camera_loc.x  ,camera_loc.y  ,camera_loc.z  ,
  //        camera_dir.x  ,camera_dir.y  ,camera_dir.z  ,
  //        camera_up.x   ,camera_up.y   ,camera_up.z   ,
  //        camera_right.x,camera_right.y,camera_right.z);
  //
  //pprintf(TextOutputStream,"head tilt %g pan %g roll %g, body angle %g height %g\n",
  //        head_angles[0],head_angles[1],head_angles[2],
  //        body_angle,body_height);

  findBall(&obj_info->ball,&vobj_info[BALL],img);
  
  findMarkers(obj_info->marker,&vobj_info[MARKER]);

  findGoal(COLOR_YELLOW,&obj_info->yellow_goal,&obj_info->yellow_goal_edges[0],&vobj_info[YELLOW_GOAL]);
  findGoal(COLOR_CYAN  ,&obj_info->cyan_goal  ,&obj_info->cyan_goal_edges  [0],&vobj_info[CYAN_GOAL  ]);

  findRobots(COLOR_RED ,obj_info->red_robots ,&vobj_info[RED_ROBOT ],3);
  findRobots(COLOR_BLUE,obj_info->blue_robots,&vobj_info[BLUE_ROBOT],3);

  findChargingTower   (&obj_info->charging_tower);
  findChargingBullseye(&obj_info->charging_bullseye);
  findSkateboardWheels(&obj_info->skateboard_wheels[0]);
  findSkateboard      (&obj_info->skateboard);

  findCornerGoalieBox(&obj_info->stripe_corner);

  findObjectInFrontOfIR(obj_info);

#ifdef PLATFORM_LINUX
  float servo_error;
  servo_error=findVisualServoingError();
  //pprintf(TextOutputStream,"servo_error=%g\n",servo_error);
#endif

  createRadialMap();

#ifdef PLATFORM_APERIOS
  // output Vision objects if desired
  static int count=0;
  if(config.spoutConfig.dumpVisionObj!=0) {
    count = (count + 1) % config.spoutConfig.dumpVisionObj;
    if(count==0) {
      if(encodeBuf!=NULL && 
         encoder  !=NULL && 
         visionObjStream!=NULL) {
        int out_size=0;
        out_size = encoder->encodeVisionObjs(encodeBuf,obj_info);
        visionObjStream->writeBinary(encodeBuf,out_size);
      }
    }
  }
#endif

  //if(obj_info->ball.confidence > .4){
  //  sendRawImage();
  //}

  return true;
}

#ifdef PLATFORM_LINUX
bool Vision::thresholdImage(CMVision::image_classified &img) {
  memcpy(cmap,img.buf,width*height);

  return true;
}

bool Vision::thresholdImage(CMVision::image_idx<rgb> &img) {
  static rgb rgb_colors[MAX_COLORS];
  static bool initted=false;

  if(!initted) {
    for(int color_idx=0; color_idx<MAX_COLORS; color_idx++){
      //pprintf(TextOutputStream,"color_idx[%d]=(%d,%d,%d)\n",
      //        color_idx,color[color_idx].color.red,color[color_idx].color.green,color[color_idx].color.blue);
      rgb_colors[color_idx]=color[color_idx].color;
    }
    initted=true;
  }

  CMVision::RgbToIndex(cmap,img.buf,width,height,rgb_colors,MAX_COLORS);

  return true;
}

bool Vision::thresholdImage(CMVision::image<rgb> &img) {
  CMVision::ThresholdImageRGB24<cmap_t,CMVision::image<rgb>,bits_y,bits_u,bits_v>(cmap,img,tmap[cur_tmap]);

  return true;
}
#endif

bool Vision::thresholdImage(CMVision::image_yuv<uchar> &img) {
  // run instantiated template
  CMVision::FixImageYUVPlanar<CMVision::image_yuv<uchar>,uchar>(img);
  CMVision::ThresholdImageYUVPlanar<cmap_t,CMVision::image_yuv<uchar>,uchar,bits_y,bits_u,bits_v>(cmap,img,tmap[cur_tmap]);
  //CMVision::ThresholdImageYUVPlanarWithAmbiguous<cmap_t,CMVision::image_yuv<const uchar>,const uchar,bits_y,bits_u,bits_v>(cmap,img,tmap[cur_tmap]);

  return true;
}

bool Vision::thresholdAndCorrectImage(CMVision::image_yuv<uchar> &img) {
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::thresholdAndCorrectImage",
				    SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  CMVision::ThresholdAndCorrectImageYUVPlanar<cmap_t,CMVision::image_yuv<uchar>,uchar,bits_y,bits_u,bits_v>(cmap,img,correction_mask,tmap[cur_tmap]);
  
  return true;
}

//#define CHECK_RUNS

//#define CHECK_REGIONS

template<class image>
bool Vision::runLowLevelVision(image &img)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Vision::runLowLevelVision",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  static int frame_cnt=-1;
  frame_cnt++;

#ifdef ENABLE_JOIN_NEARBY
  int num_runs_old;
#endif
  int max_area;

  width  = img.width;
  height = img.height;

#ifdef PLATFORM_APERIOS
  if(ShouldThresholdImage) {
    if(use_correction_mask) 
      thresholdAndCorrectImage(img);
    else
      thresholdImage(img);
  }
  
#else
  // We don't support correcting for lense distortion under
  // Linux at the moment.
  if(ShouldThresholdImage) 
    thresholdImage(img);
#endif
  num_runs = CMVision::EncodeRuns(rmap,cmap,img.width,img.height,max_runs);

  //cout << "NR" << num_runs << endl;

#ifdef ENABLE_JOIN_NEARBY
  num_runs_old = num_runs;
#endif

#if 0
  {
    bool ok=CMVision::CheckRuns("R1",rmap,num_runs,width,height,num_colors);
    if(!ok) {
      pprintf(TextOutputStream,"error in run data, consistency check failed\n");
    }
  }
#endif

  CMVision::ConnectComponents(rmap,num_runs);

#if 0
  {
    bool ok=CMVision::CheckRuns("R2",rmap,num_runs,width,height,num_colors);
    if(!ok) {
      pprintf(TextOutputStream,"error in run data, consistency check failed\n");
    }
  }
#endif

  //cout << "Nr" << num_runs << endl;

  num_regions = CMVision::ExtractRegions(reg,max_regions,rmap,num_runs);

#if 0
  {
    bool ok=CMVision::CheckRuns("R3",rmap,num_runs,width,height,num_colors);
    if(!ok) {
      pprintf(TextOutputStream,"error in run data, consistency check failed\n");
    }
  }
#endif

  //CMVision::CheckRuns("R4",rmap,num_runs,width,height,num_colors);

  //pprintf(TextOutputStream,"runs:%6d (%6d) regions:%6d (%6d)\n",
  //       num_runs,max_runs,
  //      num_regions,max_regions);

  //cout << "rN " << num_runs  << "reg " << num_regions << endl;
  //CMVision::CheckRegionColors(color,num_colors,reg,num_regions);

  max_area = CMVision::SeparateRegions(color,num_colors,reg,num_regions);
  CMVision::SortRegions(color,num_colors,max_area);

  CMVision::MergeRegions(color,num_colors,rmap);

  for(int i=0; i<num_colors; i++)
    color[i].total_area = -1;

  if(calcTotalArea)
    CMVision::CalcTotalArea(color,num_colors);

#ifdef CHECK_REGIONS
  {
    bool ok=CMVision::CheckRegions(color,num_colors,rmap);
    if(!ok) {
      pprintf(TextOutputStream,"error, CheckRegions failed\n");
    }
  }
#endif

  // CMVision::CreateRunIndex(yindex,rmap,num_runs);

#ifdef PLATFORM_APERIOS
  // output avg color if desired
  if(config.spoutConfig.dumpVisionAvgColor!=0) {
    outCountAvgColor = (outCountAvgColor + 1) % config.spoutConfig.dumpVisionAvgColor;
    if(outCountAvgColor==0) {
      sendAvgColor();
    }
  }
#endif

#ifdef PLATFORM_APERIOS
  // output color area if desired
  if(config.spoutConfig.dumpVisionColorArea!=0) {
    outCountColorArea = (outCountColorArea + 1) % config.spoutConfig.dumpVisionColorArea;
    if(outCountColorArea==0) {
      sendColorArea();
    }
  }
#endif

#ifdef PLATFORM_APERIOS
  // output color area if desired
  if(config.spoutConfig.dumpVisionRadialMap!=0) {
    outCountRadialMap = (outCountRadialMap + 1) % config.spoutConfig.dumpVisionRadialMap;
    if(outCountRadialMap==0) {
      sendRadialMap();
    }
  }
#endif

#ifdef PLATFORM_APERIOS
  // output raw frames if desired
  if(config.spoutConfig.dumpVisionRaw!=0) {
    outCountRaw = (outCountRaw + 1) % config.spoutConfig.dumpVisionRaw;
    if(outCountRaw==0) {
      sendRawImage();
    }
  }
#endif

#ifdef PLATFORM_APERIOS
  // output RLE if desired
  if(config.spoutConfig.dumpVisionRLE!=0) {
    outCountRLE = (outCountRLE + 1) % config.spoutConfig.dumpVisionRLE;
    if(outCountRLE==0) {
      sendRLEImage();
    }
  }
#endif

  return(true);
}

bool Vision::processFrame(ObjectInfo *obj_info,uchar *data_y,uchar *data_u,uchar *data_v, int width, int height, int skip) {
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("vision",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  frameTimestamp = GetTime();

  img.buf_y=data_y;
  img.buf_u=data_u;
  img.buf_v=data_v;

  img.width=width;
  img.height=height;
  img.row_stride= skip + width;

  bool ok=true;

  //cout << "Low" << endl;
  if(ok) ok=runLowLevelVision(img);
  //cout << "Params" << endl;
  if(ok) ok=getBodyParameters();
  //cout << "High" << endl;
  if(ok) ok=runHighLevelVision(obj_info,img);
  //cout << "Done" << endl;

  // HACK for testing variable lighting code
  if(score_images)
    scoreImage();
  // /HACK


  //if(obj_info->red_robots[0].confidence > 0.5)
  //  sendRLEImage();

  return ok;
}

#ifdef PLATFORM_LINUX
bool Vision::processFrame(ObjectInfo *obj_info, CMVision::image_classified *img, int width, int height) {

  frameTimestamp = GetTime();

  bool ok=true;

  if(ok) ok=runLowLevelVision(*img);
  if(ok) ok=runHighLevelVision(obj_info,*img);

  return ok;
}

bool Vision::processFrame(ObjectInfo *obj_info, CMVision::image_idx<rgb> *img, int width, int height) {

  frameTimestamp = GetTime();

  bool ok=true;

  if(ok) ok=runLowLevelVision(*img);
  if(ok) ok=runHighLevelVision(obj_info,*img);

  return ok;
}

bool Vision::processFrame(ObjectInfo *obj_info, CMVision::image<rgb> *img, int width, int height) {

  frameTimestamp = GetTime();

  bool ok=true;

  if(ok) ok=runLowLevelVision(*img);
  if(ok) ok=runHighLevelVision(obj_info,*img);

  return ok;
}
#endif

int WritePPM(char *filename,rgb *img,int width,int height)
{
  FILE *out;
  int wrote;

  out = fopen(filename,"wb");
  if(!out) return(0);

  fprintf(out,"P6\n%d %d\n%d\n",width,height,255);
  wrote = fwrite(img,sizeof(rgb),width*height,out);
  fclose(out);

  return(wrote);
}

int Vision::setThreshold(int threshold_id) {
  if(threshold_id < 0 || threshold_id >= num_tmaps) {
    return -1;
  } else {
    int old_threshold;
    old_threshold = cur_tmap;
    cur_tmap = threshold_id;
    return old_threshold;
  }
}

void Vision::scoreImage() {

#if 0
  pprintf(TextOutputStream, "--------------------\nCurrent tmap: %d\n",
	  cur_tmap);
  pprintf(TextOutputStream, "BACKGROUND: regions %d area %d\n\
ORANGE: regions %d area %d\n\
GREEN: regions %d area %d\n\
PINK: regions %d area %d\n\
CYAN: regions %d area %d\n\
YELLOW: regions %d area %d\n\
BLUE: regions %d area %d\n\
RED: regions %d area %d\n\
WHITE: regions %d area %d\n",
	  color[COLOR_BACKGROUND].num, color[COLOR_BACKGROUND].total_area,
	  color[COLOR_ORANGE].num, color[COLOR_ORANGE].total_area,
	  color[COLOR_GREEN].num, color[COLOR_GREEN].total_area,
	  color[COLOR_PINK].num, color[COLOR_PINK].total_area,
	  color[COLOR_CYAN].num, color[COLOR_CYAN].total_area,
	  color[COLOR_YELLOW].num, color[COLOR_YELLOW].total_area,
	  color[COLOR_BLUE].num, color[COLOR_BLUE].total_area,
	  color[COLOR_RED].num, color[COLOR_RED].total_area,
	  color[COLOR_WHITE].num, color[COLOR_WHITE].total_area);

  pprintf(TextOutputStream,
	  "ball conf: %lf\ncop: %lf\npoc %lf\nyop %lf\npoy %lf\n",
	  object_info->ball.confidence,
	  object_info->marker[0].confidence,
	  object_info->marker[1].confidence,
	  object_info->marker[4].confidence,
	  object_info->marker[5].confidence);
#endif

  double score = 0;
  
  if(color[COLOR_ORANGE].num > 10)
    score -= 0.1;
  if(color[COLOR_ORANGE].num > 25)
    score -= .2;
  if(color[COLOR_ORANGE].num > 50)
    score -= .2;
  if(color[COLOR_ORANGE].num > 100)
    score -= 0.5;
  if(color[COLOR_GREEN].num > 20)
    score -= 0.1;
  if(color[COLOR_YELLOW].num > 20)
    score -= 0.1;
  if(color[COLOR_YELLOW].num > 100)
    score -= 0.2;
  if(color[COLOR_PINK].num > 20)
    score -= 0.1;
  if(color[COLOR_PINK].num > 100)
    score -= 0.2;
  if(color[COLOR_CYAN].num > 20)
    score -= 0.1;

  score += 0.1*object_info->ball.confidence;
  score += 2.0*object_info->marker[0].confidence;
  score += 2.0*object_info->marker[1].confidence;
  score += 2.0*object_info->marker[4].confidence;
  score += 2.0*object_info->marker[5].confidence;
  
  tmap_scores[cur_tmap] = tmap_score_alpha*tmap_scores[cur_tmap] +
    (1.0 - tmap_score_alpha)*score;

  cur_tmap = (cur_tmap + 1) % num_tmaps;

  // We're messing with our tmaps; we'd really rather not
  // see a bunch of spurious balls and mess up our world
  // model. Markers, on the other hand, tend not to show up
  // spuriously so we'll use this time to localize.
  object_info->ball.confidence = 0.0;
}

void Vision::startThreshTest() {
  cur_tmap = 0;
  score_images = true;

  for(int i=0; i<MAX_TMAPS; i++)
    tmap_scores[i] = 0.0;
}

double Vision::endThreshTest() {
  score_images = false;

  int max_index = 0;
  for(int i=1; i<num_tmaps; i++)
    if(tmap_scores[i] > tmap_scores[max_index])
      max_index = i;

  cur_tmap = max_index;

  pprintf(TextOutputStream, "Thresh test over: %d with %lf\n",
	  max_index, tmap_scores[max_index]);
  for(int i=0; i<num_tmaps; i++)
    pprintf(TextOutputStream, "tmap %d with %lf\n", i,
	    tmap_scores[i]);

  return tmap_scores[max_index];
}

bool Vision::saveThresholdImage(char *filename)
{
  rgb *buf;
  int wrote;

  buf = new rgb[width * height];
  if(!buf) return(false);

  CMVision::IndexToRgb<cmap_t,color_class_state>(buf,cmap,width,height,color,num_colors);
  wrote = WritePPM(filename,buf,width,height);
  delete(buf);

  return(wrote > 0);
}

void Vision::sendRawImage() {
#ifdef PLATFORM_APERIOS
  if(encodeBuf!=NULL && 
     encoder  !=NULL && 
     visionRawStream!=NULL) {
    int out_size=0;
    if(!use_correction_mask) {
      out_size=encoder->encodeVisionRaw(encodeBuf,
					body_angle,body_height,
					head_angles[0],head_angles[1],head_angles[2],
					img);
    } else {
      out_size=encoder->encodeVisionRaw(encodeBuf,
					body_angle,body_height,
					head_angles[0],head_angles[1],head_angles[2],
					img, correction_mask);
    }
    if(!visionRawStream->writeBinary(encodeBuf,out_size,frameTimestamp))
      pprintf(TextOutputStream,"problem writing raw image\n");
  } else {
    if(encoder==NULL)
      pprintf(TextOutputStream,"NVEnc\n");
    else if(encodeBuf==NULL)
      pprintf(TextOutputStream,"NVEncBuf\n");
    else
      pprintf(TextOutputStream,"NVRawStream\n");
  }
#endif

  outCountRaw = 0;
}

void Vision::sendRLEImage() {
#ifdef PLATFORM_APERIOS
  if(encodeBuf!=NULL && 
     encoder  !=NULL && 
     visionRLEStream!=NULL) {
    int out_size=0;
#ifdef ENABLE_JOIN_NEARBY
    out_size=encoder->encodeVisionRLE(encodeBuf,
                                      body_angle,body_height,
                                      head_angles[0],head_angles[1],head_angles[2],
                                      width,height,
                                      num_runs-2,rmap+1);
#else
    out_size=encoder->encodeVisionRLE(encodeBuf,
                                      body_angle,body_height,
                                      head_angles[0],head_angles[1],head_angles[2],
                                      width,height,
                                      num_runs,rmap);
#endif
    visionRLEStream->writeBinary(encodeBuf,out_size,frameTimestamp);
  } else {
    if(encoder==NULL)
      pprintf(TextOutputStream,"NVEnc\n");
    else if(encodeBuf==NULL)
      pprintf(TextOutputStream,"NVEncBuf\n");
    else
      pprintf(TextOutputStream,"NVRLEStream\n");
  }
#endif

  outCountRLE = 0;
}

void Vision::sendAvgColor() {
#ifdef PLATFORM_APERIOS
  if(encodeBuf!=NULL && 
     visionAvgColorStream!=NULL) {
    int out_size=0;
    uchar *buf;
    buf = encodeBuf;
    SPOutEncoder::encodeAs<float>(&buf,(float)AvgImgColor[0]);
    SPOutEncoder::encodeAs<float>(&buf,(float)AvgImgColor[1]);
    SPOutEncoder::encodeAs<float>(&buf,(float)AvgImgColor[2]);
    out_size = buf - encodeBuf;
    visionAvgColorStream->writeBinary(encodeBuf,out_size,frameTimestamp);
  } else {
    if(encodeBuf==NULL)
      pprintf(TextOutputStream,"NVEncBuf\n");
    else
      pprintf(TextOutputStream,"NVAvgColorStream\n");
  }
#endif

  outCountAvgColor = 0;
}

void Vision::sendColorArea() {
#ifdef PLATFORM_APERIOS
  if(encodeBuf!=NULL && 
     visionColorAreaStream!=NULL) {
    int out_size=0;
    uchar *buf;
    buf = encodeBuf;
    SPOutEncoder::encodeAs<uchar>(&buf,(uchar)num_colors);
    for(int color_idx=0; color_idx<num_colors; color_idx++) {
      SPOutEncoder::encodeAs<ulong>(&buf,(ulong)color[color_idx].total_area);
    }
    out_size = buf - encodeBuf;
    visionColorAreaStream->writeBinary(encodeBuf,out_size,frameTimestamp);
  } else {
    if(encodeBuf==NULL)
      pprintf(TextOutputStream,"NVEncBuf\n");
    else
      pprintf(TextOutputStream,"NVColorAreaStream\n");
  }
#endif

  outCountAvgColor = 0;
}

void Vision::sendRadialMap() {
#ifdef PLATFORM_APERIOS
  if(encodeBuf!=NULL && 
     encoder  !=NULL && 
     visionRadialMapStream!=NULL) {
    int out_size=0;
    out_size=encoder->encodeVisionRadialMap(encodeBuf,&radial_map);
    visionRadialMapStream->writeBinary(encodeBuf,out_size,frameTimestamp);
  } else {
    if(encoder==NULL)
      pprintf(TextOutputStream,"NVEnc\n");
    else if(encodeBuf==NULL)
      pprintf(TextOutputStream,"NVEncBuf\n");
    else
      pprintf(TextOutputStream,"NVRadialMapStream\n");
  }
#endif

  outCountRadialMap = 0;
}

REGISTER_EVENT_PROCESSOR(Vision,Vision::name,Vision::create);

namespace VisionInterface {
  /*
  HeadVelocity * GetHeadVels(Vision *vision){
    return vision->hv;
  }
  */
  
  void SendRawImage(Vision *vision) {
    vision->sendRawImage();
  }

  void SendRLEImage(Vision *vision) {
    vision->sendRLEImage();
  }

  int SetThreshold(Vision *vision,int threshold_id) {
    return vision->setThreshold(threshold_id);
  }
}






/* corner detection */


const int cd_line_step = 10;
const int cd_line_step_margin = 25;  // should be about 2.5*cd_line_step?

#ifdef PLATFORM_LINUX
#define cd_debug 0
#else
#define cd_debug 0
#endif

struct FieldLine
{
private:
  std::vector<vector2d> pts;
  vector2d line_normal, endpt1, endpt2;
  double orig_offset;
  bool line_fit;

  void fitLine()
  {
    if(line_fit)
      return;

    if(pts.size() < 2U)
      return;

    // parameters for a polar line fit
    // x*sin(theta) - y*cos(theta) + ro = 0
    double ro,cos_theta,sin_theta;

    double x_mean,y_mean;
    double a,b,c,h;

    x_mean = 0.0;
    y_mean = 0.0;
    for(uint pts_idx=0U; pts_idx<pts.size(); pts_idx++){
      x_mean += pts[pts_idx].x;
      y_mean += pts[pts_idx].y;
    }
    x_mean /= pts.size();
    y_mean /= pts.size();

    a = 0.0;
    for(uint pts_idx=0U; pts_idx<pts.size(); pts_idx++){
      a += (pts[pts_idx].x - x_mean) * (pts[pts_idx].y - y_mean);
    }
    a *= 2.0;

    b = 0.0;
    for(uint pts_idx=0U; pts_idx<pts.size(); pts_idx++){
      b += sq(pts[pts_idx].x - x_mean);
      b -= sq(pts[pts_idx].y - y_mean);
    }

    c = hypot(a,b);
    h = hypot(a,b+c);

    if(h!=0.0){
      sin_theta =  a   /h;
      cos_theta = (b+c)/h;
    }else{
      sin_theta = 1.0;
      cos_theta = 0.0;
    }

    ro = y_mean * cos_theta - x_mean * sin_theta;

    line_normal.set(sin_theta,-cos_theta);
    orig_offset = -ro;

    line_fit = true;
  }

public:
  void init()
  {
    pts.clear();
    line_fit = false;
    endpt1.set(1000,1000);
    endpt2.set(-1,-1);
  }
  void init(vector2d pt)
  {
    pts.clear();
    pts.push_back(pt);
    line_fit = false;
    endpt1 = pt;
    endpt2 = pt;
  }
  bool ptNearLine(vector2d &pt);

  void appendPtToLine(vector2d &pt);

  vector2d computeIntersect(FieldLine &l2);

  int getCount()
  {
    return (int)pts.size();
  }

  vector2d endPt1()
  {
    return endpt1;
  }

  vector2d endPt2()
  {
    return endpt2;
  }

  void printInfo()
  {
#ifdef PLATFORM_LINUX
    switch(pts.size()){
      case 0:
        printf("[(%d)] ",(int)pts.size());
        break;
      case 1:
        printf("[(%d) (%d,%d)] ",(int)pts.size(),(int)pts[0].x,(int)pts[0].y);
        break;
      default:
        {
          printf("[(%d) : ",(int)pts.size());
          for(uint i=0; i<pts.size(); i++)
            printf("(%d,%d), ",(int)pts[i].x, (int)pts[i].y);
          printf("]\n");
        }
        break;
    }
#endif
  }

#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
  void drawSeg(rgb c)
  {
    if(DebugImage) 
      if(pts.size() > 2U)
        DebugImage->draw_line((int)endPt1().x,(int)endPt1().y, (int)endPt2().x,(int)endPt2().y, c);
  }

  void drawLine(rgb c,rgb c2)
  {
    fitLine();

    if(pts.size() > 2U){
      vector2d x1,x2;
      x1 = line_normal * orig_offset;
      x2 = x1 + line_normal.perp()*300;
      x1 -= line_normal.perp()*300;

      if(DebugImage) DebugImage->draw_line((int)x1.x,(int)x1.y, (int)x2.x,(int)x2.y, c);

      //pprintf(TextOutputStream,"drawing line:pts=%u fit=%d norm=(%g,%g) off=%g from pt1=(%g,%g) to pt2=(%g,%g)\n",
      //        pts.size(),line_fit?1:0,V2COMP(line_normal),orig_offset,V2COMP(x1),V2COMP(x2));
    }
    for(uint i=0; i<pts.size(); i++)
      if(DebugImage) DebugImage->fill_rect((int)pts[i].x-1,(int)pts[i].y-1, 3,3, c2);
  }
#endif
};

bool FieldLine::ptNearLine(vector2d &pt)
{
  const int base_y_thresh = 3;
  const int var_y_thresh = 5;

  if(pts.size() > 1U){
    fitLine();

    // established line with more than one pt 
    return fabs(line_normal.dot(pt) - orig_offset) < (double)(max(var_y_thresh - (int)pts.size(),0)+base_y_thresh);
  }else{
    // only 1 pt on the line
    return (abs((int)(pt.x - pts[0].x)) <= cd_line_step_margin && 
            abs((int)(pt.y - pts[0].y)) <= cd_line_step_margin);
  }
}

void FieldLine::appendPtToLine(vector2d &pt)
{
  pts.push_back(pt);
  line_fit = false;
  if(pt.x < endpt1.x || (pt.x == endpt1.x && pt.y < endpt1.y)){
    endpt1 = pt;
  }else if(pt.x > endpt2.x || (pt.x == endpt2.x && pt.y > endpt2.y)){
    endpt2 = pt;
  }
}

vector2d FieldLine::computeIntersect(FieldLine &l2)
{
  vector2d ret(-1.0,-1.0);

  fitLine();
  l2.fitLine();

  //pprintf(TextOutputStream,"computing intersection of l1:pts=%u fit=%d norm=(%g,%g) off=%g and l2:pts=%u fit=%d norm=(%g,%g) off=%g\n",
  //        pts.size(),line_fit?1:0,V2COMP(line_normal),orig_offset,
  //        l2.pts.size(),l2.line_fit?1:0,V2COMP(l2.line_normal),l2.orig_offset);

  intersect(ret,line_normal,orig_offset,l2.line_normal,l2.orig_offset);

  return ret;
}

struct ScanRLEParams {
  Vision *vis;
  int runIdx;
  int curColor;
  ScanRun *curRun;
  ScanRun *runs;
  int maxRuns;

  void reset()
  {
    runIdx = -1;
    curColor = -1;
    curRun = NULL;    
  }

  void init(Vision *_vis,ScanRun *_runs,int max_runs)
  {
    vis = _vis;
    runs = _runs;
    maxRuns = max_runs;
    reset();
  }
};

struct ScanRLECallback {
  bool operator()(int cur_x,int cur_y,ScanRLEParams *params) {
    int color;

    color = params->vis->getColorUnsafe(cur_x,cur_y);
    if(color == params->curColor) {
      params->curRun->endPt.set(cur_x,cur_y);
      params->curRun->length++;
    } else {
      if(params->runIdx+1 >= params->maxRuns)
        return false;
      params->runIdx++;
      params->curColor = color;
      params->curRun = &params->runs[params->runIdx];
      params->curRun->startPt.set(cur_x,cur_y);
      params->curRun->endPt  .set(cur_x,cur_y);
      params->curRun->color = color;
      params->curRun->length = 1;
    }

    return true;
  }
};

int Vision::detectTransitionsScan(vector2d *line_pts,int max_line_pts,ScanRun *runs,int num_runs)
{
  int num_line_pts = 0;

  static const int MaxBigGreen=10;
  // stores run indices where large green regions start
  int big_green_starts[MaxBigGreen];
  // stores run indices where large green regions end
  int big_green_ends[MaxBigGreen];

  int num_big_green;
  bool green_extendable;
  int green_cnt; // number of green pixels in current last green region
  // counts in possible extension
  int ext_green_cnt,ext_other_cnt;

  num_big_green = 0;
  green_extendable = false;
  green_cnt = 0;
  ext_green_cnt = 0;
  ext_other_cnt = 0;
  for(int run_idx=0; run_idx<num_runs; run_idx++){
    ScanRun *cur_run;

    cur_run = &runs[run_idx];
    //pprintf(TextOutputStream,"run_idx=%2d cur_run:color=%d length=%2d (%3d,%3d)-(%3d,%3d)\n",
    //        run_idx,cur_run->color,cur_run->length,V2COMP(cur_run->startPt),V2COMP(cur_run->endPt));
    //pprintf(TextOutputStream,"extendable=%d green_cnt=%d ext_green_cnt=%d ext_other_cnt=%d num_big_green=%d\n",
    //        green_extendable,green_cnt,ext_green_cnt,ext_other_cnt,num_big_green);
    switch(cur_run->color){
      case COLOR_GREEN:
        if(green_extendable){
          ext_green_cnt += cur_run->length;
          if(ext_green_cnt >= ext_other_cnt){
            // extend current big green region
            big_green_ends[num_big_green-1] = run_idx;
            green_cnt += ext_green_cnt;
            // reset extension counts
            ext_green_cnt = 0;
            ext_other_cnt = 0;
          }
        }else{
          // if last green region had less than 3 green pixels, ditch it
          if(num_big_green > 0 && green_cnt < 3){
            num_big_green--;
          }
          // start new green region
          if(num_big_green < MaxBigGreen){
            big_green_starts[num_big_green] = run_idx;
            big_green_ends  [num_big_green] = run_idx;
            num_big_green++;
            green_extendable = true;
            green_cnt = cur_run->length;
            // reset extension counts
            ext_green_cnt = 0;
            ext_other_cnt = 0;
          }
        }
        break;
      case COLOR_RED:
      case COLOR_ORANGE:
      case COLOR_PINK:
      case COLOR_WHITE:
        green_extendable = false;
        break;
      default:
        if(green_extendable){
          ext_other_cnt += cur_run->length;
          green_extendable = (ext_other_cnt - ext_green_cnt < 4);
        }
        break;
    }
  }
  // if last green region had less than 3 green pixels, ditch it
  if(green_cnt < 3){
    num_big_green--;
  }

//#if cd_debug
//#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
//  for(int i=0; i<num_big_green; i++){
//    vector2i start,end;
//    rgb c;
//    c.set(255,0,0);
//    start = runs[big_green_starts[i]].startPt;
//    end   = runs[big_green_ends  [i]].endPt  ;
//    if(DebugImage) DebugImage->draw_line(V2COMP(start),V2COMP(end),c);
//    pprintf(TextOutputStream,"green %d from (%3d,%3d) to (%3d,%3d)\n",i,V2COMP(start),V2COMP(end));
//  }
//#endif
//#endif

  // look for white in between the big green regions
  for(int big_green_idx=0; big_green_idx<num_big_green-1; big_green_idx++){
    int green1_end_idx;
    int green2_start_idx;
    
    green1_end_idx   = big_green_ends  [big_green_idx  ];
    green2_start_idx = big_green_starts[big_green_idx+1];

    int white_cnt;
    int other_cnt;
    bool ok;
    white_cnt = 0;
    other_cnt = 0;
    ok = true;
    for(int run_idx=green1_end_idx+1; run_idx<green2_start_idx; run_idx++){
      ScanRun *cur_run;
      
      cur_run = &runs[run_idx];
      switch(cur_run->color){
        case COLOR_WHITE:
          white_cnt += cur_run->length;
          break;
        case COLOR_CYAN:
        case COLOR_YELLOW:
        case COLOR_BACKGROUND:
        case COLOR_GREEN:
          other_cnt += cur_run->length;
          break;
        default:
          ok = false;
          break;
      }
      //pprintf(TextOutputStream,"run_idx=%2d cur_run:color=%d length=%2d (%3d,%3d)-(%3d,%3d), white_cnt=%2d other_cnt=%2d ok=%d\n",
      //        run_idx,cur_run->color,cur_run->length,V2COMP(cur_run->startPt),V2COMP(cur_run->endPt),
      //        white_cnt,other_cnt,ok?1:0);
    }
    // ensure at least 1/3 of pixels are white
    ok = ok && (2*white_cnt >= other_cnt);
    // ensure at least 2 pixels of white
    ok = ok && (white_cnt >= 2);

    if(ok){
      vector2i start,end;
      start = runs[green1_end_idx  +1].startPt;
      end   = runs[green2_start_idx-1].endPt  ;
      if(num_line_pts < max_line_pts){
        line_pts[num_line_pts] = vector2d(V2COMP(start+end)) / 2.0;
        num_line_pts++;
      }
#if cd_debug
#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
      rgb c;
      c.set(255,0,0);
      if(DebugImage) DebugImage->draw_line(V2COMP(start),V2COMP(end),c);
      pprintf(TextOutputStream,"stripe between %d and %d from (%3d,%3d) to (%3d,%3d)\n",
              big_green_idx,big_green_idx+1,V2COMP(start),V2COMP(end));
#endif
#endif
    }
  }

  return num_line_pts;
}

int Vision::findTransitions(vector2d *line_pts,int max_line_pts)
{
  static const int MaxRuns = 50;
  static ScanRun runs[MaxRuns];

  int num_line_pts;
  num_line_pts = 0;

  ScanRLEParams params;
  params.init(this,runs,MaxRuns);
  ScanRLECallback cb;

  for(int x=(width % cd_line_step)/2; x<width; x+=cd_line_step){
    params.reset();
    drawLine(x,0,x,height-1,cb, &params);
    //pprintf(TextOutputStream,"x=%3d scan had %3d runs\n",x,params.runIdx+1);
    num_line_pts += detectTransitionsScan(line_pts+num_line_pts,max_line_pts-num_line_pts,runs,params.runIdx+1);
  }
  for(int y=(height % cd_line_step)/2; y<height; y+=cd_line_step){
    params.reset();
    drawLine(0,y,width-1,y,cb,&params);
    //pprintf(TextOutputStream,"y=%3d scan had %3d runs\n",y,params.runIdx+1);
    num_line_pts += detectTransitionsScan(line_pts+num_line_pts,max_line_pts-num_line_pts,runs,params.runIdx+1);
  }

  return num_line_pts;
}

int Vision::fitLines(FieldLine *lines,int max_lines,vector2d *line_pts,int num_line_pts)
{
  int num_lines=0;

  for(int pt_idx=0; pt_idx<num_line_pts; pt_idx++){
    bool used_pt = false;
    int line_idx;
    vector2d cur_pt;

    cur_pt = line_pts[pt_idx];
    for(line_idx=0; line_idx<num_lines; line_idx++){
      if(lines[line_idx].getCount() > 0){
        if(lines[line_idx].ptNearLine(cur_pt)){
          lines[line_idx].appendPtToLine(cur_pt);
          used_pt = true;
          break;
        }
      }
    }
    if(!used_pt){
      if(num_lines < max_lines){
        lines[line_idx].init(cur_pt);
        num_lines++;
      }
    }
  }

  return num_lines;
}

struct FieldLineCountSort
{
private:
  FieldLine *lines;
public:
  FieldLineCountSort(FieldLine *_lines)
  {
    lines = _lines;
  }

  bool operator()(int x_idx,int y_idx) const
  {
    return lines[x_idx].getCount() > lines[y_idx].getCount();
  }
};

void Vision::findCornerGoalieBox(VObject *corner)
{
  static const uint MaxLinePts =  50;
  static const uint MaxLines   =  20;

  corner->confidence = 0;
  corner->loc.set(0.0,0.0,0.0);
  corner->left = 0.0;
  corner->right = 0.0;
  corner->distance = 0.0;
  corner->confInFrontOfIR = 0.0;
  corner->orientation = 0.0;
  corner->edge = 0;

  // storage for FindEdgeParams
  static vector2d LinePts[MaxLinePts];
  static FieldLine Lines[MaxLines];

  vector2d cornerLoc;
  int num_line_pts;
  int num_lines;

  num_line_pts = findTransitions(LinePts,MaxLinePts);
  num_lines = fitLines(Lines,MaxLines,LinePts,num_line_pts);
  
#if cd_debug
  printf("\n%d line pts\n", num_line_pts);
  /*
  for(uint i=0; i<num_line_pts; i++)
    printf("(%d,%d) ",(int)LinePts[i].x, (int)LinePts[i].y);
  */
  pprintf(TextOutputStream,"%d Lines:\n",num_lines);
#endif

  // sort lines by number of points
  FieldLineCountSort sorter(Lines);
  int line_idxs[MaxLines];
  for(int line_idx=0; line_idx<num_lines; line_idx++)
    line_idxs[line_idx] = line_idx;
  sort(line_idxs,line_idxs+num_lines,sorter);

#if cd_debug
  // print out info about lines
  for(int sort_idx=0; sort_idx<num_lines; sort_idx++){
    int i = line_idxs[sort_idx];
    Lines[i].printInfo();
#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
    rgb c;
    c.set(255,0,255);
    Lines[i].drawSeg(c);
#endif
  }
#endif
  // find 2 lines with most points
  FieldLine *l1=NULL,*l2=NULL;
  if(num_lines > 0 && Lines[line_idxs[0]].getCount()>3){
    l1 = &Lines[line_idxs[0]];
  }
  if(num_lines > 1 && Lines[line_idxs[1]].getCount()>3){
    l2 = &Lines[line_idxs[1]];
  }

  // We have two good lines
  if(l1!=NULL && l2!=NULL){
    // Get corner location
    cornerLoc = l1->computeIntersect(*l2);

    if(onImage((int)rint(cornerLoc.x),(int)rint(cornerLoc.y))){
      // Check for lines orthagonal in the plane
      vector3d lvec1, lvec2, lvec12, lvec22;
      GVector::intersect_ray_plane(camera_loc, getPixelDirection(l1->endPt1().x, l1->endPt1().y),vector3d(0.0,0.0,0.0),vector3d(0.0,0.0,1.0),lvec1);
      GVector::intersect_ray_plane(camera_loc, getPixelDirection(l1->endPt2().x, l1->endPt2().y),vector3d(0.0,0.0,0.0),vector3d(0.0,0.0,1.0),lvec12);
      GVector::intersect_ray_plane(camera_loc, getPixelDirection(l2->endPt1().x, l2->endPt1().y),vector3d(0.0,0.0,0.0),vector3d(0.0,0.0,1.0),lvec2);
      GVector::intersect_ray_plane(camera_loc, getPixelDirection(l2->endPt2().x, l2->endPt2().y),vector3d(0.0,0.0,0.0),vector3d(0.0,0.0,1.0),lvec22);
      lvec1 = lvec12 - lvec1;
      lvec2 = lvec22 - lvec2;

      double dp = (lvec1.norm()).dot(lvec2.norm());
      
      // Confidence should drop if there aren't many pts on the line
      // or if there lots of lines with similar number of pts
      corner->confidence = 1 - fabs(dp);

      // Map corner location into the real world
      vector3d cornerDir = getPixelDirection(cornerLoc.x, cornerLoc.y);
      bool intersects = GVector::intersect_ray_plane(camera_loc, cornerDir,vector3d(0.0,0.0,0.0),vector3d(0.0,0.0,1.0),corner->loc);
      corner->distance = corner->loc.length();
      
      if(!intersects)
        corner->confidence = 0;

      //pprintf(TextOutputStream,"Corner! Conf = %g   %d,%d\n",corner->confidence, l1->getCount(), l2->getCount());
      
#if cd_debug
      if(corner->confidence > 0.5){
        printf("\n#################Corner found! (%d,%d)\n",(int)cornerLoc.x,(int)cornerLoc.y);
        printf("world loc (%g,%g,%g)\n",V3COMP(corner->loc));
#if defined(PLATFORM_LINUX) && !defined(VIS_NO_LIBS)
        rgb c,c2; 
        c .set(0,0,255);
        c2.set(0,255,0);
        if(DebugImage) DebugImage->draw_line((int)cornerLoc.x-3,(int)cornerLoc.y  , (int)cornerLoc.x+3,(int)cornerLoc.y  , c);
        if(DebugImage) DebugImage->draw_line((int)cornerLoc.x  ,(int)cornerLoc.y-3, (int)cornerLoc.x  ,(int)cornerLoc.y+3, c);
        l1->drawLine(c,c2);
        l2->drawLine(c,c2);
#endif
      }
#endif
    }
  }else{
    corner->confidence = 0;
#if cd_debug
    printf("\nNo corner found...\n");
#endif
  }
}