/* 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 <algorithm>

using std::sort;

#include "../headers/Config.h"
#include "../headers/Reporting.h"

#include "Vision.h"
#include "VisionInterface.h"

using namespace VisionInterface;

// if this is true the radial map will be filled in properly,
// otherwise it will claim that nothing is visible
static const bool EnableRadialMap = true;

static const bool debug_radial = false;

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

extern SImage *DebugImage;
static bool DrawRadialLines = false;
#endif
#endif

template<class T>
T CheckBound(T x,T low,T high,const char *var_name) {
  if(x != bound(x,low,high)) {
    pprintf(TextOutputStream,"failed bounds check on %s\n",var_name);
  }
  return bound(x,low,high);
}

void Vision::AddRadialObject(double x, double y, RadialObjectMap *radial_map, int robj_idx, int ang_idx) {
  //robj_idx = CheckBound(robj_idx,0,NUM_ROBJECTS,"robj_idx");
  //ang_idx  = CheckBound(ang_idx, 0,NUM_ANGLES-1,"ang_idx");

  if(debug_radial) pprintf(TextOutputStream,"trying to add object at (%g,%g), robj_idx=%d, ang_idx=%d\n",
                           x,y,robj_idx,ang_idx);

  if(radial_map->robjects[robj_idx][ang_idx].confidence==0.0) {
    if(debug_radial) pprintf(TextOutputStream,"adding object at (%g,%g), robj_idx=%d, ang_idx=%d\n",
                             x,y,robj_idx,ang_idx);

    RadialObjectReading *ror;
    vector3d loc;

    ror = &radial_map->robjects[robj_idx][ang_idx];
    ror->confidence = 1.0;
    if(intersectPixelZPlane(x,y,0.0,loc)) {
      ror->distance = hypot(loc.x,loc.y);
    } else {
      ror->distance = RadialFarDistance;
    }
  }
}

struct RadialRLEParams {
  Vision *vis;
  int runIdx;
  int curColor;
  RadialRun *curRun;

  void init(Vision *_vis) {
    vis = _vis;
    runIdx = -1;
    curColor = -1;
    curRun = NULL;
  }
};

struct RadialRLECallback {
  bool operator()(int cur_x,int cur_y,RadialRLEParams *rsp) {
    int color;

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

    return true;
  }
};

static const int ColorToROType[NUM_COLORS]=
  {
    /* COLOR_BACKGROUND */ RO_Obs,
    /* COLOR_ORANGE     */ RO_Ball,
    /* COLOR_GREEEN     */ RO_Field,
    /* COLOR_PINK       */ RO_Obs,
    /* COLOR_CYAN       */ RO_Goal,
    /* COLOR_YELLOW     */ RO_Goal,
    /* COLOR_BLUE       */ RO_Obs,
    /* COLOR_RED        */ RO_Obs,
    /* COLOR_WHITE      */ RO_Stripe
  };

static const int ColorToROSubType[NUM_COLORS]=
  {
    /* COLOR_BACKGROUND */ RO_Obs_Unknown,
    /* COLOR_ORANGE     */ 0,
    /* COLOR_GREEEN     */ 0,
    /* COLOR_PINK       */ RO_Obs_Unknown,
    /* COLOR_CYAN       */ RO_Goal_Cyan,
    /* COLOR_YELLOW     */ RO_Goal_Yellow,
    /* COLOR_BLUE       */ RO_Obs_Blue_Robot,
    /* COLOR_RED        */ RO_Obs_Red_Robot,
    /* COLOR_WHITE      */ 0
  };

static char const * const ROTypeToStr[RO_Num_Types]=
  {
    "Field",
    "Stripe",
    "Wall",
    "Goal",
    "Ball",
    "Obs"
  };

static char const * const ROSubTypeToStr[RO_Num_Types][RO_Max_Num_SubTypes]=
  {
    {"",       "",        ""},
    {"",       "",        ""},
    {"",       "",        ""},
    {"Yellow", "Cyan",    ""},
    {"",       "",        ""},
    {"Unknown","RedRobot","BlueRobot"},
  };

//                               base_color  new_color
static const int ColorExpandCost[NUM_COLORS][NUM_COLORS]=
  {
    // new_color
    // background, orange,  green, pink, cyan, yellow, blue,  red, white   // base_color
    {          -2,    256,      2,  256,  256,    256,    1,  256,   256}, // background
    {           2,     -2,    256,  256,  256,      8,  256,  256,     8}, // orange     
    {           2,    256,     -2,  256,  256,    256,   16,  256,   256}, // green
    {           8,    256,    256,   -2,  256,    256,  256,  256,   256}, // pink  
    {           4,    256,    256,  256,   -2,    256,    4,  256,   256}, // cyan  
    {           4,      4,    256,  256,  256,     -2,  256,  256,     8}, // yellow 
    {           1,    256,      2,  256,    4,    256,   -2,  256,   256}, // blue      
    {           2,      2,    256,    2,  256,    256,  256,   -2,   256}, // red
    {           2,      4,    256,  256,  256,      4,  256,  256,    -2}  // white
  };

static const int ObsExpandCost[NUM_COLORS]=
  {
    /* COLOR_BACKGROUND */ -2,
    /* COLOR_ORANGE     */  1,
    /* COLOR_GREEEN     */  2,
    /* COLOR_PINK       */  1,
    /* COLOR_CYAN       */  2,
    /* COLOR_YELLOW     */  4,
    /* COLOR_BLUE       */ -2,
    /* COLOR_RED        */ -2,
    /* COLOR_WHITE      */  2
  };

// redness - blueness
static const int ObsRedness[NUM_COLORS]=
  {
    /* COLOR_BACKGROUND */  0,
    /* COLOR_ORANGE     */  1,
    /* COLOR_GREEEN     */  0,
    /* COLOR_PINK       */  1,
    /* COLOR_CYAN       */ -1,
    /* COLOR_YELLOW     */  0,
    /* COLOR_BLUE       */ -2,
    /* COLOR_RED        */  4,
    /* COLOR_WHITE      */  0
  };

static const int ObsRednessPixelMult = 4;

struct RadialSizeSorter {
  RO *ros;

  RadialSizeSorter(RO *_ros) : ros(_ros)
  {}

  bool operator()(int a,int b){
    return ros[a].size > ros[b].size;
  }
};

void Vision::radialFindObjects(int num_radial_runs,RadialRun *radial_runs,RO *ros,int *num_ros,int ang_idx,float angle)
{
  static const bool debug=debug_radial;

  static const int min_large_object_size = 5;

  int ro_idx;

  num_ros[ang_idx] = 0;

  int num_ros_here=0;

  if(debug) pprintf(TextOutputStream,"doing ang_idx=%02d\n",ang_idx);

  // first find big objects >= 5 pixels
  for(int cur_rr_idx=0; cur_rr_idx<num_radial_runs; cur_rr_idx++){
    RadialRun *rr;

    rr = &radial_runs[cur_rr_idx];
    if(rr->length >= min_large_object_size){
      RO *ro;
      int ro_idx;

      // don't add if out of ROs
      if(num_ros_here < MAX_ROS-1){
        ro_idx = num_ros_here;
        ro = &ros[ro_idx];
        num_ros_here++;

        ro->start_run = cur_rr_idx;
        ro->end_run   = cur_rr_idx;
        ro->size      = rr->length;
      }
    }
  }

  // try to expand large objects in order of largest first
  // sort the objects by size first
  int sort_idx[MAX_ROS];
  for(int i=0; i<num_ros_here; i++)
    sort_idx[i] = i;

  RadialSizeSorter sorter(ros);
  sort(&sort_idx[0],&sort_idx[num_ros_here],sorter);

  if(debug){
    for(int i=0; i<num_ros_here; i++){
      RO *ro;
      ro = &ros[sort_idx[i]];
      pprintf(TextOutputStream,"RO i=%02d ro_idx=%02d size=%02d start_run=%02d color=%02d start=(%03d,%03d) end=(%03d,%03d)\n",
              i,sort_idx[i],
              ro->size,ro->start_run,radial_runs[ro->start_run].color,V2COMP(radial_runs[ro->start_run].startPt),
              V2COMP(radial_runs[ro->end_run].endPt));
    }
  }

  // now try to expand objects
  for(int i=0; i<num_ros_here; i++){
    int base_color;
    int ro_idx;
    RO *ro;

    ro_idx = sort_idx[i];
    ro = &ros[ro_idx];
    base_color = radial_runs[ro->start_run].color;

    // try to expand towards first run
    int cost; // weighted count of pixels of the wrong color - weighted count of correct color
    int expansion; // number of new pixels
    cost = 0;
    expansion = 0;
    for(int rr_idx=ro->start_run-1; rr_idx>=0 && cost<256; rr_idx--){
      RadialRun *rr;
      int rr_color;

      rr = &radial_runs[rr_idx];
      rr_color = rr->color;     
      cost += rr->length*ColorExpandCost[base_color][rr_color];
      expansion += rr->length;
      if(cost <= 0){
        ro->start_run = rr_idx;
        ro->size += expansion;
        if(debug){
          pprintf(TextOutputStream,"expanding i %02d ro %02d backward to include run %02d, cost=%3d\n",i,ro_idx,rr_idx,cost);
        }
        cost = 0;
        expansion = 0;
      }
      if(rr->length >= min_large_object_size)
        break;
    }
    // try to expand towards last run
    cost = 0;
    expansion = 0;
    for(int rr_idx=ro->end_run+1; rr_idx<num_radial_runs && cost<256; rr_idx++){
      RadialRun *rr;
      int rr_color;

      rr = &radial_runs[rr_idx];
      rr_color = rr->color;
      cost += rr->length*ColorExpandCost[base_color][rr_color];
      expansion += rr->length;
      if(cost <= 0){
        ro->end_run = rr_idx;
        ro->size += expansion;
        if(debug){
          pprintf(TextOutputStream,"expanding i %02d ro %02d forward  to include run %02d, cost=%3d\n",i,ro_idx,rr_idx,cost);
        }
        cost = 0;
        expansion = 0;
      }
      if(rr->length >= min_large_object_size)
        break;
    }
  }
  // coalesce objects that are overlapping and same color
  // divide objects that are overlapping and different colors
  ro_idx=0;
  while(ro_idx < num_ros_here-1){
    if(ros[ro_idx].end_run >= ros[ro_idx+1].start_run){
      if(radial_runs[ros[ro_idx].end_run].color == radial_runs[ros[ro_idx+1].start_run].color){
        // coalesce
        // add new length onto first run
        while(ros[ro_idx].end_run < ros[ro_idx+1].end_run){
          ros[ro_idx].end_run++;
          ros[ro_idx].size += radial_runs[ros[ro_idx].end_run].length;
        }
        // remove second run from list
        for(int i=ro_idx+1; i<num_ros_here-1; i++){
          ros[i]=ros[i+1];
        }
        num_ros_here--;
      }else{
        // divide up runs
        int rr_q_idx1,rr_q_idx2; // radial run question idx for start and end
        int split_cost;
        int best_split_cost;
        int best_split_idx; // idx of first run that should belong to second RO for best split
        int color1,color2; // colors of ros

        color1 = radial_runs[ros[ro_idx  ].end_run  ].color;
        color2 = radial_runs[ros[ro_idx+1].start_run].color;

        rr_q_idx1 = ros[ro_idx+1].start_run;
        rr_q_idx2 = ros[ro_idx  ].end_run  ;

        // determine best place to split
        // make initial guess of everything should belong to second ro
        split_cost = 0;
        best_split_idx = rr_q_idx1;
        for(int rr_idx=rr_q_idx1; rr_idx<=rr_q_idx2; rr_idx++){
          RadialRun *rr;
          rr = &radial_runs[rr_idx];
          split_cost += rr->length*ColorExpandCost[color2][rr->color];
        }
        best_split_cost = split_cost;

        // consider other possibilities to see it better
        for(int split_idx=rr_q_idx1+1; split_idx<=rr_q_idx2+1; split_idx++){
          RadialRun *rr;
          rr = &radial_runs[split_idx-1];
          split_cost -= rr->length*ColorExpandCost[color2][rr->color];
          split_cost += rr->length*ColorExpandCost[color1][rr->color];
          if(split_cost < best_split_cost){
            best_split_cost = split_cost;
            best_split_idx  = split_idx;
          }
        }

        // split runs using best split found
        // update data structure
        ros[ro_idx  ].end_run   = best_split_idx-1;
        ros[ro_idx+1].start_run = best_split_idx  ;
        // update summary stats
        for(int rr_idx=rr_q_idx1; rr_idx<=rr_q_idx2; rr_idx++){
          RadialRun *rr;
          rr = &radial_runs[rr_idx];
          if(rr_idx < best_split_idx){
            ros[ro_idx+1].size -= rr->length;
          }else{
            ros[ro_idx  ].size -= rr->length;
          }
        }
      }
    }else{
      ro_idx++;
    }
  }
  if(debug){
    for(int i=0; i<num_ros_here; i++){
      RO *ro;
      ro = &ros[i];
      pprintf(TextOutputStream,"RO size=%02d start_run=%02d end_run=%02d color=%02d start=(%03d,%03d) end=(%03d,%03d)\n",
              ro->size,ro->start_run,ro->end_run,radial_runs[ro->start_run].color,V2COMP(radial_runs[ro->start_run].startPt),
              V2COMP(radial_runs[ro->end_run].endPt));
    }
  }

  //assign types to the large objects detected
  for(int ro_idx=0; ro_idx<num_ros_here; ro_idx++){
    RO *ro;
    RadialRun *rr;

    ro = &ros[ro_idx];
    rr = &radial_runs[ro->start_run];
    ro->type      = ColorToROType   [rr->color];
    ro->sub_type  = ColorToROSubType[rr->color];
    
    if(ro->type == RO_Stripe){
      // decide between stripe and wall
      static const double min_wall_length = 50.0;

      bool wall;

      // should occupy at least 50.0mm of length
      vector3d start_loc(0.0,0.0,0.0),end_loc(0.0,0.0,0.0);
      vector2i start_pt,end_pt;
      bool start_intersects,end_intersects;

      start_pt = radial_runs[ro->start_run].startPt;
      end_pt   = radial_runs[ro->end_run  ].endPt  ;

      start_intersects = intersectPixelZPlane(start_pt.x,
                                              start_pt.y,
                                              0.0,start_loc);
      end_intersects   = intersectPixelZPlane(end_pt.x,
                                              end_pt.y,
                                              0.0,end_loc  );

      // should contain more white pixels than there are green pixels after it
      int white_cnt;
      int green_cnt;
      white_cnt=0;
      for(int rr_idx=ro->start_run; rr_idx<=ro->end_run; rr_idx++){
        if(radial_runs[rr_idx].color == COLOR_WHITE){
          white_cnt += radial_runs[rr_idx].length;
        }
      }
      green_cnt=0;
      for(int rr_idx=ro->end_run+1; rr_idx<num_radial_runs; rr_idx++){
        if(radial_runs[rr_idx].color == COLOR_GREEN){
          green_cnt += radial_runs[rr_idx].length;
        }
      }

      // make decision
      wall = ((!start_intersects ||
               !end_intersects   ||
               ((end_loc - start_loc).length() >= min_wall_length)) &&
              (green_cnt < white_cnt));

      // label object appropriately
      if(wall){
        ro->type = RO_Wall;
      }else{
        ro->type = RO_Stripe;
      }
    }
  }

  // print overview
  if(false && debug){
    pprintf(TextOutputStream,"overview pre-stripe\n");
    int rr_idx,ro_idx;
    rr_idx = ro_idx = 0;
    while(rr_idx < num_radial_runs){
      if(ro_idx < num_ros_here && ros[ro_idx].start_run == rr_idx){
        RO *ro;
        ro = &ros[ro_idx];
        pprintf(TextOutputStream,"RO type=%6s sub_type=%8s size=%02d start_run=%02d end_run=%02d color=%02d start=(%03d,%03d) end=(%03d,%03d)\n",
                ROTypeToStr[ro->type],ROSubTypeToStr[ro->type][ro->sub_type],ro->size,
                ro->start_run,ro->end_run,radial_runs[ro->start_run].color,V2COMP(radial_runs[ro->start_run].startPt),
                V2COMP(radial_runs[ro->end_run].endPt));
        ro_idx++;
        rr_idx = ro->end_run+1;
      }else{
        RadialRun *rr=&radial_runs[rr_idx];
        pprintf(TextOutputStream,"rr_idx %2d color %2d length %2d start (%3d,%3d) end (%3d,%3d)\n",
                rr_idx,rr->color,rr->length,rr->startPt.x,rr->startPt.y,rr->endPt.x,rr->endPt.y);
        rr_idx++;
      }
    }
  }

  // identify thin stripes that might not have segmented well
  int next_field_ro_idx=0;
  ro_idx=0;
  while(ro_idx < num_ros_here){
    if(ros[ro_idx].type == RO_Field)
      break;
    ro_idx++;
  }
  // loop over neighboring pairs of field objects
  next_field_ro_idx = ro_idx+1;
  while(next_field_ro_idx < num_ros_here){
    if(ros[next_field_ro_idx].type != RO_Field){
      next_field_ro_idx++;
      continue;
    }

    // at this point ro_idx is one field ro and next_field_ro_idx is the next field ro
    if(next_field_ro_idx - ro_idx > 1){
      // skip this one as another object is inbetween field segments
      ro_idx = next_field_ro_idx;
      next_field_ro_idx++;
      continue;
    }

    // field ros are right next to each other, look at stuff inbetween
    // if stuff matches (cyan)?(white)(yellow)? or (yellow)?(white)(cyan)
    //   possibly with some black thrown in, call it a stripe
    int rr_idx;
    bool seen_white=false;
    bool seen_end=false;
    bool possibly_stripe=true;
    int boundary_color_start = COLOR_BACKGROUND; // background, cyan, or yellow
    int pixel_size = 0;
    for(rr_idx=ros[ro_idx].end_run+1; possibly_stripe && rr_idx<ros[next_field_ro_idx].start_run; rr_idx++){
      RadialRun *rr;
      rr = &radial_runs[rr_idx];
      pixel_size += rr->length;
      switch(rr->color){
        case COLOR_WHITE:
          seen_white = true;
          if(seen_end)
            possibly_stripe = false;
          break;
        case COLOR_CYAN:
          if(seen_white){
            switch(boundary_color_start){
              case COLOR_BACKGROUND:
                boundary_color_start = COLOR_YELLOW; // assume started with other color
                seen_end = true;
                break;
              case COLOR_CYAN: possibly_stripe = false; break;
              case COLOR_YELLOW: seen_end = true; break;
            }
          }else{
            switch(boundary_color_start){
              case COLOR_BACKGROUND: boundary_color_start = COLOR_CYAN; break;
              case COLOR_CYAN: break;
              case COLOR_YELLOW: possibly_stripe = false; break;
            }
          }
          break;
        case COLOR_YELLOW:
          if(seen_white){
            switch(boundary_color_start){
              case COLOR_BACKGROUND:
                boundary_color_start = COLOR_CYAN; // assume started with other color
                seen_end = true;
                break;
              case COLOR_YELLOW: possibly_stripe = false; break;
              case COLOR_CYAN: seen_end = true; break;
            }
          }else{
            switch(boundary_color_start){
              case COLOR_BACKGROUND: boundary_color_start = COLOR_YELLOW; break;
              case COLOR_YELLOW: break;
              case COLOR_CYAN: possibly_stripe = false; break;
            }
          }
          break;
        case COLOR_BACKGROUND:
          break;
        default:
          possibly_stripe=false;
          break;
      }
    }
    if(possibly_stripe && seen_white){
      // call it a stripe
      // move ros down to make room
      for(int move_ro_idx=num_ros_here-1; move_ro_idx>=next_field_ro_idx; move_ro_idx--){
        ros[move_ro_idx+1] = ros[move_ro_idx];
      }
      // insert new ro
      // skip insert if out of ROs
      if(num_ros_here < MAX_ROS-1){
        RO *ro;
        ro = &ros[next_field_ro_idx];
        ro->start_run = ros[ro_idx       ].end_run  +1;
        ro->end_run   = ros[next_field_ro_idx+1].start_run-1;
        ro->size      = pixel_size;
        ro->type      = RO_Stripe;
        ro->sub_type  = 0;
        num_ros_here++;
      }
        
      ro_idx = next_field_ro_idx+1;
      next_field_ro_idx = ro_idx+1;
    }else{
      ro_idx = next_field_ro_idx;
      next_field_ro_idx++;
    }
  }

  // print overview
  if(debug){
    pprintf(TextOutputStream,"overview pre-obs expand\n");
    int rr_idx,ro_idx;
    rr_idx = ro_idx = 0;
    while(rr_idx < num_radial_runs){
      if(ro_idx < num_ros_here && ros[ro_idx].start_run == rr_idx){
        RO *ro;
        ro = &ros[ro_idx];
        pprintf(TextOutputStream,"RO type=%6s sub_type=%8s size=%02d start_run=%02d end_run=%02d color=%02d start=(%03d,%03d) end=(%03d,%03d)\n",
                ROTypeToStr[ro->type],ROSubTypeToStr[ro->type][ro->sub_type],ro->size,
                ro->start_run,ro->end_run,radial_runs[ro->start_run].color,V2COMP(radial_runs[ro->start_run].startPt),
                V2COMP(radial_runs[ro->end_run].endPt));
        ro_idx++;
        rr_idx = ro->end_run+1;
      }else{
        RadialRun *rr=&radial_runs[rr_idx];
        pprintf(TextOutputStream,"rr_idx %2d color %2d length %2d start (%3d,%3d) end (%3d,%3d)\n",
                rr_idx,rr->color,rr->length,rr->startPt.x,rr->startPt.y,rr->endPt.x,rr->endPt.y);
        rr_idx++;
      }
    }
  }

  // try to expand obstacle to capture neighboring robot colors and tag obstacles appropriately
  ro_idx = 0;
  while(ro_idx < num_ros_here){
    RO *ro;

    ro = &ros[ro_idx];
    if(ro->type != RO_Obs){
      ro_idx++;
      continue;
    }

    int cost = 0;
    int redness = 0; // vs. blueness
    int total_redness = 0;
    int expansion = 0;
    int stop_run = 0;

    switch(ro->sub_type){
      case RO_Obs_Red_Robot:  total_redness +=  ObsRednessPixelMult * ro->size; break;
      case RO_Obs_Blue_Robot: total_redness += -ObsRednessPixelMult * ro->size; break;
      default: break;
    }

    // try to expand backwards to previous ro
    int back_redness = 0; // redness total of last addition backwards
    if(ro_idx > 0)
      stop_run = ros[ro_idx-1].end_run+1;
    for(int rr_idx=ro->start_run-1; rr_idx>=stop_run; rr_idx--){
      RadialRun *rr;
      int length;

      rr = &radial_runs[rr_idx];
      length = rr->length;
      cost    += length*ObsExpandCost[rr->color];
      redness += length*ObsRedness   [rr->color];
      expansion += length;
      if(cost <= 0){
        if(debug){
          pprintf(TextOutputStream,"expanding ro %02d backward to include run %02d, cost=%3d\n",ro_idx,rr_idx,cost);
        }
        // add to obstacle
        back_redness = redness;
        cost = 0;
        ro->start_run = rr_idx;
        ro->size += expansion;
      }
    }
    total_redness += back_redness;

    // try to expand forwards
    bool try_more = true;
    while(try_more && ro_idx<num_ros_here){
      int forward_redness = 0;
      cost = 0;
      redness = 0;
      expansion = 0;
      stop_run = num_radial_runs-1;
      if(ro_idx < num_ros_here-1)
        stop_run = ros[ro_idx+1].start_run-1;
      for(int rr_idx=ro->end_run+1; rr_idx<=stop_run; rr_idx++){
        RadialRun *rr;
        int length;

        rr = &radial_runs[rr_idx];
        length = rr->length;
        cost    += length*ObsExpandCost[rr->color];
        redness += length*ObsRedness   [rr->color];
        expansion += length;
        if(cost <= 0){
          if(debug){
            pprintf(TextOutputStream,"expanding ro %02d forward to include run %02d, cost=%3d\n",ro_idx,rr_idx,cost);
          }
          // add to obstacle
          forward_redness = redness;
          cost = 0;
          ro->end_run = rr_idx;
          ro->size += expansion;
        }
      }
      total_redness += forward_redness;
      try_more = (ro->end_run == stop_run);
      if(try_more){
        if(ro_idx+1<num_ros_here){
          RO *next_ro;
          next_ro = &ros[ro_idx+1];
          if(next_ro->type != RO_Obs){
            try_more = false;
          }else{
            // add following obstacle to this one
            // add to stats
            switch(next_ro->sub_type){
              case RO_Obs_Red_Robot:  total_redness +=  ObsRednessPixelMult * next_ro->size; break;
              case RO_Obs_Blue_Robot: total_redness += -ObsRednessPixelMult * next_ro->size; break;
              default: break;
            }
            ro->size    += next_ro->size;
            ro->end_run  = next_ro->end_run;
            // delete following obstacle
            for(int move_ro_idx=ro_idx+1; move_ro_idx+1<num_ros_here; move_ro_idx++){
              ros[move_ro_idx] = ros[move_ro_idx+1];
            }
            num_ros_here--;
          }
        }else{
          try_more = false;
        }
      }
    }

    // assign appropriate type
    if(total_redness > ObsRednessPixelMult*2){
      ro->sub_type = RO_Obs_Red_Robot;
    }else if(total_redness < -ObsRednessPixelMult*2){
      ro->sub_type = RO_Obs_Blue_Robot;
    }else{
      ro->sub_type = RO_Obs_Unknown;
    }

    // next ro
    ro_idx++;
  }

  // print overview
  if(debug){
    pprintf(TextOutputStream,"\n\noverview\n");
    int rr_idx,ro_idx;
    rr_idx = ro_idx = 0;
    while(rr_idx < num_radial_runs){
      if(ro_idx < num_ros_here && ros[ro_idx].start_run == rr_idx){
        RO *ro;
        ro = &ros[ro_idx];
        pprintf(TextOutputStream,"RO type=%6s sub_type=%8s size=%02d start_run=%02d end_run=%02d color=%02d start=(%03d,%03d) end=(%03d,%03d)\n",
                ROTypeToStr[ro->type],ROSubTypeToStr[ro->type][ro->sub_type],ro->size,
                ro->start_run,ro->end_run,radial_runs[ro->start_run].color,V2COMP(radial_runs[ro->start_run].startPt),
                V2COMP(radial_runs[ro->end_run].endPt));
        ro_idx++;
        rr_idx = ro->end_run+1;
      }else{
        RadialRun *rr=&radial_runs[rr_idx];
        pprintf(TextOutputStream,"rr_idx %2d color %2d length %2d start (%3d,%3d) end (%3d,%3d)\n",
                rr_idx,rr->color,rr->length,rr->startPt.x,rr->startPt.y,rr->endPt.x,rr->endPt.y);
        rr_idx++;
      }
    }
    pprintf(TextOutputStream,"\n\n");
  }

  // calculate starting and ending locations in the world
  for(ro_idx=0; ro_idx<num_ros_here; ro_idx++){
    RO *ro;
    vector3d start_loc(0.0,0.0,0.0),end_loc(0.0,0.0,0.0);
    vector2i start_pt,end_pt;
    bool start_intersects,end_intersects;
    
    ro = &ros[ro_idx];

    start_pt = radial_runs[ro->start_run].startPt;
    end_pt   = radial_runs[ro->end_run  ].endPt  ;
    
    start_intersects = intersectPixelZPlane(start_pt.x,
                                            start_pt.y,
                                            0.0,start_loc);
    end_intersects   = intersectPixelZPlane(end_pt.x,
                                            end_pt.y,
                                            0.0,end_loc  );
    
    if(!start_intersects){
      start_loc.set(cos(angle),sin(angle),0.0);
      start_loc *= RadialFarDistance;
    }
    if(!end_intersects){
      end_loc.set(cos(angle),sin(angle),0.0);
      end_loc *= RadialFarDistance;
    }

    ro->start_pt.set(start_loc.x,start_loc.y);
    ro->end_pt  .set(end_loc  .x,end_loc  .y);
  }

  // update external variables
  num_ros[ang_idx] = num_ros_here;
}


void Vision::UpdateRadialAngle(int ang_idx,float angle,GVector::vector2d<int> scan_start_img,GVector::vector2d<int> scan_end_img)
{
  static const bool debug_verbose = true;
  RadialRLEParams rrp;
  rrp.init(this);

  RadialRLECallback callback;
  drawLine(scan_start_img.x,scan_start_img.y,scan_end_img.x,scan_end_img.y,callback,&rrp);
  num_radial_runs = rrp.runIdx+1;
  
  for(int rr_idx=0; rr_idx<num_radial_runs; rr_idx++) {
    RadialRun *rr=&radial_runs[rr_idx];
    if(debug_radial && debug_verbose)
      pprintf(TextOutputStream,"rr_idx %2d color %2d length %2d start (%3d,%3d) end (%3d,%3d)\n",
              rr_idx,rr->color,rr->length,rr->startPt.x,rr->startPt.y,rr->endPt.x,rr->endPt.y);
  }

  radialFindObjects(num_radial_runs,radial_runs,radial_map.ros[ang_idx],radial_map.num_ros,ang_idx,angle);

  //////////////////////////////////////////////////////////////////////
  // transition below this line
  //////////////////////////////////////////////////////////////////////

  for(int ro_idx=0; ro_idx<radial_map.num_ros[ang_idx]; ro_idx++){
    RO *ro;
    vector2i start_pt;
    vector2i end_pt;

    ro = &radial_map.ros[ang_idx][ro_idx];
    start_pt = radial_runs[ro->start_run].startPt;
    end_pt   = radial_runs[ro->end_run  ].endPt  ;
    switch(ro->type){
      case RO_Field:
        {
          AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_FIELD_CLEAR_START,ang_idx);
          bool done=false;
          for(int ro_idx2=ro_idx+1; !done && ro_idx2<radial_map.num_ros[ang_idx]; ro_idx2++){
            RO *ro2;
            ro2 = &radial_map.ros[ang_idx][ro_idx2];
            switch(ro2->type){
              case RO_Field:
              case RO_Stripe:
              case RO_Ball:
                end_pt = radial_runs[ro2->end_run].endPt;
                break;
              default:
                done = true;
                break;
            }
          }
          AddRadialObject(end_pt.x,  end_pt.y,  &radial_map,ROBJECT_FIELD_CLEAR_END,ang_idx);
        }
        break;
      case RO_Stripe:
        AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_STRIPE,ang_idx);
        break;
      case RO_Wall:
        AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_WALL,ang_idx);
        break;
      case RO_Goal:
        switch(ro->sub_type){
          case RO_Goal_Yellow:
            AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_YELLOW_GOAL,ang_idx);
            break;
          case RO_Goal_Cyan:
            AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_CYAN_GOAL,ang_idx);
            break;
        }
        break;
      case RO_Ball:
        AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_BALL,ang_idx);
        break;
      case RO_Obs:
        AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_ROBOT,ang_idx);
        switch(ro->sub_type){
          case RO_Obs_Red_Robot:
            AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_RED_ROBOT,ang_idx);
            break;
          case RO_Obs_Blue_Robot:
            AddRadialObject(start_pt.x,start_pt.y,&radial_map,ROBJECT_BLUE_ROBOT,ang_idx);
            break;
          default:
            break;
        }
        break;
    }
  }
}

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

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

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

  // Clear out old map contents.
  for(int obj_idx=0; obj_idx<NUM_ROBJECTS; obj_idx++) {
    for(int angle_idx=0; angle_idx<NUM_ANGLES; angle_idx++) {
      radial_map.robjects[obj_idx][angle_idx].confidence = 0.0;
      radial_map.robjects[obj_idx][angle_idx].distance   = RadialFarDistance;

      radial_map.num_ros[angle_idx] = 0;
    }
  }

  if(!EnableRadialMap)
    return;

  double angle_left,angle_right;

  // Figure out which angles might be visible on the ground
  findSpan(angle_left,angle_right,0,width-1,0,height-1);
  if(print_now && debug_verbose) {
    pprintf(TextOutputStream,"angle range [%g,%g]\n",angle_left,angle_right);
  }

  // Process each angle
  double angle_first;
  angle_first = angle_left - fmodt(angle_left,RadialAngleSpacing);
  double angle_cur;
  int angle_cur_idx;
  for(angle_cur=angle_first, angle_cur_idx=RadialObjectMap::angleToIdx(angle_cur);
      angle_cur>angle_right;
      angle_cur-=RadialAngleSpacing, angle_cur_idx--) {
    if(print_now && debug_verbose) {
      pprintf(TextOutputStream,"PROCESSING angle %g\n",angle_cur);
    }

    if(angle_cur_idx < 0 || angle_cur_idx >= NUM_ANGLES) {
      pprintf(TextOutputStream,"angle_cur_idx of %d is out of bounds!!!!!!!\n",angle_cur_idx);
      continue;
    }

    // determine start/end location in ego coordinates of scan
    vector3d scan_start,scan_dir;
    scan_start.set(0.0,0.0,0.0);
    scan_dir.set(cos(angle_cur),sin(angle_cur),0.0);

    // convert to camera coordinates
    vector3d scan_start_cam,scan_dir_cam;
    scan_start_cam = convertToCamera(scan_start);
    scan_dir_cam   = convertToCamera(scan_start+scan_dir) - scan_start_cam;
    if(print_now && debug_verbose)
      pprintf(TextOutputStream,"scan start cam (%g,%g,%g), scan dir cam (%g,%g,%g)\n",
              scan_start_cam.x,scan_start_cam.y,scan_start_cam.z,
              scan_dir_cam.x,  scan_dir_cam.y,  scan_dir_cam.z);

    if(scan_start_cam.z < 1e-3) {
      vector3d intersect_pt;
      if(GVector::intersect_ray_plane(scan_start_cam,scan_dir_cam,vector3d(0.0,0.0,1e-3),vector3d(0.0,0.0,1.0),intersect_pt)) {
        scan_start_cam = intersect_pt;
      } else {
        // drop this angle, doesn't intersect viewable section
        if(print_now)
          pprintf(TextOutputStream,"dropping angle %g because it fails to be in front of camera\n",angle_cur);
        continue;
      }
    }

    // This is the distance to project out to find the horizon
    // It must be large enough to represent infinity reasonably on the camera
    //   but small enough to not screw up the intersection with the plane of the camera.
    static const double inf_distance = 1e6; // mm
    vector3d scan_end_cam;
    scan_end_cam = scan_start_cam + scan_dir_cam * inf_distance;

    if(print_now && debug_verbose)
      pprintf(TextOutputStream,"ssc (%g,%g,%g) sec (%g,%g,%g)\n",
              scan_start_cam.x,scan_start_cam.y,scan_start_cam.z,
              scan_end_cam.x,scan_end_cam.y,scan_end_cam.z);

    if(scan_end_cam.z < 1e-3) {
      vector3d intersect_pt;
      if(GVector::intersect_ray_plane(scan_end_cam,-scan_dir_cam,vector3d(0.0,0.0,1e-3),vector3d(0.0,0.0,1.0),intersect_pt)) {
        scan_end_cam = intersect_pt;
      } else {
        // drop this angle, doesn't intersect viewable section
        if(print_now)
          pprintf(TextOutputStream,"dropping angle %g because end fails to be in front of camera\n",angle_cur);
        continue;
      }
    }

    if(print_now && debug_verbose)
      pprintf(TextOutputStream,"ssc (%g,%g,%g) sec (%g,%g,%g)\n",
              scan_start_cam.x,scan_start_cam.y,scan_start_cam.z,
              scan_end_cam.x,scan_end_cam.y,scan_end_cam.z);

    vector2d scan_start_img_r,scan_end_img_r;
    GVector::vector2d<int> scan_start_img,scan_end_img;

    scan_start_img_r = cameraToImg(scan_start_cam);
    scan_end_img_r   = cameraToImg(scan_end_cam  );

    if(print_now && debug_verbose)
      pprintf(TextOutputStream,"ssc (%g,%g,%g) sec (%g,%g,%g), scan_start_img_r (%g,%g) scan_end_img_r (%g,%g)\n",
              scan_start_cam.x,scan_start_cam.y,scan_start_cam.z,
              scan_end_cam.x,scan_end_cam.y,scan_end_cam.z,
              scan_start_img_r.x,scan_start_img_r.y,
              scan_end_img_r  .x,scan_end_img_r  .y);

    if(GVector::clip_ray(scan_start_img_r,scan_end_img_r  ,0,width-1,0,height-1)) {
      if(print_now && debug_frustum_intersect)
        pprintf(TextOutputStream,"after clip scan_start_img_r (%g,%g) scan_end_img_r (%g,%g)\n",
                scan_start_img_r.x,scan_start_img_r.y,
                scan_end_img_r  .x,scan_end_img_r  .y);

      scan_start_img.set((int)(scan_start_img_r.x + .5),(int)(scan_start_img_r.y + .5));
      scan_end_img  .set((int)(scan_end_img_r  .x + .5),(int)(scan_end_img_r  .y + .5));
    } else {
      if(print_now && debug_frustum_intersect)
        pprintf(TextOutputStream,"clipping failed, angle not visible, skipping\n");
      continue;
    }

    scan_start_img = scan_start_img;
    scan_end_img   = scan_end_img;

    if(print_now && debug_verbose) {
      pprintf(TextOutputStream,"scan img start (%d,%d) r=(%g,%g), end (%d,%d) r=(%g,%g)\n",
              scan_start_img.x,scan_start_img.y,
              scan_start_img_r.x,scan_start_img_r.y,
              scan_end_img.x,scan_end_img.y,
              scan_end_img_r.x,scan_end_img_r.y);
    }

    if(!onImage(scan_start_img.x,scan_start_img.y)) {
      if(print_now)
        pprintf(TextOutputStream,"###Scan starts off image at (%d,%d)!\n",
                scan_start_img.x,scan_start_img.y);
      clipToImage(scan_start_img.x,scan_start_img.y);
    }
    if(!onImage(scan_end_img.x,scan_end_img.y)) {
      if(print_now)
        pprintf(TextOutputStream,"###Scan ends off image at (%d,%d)!\n",
                scan_end_img.x,scan_end_img.y);
      clipToImage(scan_end_img.x,scan_end_img.y);
    }
#ifdef PLATFORM_LINUX
#ifndef VIS_NO_LIBS
    if(DrawRadialLines) {
      rgb c;
      c.set(255,0,255);
      if(DebugImage) DebugImage->draw_line(scan_start_img.x,scan_start_img.y,
                                           scan_end_img.x,  scan_end_img.y,
                                           c);
    }
#endif
#endif

    AddRadialObject(scan_start_img.x,scan_start_img.y,&radial_map,ROBJECT_VISIBLE_START,angle_cur_idx);
    AddRadialObject(scan_end_img.x,  scan_end_img.y,  &radial_map,ROBJECT_VISIBLE_END,  angle_cur_idx);

    // Update radial map for this angle
    UpdateRadialAngle(angle_cur_idx,angle_cur,scan_start_img,scan_end_img);
  }

  if(print_now) {
    pprintf(TextOutputStream,"%17s %1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s,"
            " %1d %13s, %1d %13s, %1d %13s, %1d %13s, %1d %13s\n","",
            0,"VisStart",1,"VisEnd",2,"ClearStart",3,"ClearEnd",4,"Wall",5,"Robot",
            6,"RedRobot",7,"BlueRobot",8,"Ball",9,"CyanGoal",10,"YellowGoal",11,"Stripe");
    for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++) {
      pprintf(TextOutputStream,"ang %5.2f idx %3d ",RadialObjectMap::idxToAngle(ang_idx),ang_idx);
      for(int obj_type=0; obj_type<NUM_ROBJECTS; obj_type++) {
        double dist = radial_map.robjects[obj_type][ang_idx].distance;
        if(dist < HUGE_VAL && dist > 1e48)
          pprintf(TextOutputStream,"c%4.2f d%8s, ",
                  radial_map.robjects[obj_type][ang_idx].confidence,
                  "BIG");
        else
          pprintf(TextOutputStream,"c%4.2f d%8.f, ",
                  radial_map.robjects[obj_type][ang_idx].confidence,
                  dist);
      }
      pprintf(TextOutputStream,"\n");
    }
  }
}