/* 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 <map>
#include <set>

using namespace std;

#include "../headers/random.h"
#include "../headers/CircBufPacket.h"
#include "../Motion/MotionInterface.h"
#include "../Vision/VisionInterface.h"

#include "LocalModel.h"

using namespace Motion;
using namespace VisionInterface;

namespace LocalModel {

static const bool EnableLocalModel = true;

#ifdef PLATFORM_LINUX
static bool DumpDebugMaps=false;
#endif

LModel::LModel()
{
  debug_prints_occ_grid =false;
  debug_prints_separator=false;
}

void LModel::init()
{
  cur_map = 0;

  for(int map_idx=0; map_idx<NumMaps; map_idx++){
    maps[map_idx].setdim(vector2f(-1000.0,-1000.0),vector2f(vector2f(1000.0,1000.0)),10,32);
    birth_time[map_idx] = 0UL;
  }
  cur_map_birth_time = 0UL;
  cur_time = 0UL;

  start_storage_loc = 0;
  end_storage_loc   = 0;

  robot_body_loc.set(0.0,0.0);
  robot_loc.set(100.0,0.0);
  robot_heading.set(1.0,0.0);

  mzero(pt_storage);

  for(int x_cell=0; x_cell<OccGrid::EntriesHoriz; x_cell++){
    for(int y_cell=0; y_cell<OccGrid::EntriesVert; y_cell++){
      occ_grid.grid[x_cell][y_cell].obs      = 0.0;
      occ_grid.grid[x_cell][y_cell].evidence = 0.0;
      occ_grid.grid[x_cell][y_cell].time = 0UL;
    }
  }
}

void LModel::process_motion_update(const MotionLocalizationUpdate *mot_update,ulong timestamp)
{
  if(!EnableLocalModel)
    return;
  
  cur_time = timestamp;

  vector2d robot_body_loc_d;
  vector2d robot_heading_d;

  robot_heading_d.set(V2COMP(robot_heading));

  robot_body_loc_d.set(V2COMP(robot_body_loc));
  robot_body_loc_d += mot_update->pos_delta.project(robot_heading_d);
  robot_body_loc.set(V2COMP(robot_body_loc_d));

  if(mot_update->heading_delta.x!=0.0 || mot_update->heading_delta.y!=0.0){
    vector2d new_heading;

    robot_heading_d.set(V2COMP(robot_heading));
    new_heading = mot_update->heading_delta.project(robot_heading_d);
    robot_heading.set(V2COMP(new_heading));
  }

  robot_loc = robot_body_loc + robot_heading * (float)mot_update->body.neck_offset.x;

  //printf("time=%lu\n",timestamp);
  //printf("mot update, dpos=(%g,%g) dhead=(%g,%g) robot_loc=(%g,%g) robot_head=(%g,%g)\n",
  //       V2COMP(mot_update->pos_delta),V2COMP(mot_update->heading_delta),
  //       V2COMP(robot_loc),V2COMP(robot_heading));
}

void LModel::perform_map_rotation(ulong cur_time)
{
  while(cur_time - cur_map_birth_time > MapGenerationTime){
    cur_map = (cur_map + 1) % NumMaps;
    cur_map_birth_time += MapGenerationTime;

    // delete unneeded points
    // FIXME: index these locations so the linear search is unnecessary
    ulong kill_end_time;
    kill_end_time = birth_time[(cur_map+1)%NumMaps];
    while(pt_storage[start_storage_loc].timestamp < kill_end_time &&
          start_storage_loc != end_storage_loc){
      start_storage_loc = increment_storage_loc(start_storage_loc);
    }

    //STDumpProcessor dump;
    //dump.file = stdout;
    //maps[cur_map].process_locs(dump);

    maps[cur_map].clear();
    maps[cur_map].setdim(robot_loc - vector2f(BaseRange,BaseRange),
                         robot_loc + vector2f(BaseRange,BaseRange),
                         10,32);

    birth_time[cur_map] = cur_map_birth_time;
  }
}

struct RelPointAddProcessor {
  RelPoint *pts;
  int next_pt;

  RelPointAddProcessor() {next_pt=0;}

  void add_loc(MapPoint *pt,vector2f loc);
};

void RelPointAddProcessor::add_loc(MapPoint *pt,vector2f loc)
{
  RelPoint *rel_pt;

  rel_pt = &pts[next_pt];
  rel_pt->loc = loc;
  rel_pt->map_pt = pt;
  next_pt++;
}

struct RPLessThan {
  bool operator()(const RelPoint &x,const RelPoint &y){
    if(x.map_pt->timestamp < y.map_pt->timestamp)
      return true;
    if(x.map_pt->timestamp > y.map_pt->timestamp)
      return false;

    if(x.map_pt->obj_id < y.map_pt->obj_id)
      return true;
    if(x.map_pt->obj_id > y.map_pt->obj_id)
      return false;

    return false;
  }
};

//============================================================================
// planar separator calculations

void SeparatorEntryBuildState::init(ulong _cur_time,bool debug)
{
  cur_time = _cur_time;
  debug_prints_separator = debug;

  obs_pts.clear();
  clear_pts.clear();
}

void SeparatorEntryBuildState::add(const MapPoint *map_pt,vector2f loc)
{
  ulong time;
  map<ulong,uchar>::iterator iter;
  bool obs=false,clear=false;

  time = map_pt->timestamp;

  if(cur_time - time > 3000000)
    return;

  switch(map_pt->obj_id){
    case ROBJECT_WALL:              obs=true;   break;
    case ROBJECT_ROBOT:             obs=true;   break;
    case ROBJECT_RED_ROBOT:         obs=true;   break;
    case ROBJECT_BLUE_ROBOT:        obs=true;   break;
    case ROBJECT_BALL:                          break;
    case ROBJECT_CYAN_GOAL:         obs=true;   break;
    case ROBJECT_YELLOW_GOAL:       obs=true;   break;
    case ROBJECT_STRIPE:                        break;
    case ROBJECT_FIELD_CLEAR_START: clear=true; break;
  }

  if(obs || clear){
    SepPt sep_pt;
    sep_pt.loc   = loc;
    sep_pt.time  = time;
    sep_pt.obs   = obs;
    sep_pt.sides = 0;

    if(obs)
      obs_pts.insert(make_pair(time,sep_pt));
    else
      clear_pts.insert(make_pair(time,sep_pt));
  }


  if(debug_prints_separator)
    pprintf(TextOutputStream,"time=%9lu loc=(%g,%g) obs=%d clear=%d\n",
            time,V2COMP(loc),obs,clear);
}

void SeparatorEntryBuildState::filterBehind(vector2f ref_pt)
{
  SepPtMap::iterator obs_time_iter_start,obs_time_iter_end;

  // mark clear points that are behind obstacles
  obs_time_iter_end = obs_pts.begin();
  while(obs_time_iter_end!=obs_pts.end()){
    obs_time_iter_start = obs_time_iter_end;

    ulong time = (*obs_time_iter_start).first;

    // find obstacles with the same timestamp
    for(obs_time_iter_end=obs_time_iter_start; obs_time_iter_end!=obs_pts.end(); obs_time_iter_end++){
      if((*obs_time_iter_end).first != time)
        break;
    }

    SepPtMap::iterator obs_time_iter_cur;
    pair<SepPtMap::iterator,SepPtMap::iterator> clear_time_pair;
    SepPtMap::iterator clear_time_iter;

    // find clear points with the same timestamp
    //clear_time_pair = clear_pts.equal_range(time);
    clear_time_pair.first  = clear_pts.begin();
    clear_time_pair.second = clear_pts.upper_bound(time);

    // mark all combos of clear and obstacle
    for(obs_time_iter_cur=obs_time_iter_start; obs_time_iter_cur!=obs_time_iter_end; obs_time_iter_cur++){
      vector2f obs_loc;
      obs_loc = (*obs_time_iter_start).second.loc;
      for(clear_time_iter=clear_time_pair.first; clear_time_iter!=clear_time_pair.second; clear_time_iter++){
        vector2f clear_loc;
        ulong clear_time;
        clear_loc = (*clear_time_iter).second.loc;
        clear_time = (*clear_time_iter).first;

        if(debug_prints_separator)
          pprintf(TextOutputStream,"considering, obs_time=%lu obs_loc=(%g,%g), clear_time=%lu clear_loc=(%g,%g)\n",time,V2COMP(obs_loc),clear_time,V2COMP(clear_loc));

        // if clear point is closer than obstacle point, then keep
        if(GVector::sdistance(clear_loc,ref_pt) < GVector::sdistance(obs_loc,ref_pt))
          continue;

        // mark whether this clear point is to left or right of obstacle point
        if(obs_loc.cross(clear_loc) < 0){
          (*clear_time_iter).second.sides |= SepPt::LEFT;
          if(debug_prints_separator)
            pprintf(TextOutputStream,"marking clear at (%g,%g), time %lu with LEFT, new sides %u\n",V2COMP(clear_loc),time,(*clear_time_iter).second.sides);
        }else{
          (*clear_time_iter).second.sides |= SepPt::RIGHT;
          if(debug_prints_separator)
            pprintf(TextOutputStream,"marking clear at (%g,%g), time %lu with RIGHT, new sides %u\n",V2COMP(clear_loc),time,(*clear_time_iter).second.sides);
        }
      }
    }
  }

  // erase clear points that are marked both left and right of obstacles
  SepPtMap::iterator clear_time_iter;
  for(clear_time_iter=clear_pts.begin(); clear_time_iter!=clear_pts.end(); ){
    if(debug_prints_separator)
      pprintf(TextOutputStream,"clear at (%g,%g), time %lu has sides %u\n",V2COMP((*clear_time_iter).second.loc),(*clear_time_iter).second.time,(*clear_time_iter).second.sides);
    if((*clear_time_iter).second.sides == (SepPt::LEFT | SepPt::RIGHT)){
      SepPtMap::iterator clear_time_iter2;
      clear_time_iter2 = clear_time_iter;
      clear_time_iter2++;

      if(debug_prints_separator)
        pprintf(TextOutputStream,"erasing clear pt at (%g,%g), time %lu\n",
                V2COMP((*clear_time_iter).second.loc),(*clear_time_iter).second.time);

      clear_pts.erase(clear_time_iter);

      clear_time_iter = clear_time_iter2;
    }else{
      clear_time_iter++;
    }
  }
}

void SeparatorEntryBuildState::calc(SeparatorEntry *entry,vector2f ego_basis,vector2f ego_center,vector2f range)
{
  // calculate location of linear separator

  vector2f c_orig[4];
  vector2f c[4]; // c[0] is a closest corner, c[2] is a farthest corner opposite c[0]
  int close_idx;

  c_orig[0] = ego_center + ego_basis*range.x + ego_basis.perp()*range.y;
  c_orig[1] = ego_center + ego_basis*range.x - ego_basis.perp()*range.y;
  c_orig[2] = ego_center - ego_basis*range.x - ego_basis.perp()*range.y;
  c_orig[3] = ego_center - ego_basis*range.x + ego_basis.perp()*range.y;

  // ensure c[0] is closest while preserving rotational order of corners
  close_idx=0;
  for(int i=1; i<4; i++){
    if(c_orig[i].sqlength() < c_orig[close_idx].sqlength())
      close_idx=i;
  }
  for(int i=0; i<4; i++){
    c[i] = c_orig[(i+close_idx)%4];
  }

  // calculate splitting plane
  vector2f &close_corner=c[0];
  vector2f &far_corner  =c[2];
  vector2f center;
  vector2f plane_n(1.0,0.0);
  float orig_dist=0.0;

  center = (far_corner + close_corner)/2.0;

  bool splitting_plane_valid=false;
  float best_goodness=-HUGE_VAL;

  static const int num_angles = 16;
  for(int angle_idx=0; angle_idx<num_angles; angle_idx++){
    float angle;
    vector2f cand_plane_n;

    angle = (2*M_PI*angle_idx)/num_angles;
    cand_plane_n.set(cos(angle),sin(angle));

    if(debug_prints_separator)
      pprintf(TextOutputStream,"processing angle_idx=%d angle=%g heading=(%g,%g)\n",
              angle_idx,angle,V2COMP(cand_plane_n));

    SepPtMap::iterator obs_iter;
    float cand_orig_dist;

    // calculate splitting plane
    cand_orig_dist = cand_plane_n.dot(far_corner);
    for(obs_iter=obs_pts.begin(); obs_iter!=obs_pts.end(); obs_iter++){
      vector2f loc;
      float val;
  
      loc  = (*obs_iter).second.loc;
      val = loc.dot(cand_plane_n);
  
      if(debug_prints_separator)
        pprintf(TextOutputStream,"processing loc=(%g,%g) cand_plane_n=(%g,%g) new_orig_dist=%g\n",
                V2COMP(loc),V2COMP(cand_plane_n),val);
  
      if(val < cand_orig_dist){
        cand_orig_dist = val;
      }
    }

    // throw out planes on the wrong side of the origin
    if(cand_orig_dist <= 0.0){
      if(debug_prints_separator)
        pprintf(TextOutputStream,"throwing out plane with heading=(%g,%g) and orig_dist=%g\n",V2COMP(cand_plane_n),cand_orig_dist);
      continue;
    }

    // calculate approx stand in for free area for this splitting plane
    float cand_goodness;
    cand_goodness = (cand_orig_dist - cand_plane_n.dot(center));

    if(debug_prints_separator)
      pprintf(TextOutputStream,"plane with heading=(%g,%g) has goodness=%g\n",V2COMP(cand_plane_n),cand_goodness);
    if(!splitting_plane_valid || cand_goodness>best_goodness){
      if(debug_prints_separator)
        pprintf(TextOutputStream,"replacing best plane with plane: heading=(%g,%g) orig_dist=%g\n",V2COMP(cand_plane_n),cand_orig_dist);

      splitting_plane_valid = true;
      best_goodness = cand_goodness;

      plane_n = cand_plane_n;
      orig_dist = cand_orig_dist;
    }
  }

  if(!splitting_plane_valid)
    pprintf(TextOutputStream,"No valid splitting plane found\n");

  if(debug_prints_separator)
    pprintf(TextOutputStream,"final plane_n=(%g,%g) orig_dist=%g\n",
            V2COMP(plane_n),orig_dist);

  entry->time = cur_time;
  entry->plane_n = plane_n;
  entry->orig_dist = orig_dist;

  if(debug_prints_separator)
    pprintf(TextOutputStream,"separator calc result, time=%lu plane_n=(%g,%g) orig_dist=%g\n",
            entry->time,V2COMP(entry->plane_n),entry->orig_dist);
}

struct SeparatorEntryBuilder {
  SeparatorEntryBuildState *state;

  void add_loc(MapPoint *pt,vector2f loc);
};

void SeparatorEntryBuilder::add_loc(MapPoint *pt,vector2f loc)
{
  state->add(pt,loc);
}

void LModel::calc_separator(SeparatorEntry *cell,vector2f ego_basis,vector2f ego_center,vector2f range)
{
  SeparatorEntryBuilder build_proc;
  SeparatorEntryBuildState sebs;

  sebs.init(cur_time,debug_prints_separator);

  build_proc.state = &sebs;
  query_full(ego_basis,ego_center,range,build_proc);

  sebs.calc(cell,ego_basis,ego_center,range);
}

//============================================================================
// occupancy grid calculations

void OccGridEntryBuildState::init(ulong _cur_time,bool debug)
{
  cur_time = _cur_time;
  debug_prints_occ_grid = debug;

  for(int robject_idx=0; robject_idx<NUM_ROBJECTS; robject_idx++){
    values[robject_idx] = 0.0;
  }

  seen_state.clear();
  clear_cnt.clear();
}

void OccGridEntryBuildState::add(const MapPoint *map_pt,vector2f loc)
{
  ulong time;
  map<ulong,uchar>::iterator iter;
  bool obs=false,clear=false;

  time = map_pt->timestamp;

  if(cur_time - time > 3000000)
    return;

  double time_val;
  time_val = pow(.1,((double)(cur_time - time)/1.0e6));

  uchar time_seen_state = NONE;
  iter = seen_state.find(time);
  if(iter != seen_state.end())
    time_seen_state = (*iter).second;

  switch(map_pt->obj_id){
    case ROBJECT_WALL:              obs=true;   break;
    case ROBJECT_ROBOT:             obs=true;   break;
    case ROBJECT_RED_ROBOT:         obs=true;   break;
    case ROBJECT_BLUE_ROBOT:        obs=true;   break;
    case ROBJECT_BALL:                          break;
    case ROBJECT_CYAN_GOAL:         obs=true;   break;
    case ROBJECT_YELLOW_GOAL:       obs=true;   break;
    case ROBJECT_STRIPE:                        break;
    case ROBJECT_FIELD_CLEAR_START: clear=true; break;
  }

  if( ! ((time_seen_state & OBS)!=0 && clear)){
    values[map_pt->obj_id] += time_val;
    
    if(clear){
      map<ulong,int>::iterator clear_cnt_iter;
      clear_cnt_iter = clear_cnt.find(time);
      if(clear_cnt_iter != clear_cnt.end())
        (*clear_cnt_iter).second++;
      else
        clear_cnt.insert(make_pair(time,1));
    }
  }

  // update seen_state
  if(obs && ((time_seen_state & OBS)==0)){
    if((time_seen_state & CLEAR)!=0){
      int cnt;
      map<ulong,int>::iterator clear_cnt_iter;
      clear_cnt_iter = clear_cnt.find(time);
      // should always be true
      if(clear_cnt_iter != clear_cnt.end()){
        cnt = (*clear_cnt_iter).second;
        values[ROBJECT_FIELD_CLEAR_START] -= time_val * cnt;
        clear_cnt.erase(clear_cnt_iter);
      }
    }
    time_seen_state |= OBS;
    if(iter != seen_state.end())
      (*iter).second = time_seen_state;
    else
      seen_state.insert(make_pair(time,time_seen_state));
  }else if(clear && ((time_seen_state & CLEAR)==0)){
    time_seen_state |= CLEAR;
    if(iter != seen_state.end())
      (*iter).second = time_seen_state;
    else
      seen_state.insert(make_pair(time,time_seen_state));
  }

  if(debug_prints_occ_grid)
    pprintf(TextOutputStream,"time=%9lu obj_id=%02d time_val=%8.5f val=%8.5f\n",
            time,map_pt->obj_id,time_val,values[map_pt->obj_id]);
}

void OccGridEntryBuildState::calc(OccGridEntry *cell, bool black_obs)
{
  // calculate whether obstacle is present

  double obs_total;
  double obs_confuse_total;
  double clear_total;
  double complete_total;

  obs_total = 0.0;
  clear_total = 0.0;
  obs_confuse_total = 0.0;

  for(int robject_idx=0; robject_idx<NUM_ROBJECTS; robject_idx++){
    double val;
    val = values[robject_idx];
    switch(robject_idx){
      case ROBJECT_WALL:              obs_total         += val; break;
      case ROBJECT_ROBOT:             if(black_obs) 
	                              obs_total         += val; break;
      case ROBJECT_RED_ROBOT:         obs_total         += val; break;
      case ROBJECT_BLUE_ROBOT:        obs_total         += val; break;
      case ROBJECT_BALL:              obs_confuse_total += val; break;
      case ROBJECT_CYAN_GOAL:         obs_total         += val; break;
      case ROBJECT_YELLOW_GOAL:       obs_total         += val; break;
      case ROBJECT_STRIPE:            obs_confuse_total += val; break;
      case ROBJECT_FIELD_CLEAR_START: clear_total       += val; break;
    }
  }
  clear_total *= SamplingFrac;

  complete_total = obs_total + clear_total + obs_confuse_total;

  double obsness;
  double obs_final;
  double evidence;
  obsness = obs_total - .0*obs_confuse_total;
  if(obsness < 0.0)
    obsness = 0.0;
  evidence = clear_total + obsness;
  if(evidence > 0.0)
    obs_final = obsness/evidence;
  else
    obs_final = 0.0;
  cell->obs      = obs_final;
  cell->evidence = evidence;
  cell->time     = cur_time;

  // calculate object information
  if(complete_total == 0.0) // don't divide by 0.0
    complete_total = 1.0;
  cell->wall_conf        = values[ROBJECT_WALL       ] / complete_total;
  cell->unknown_obs_conf = values[ROBJECT_ROBOT      ] / complete_total;
  cell->red_robot_conf   = values[ROBJECT_RED_ROBOT  ] / complete_total;
  cell->blue_robot_conf  = values[ROBJECT_BLUE_ROBOT ] / complete_total;
  cell->ball_conf        = values[ROBJECT_BALL       ] / complete_total;
  cell->cyan_goal_conf   = values[ROBJECT_CYAN_GOAL  ] / complete_total;
  cell->yellow_goal_conf = values[ROBJECT_YELLOW_GOAL] / complete_total;
  double non_clear_total;
  non_clear_total = complete_total - clear_total;
  if(non_clear_total == 0.0) // don't divide by 0.0
    non_clear_total = 1.0;
  cell->stripe_conf      = values[ROBJECT_STRIPE     ] / non_clear_total;

  if(debug_prints_occ_grid)
    pprintf(TextOutputStream,"clear=%g obs=%g obs_confuse=%g, obsness=%g, obstacle=%g%%\n",
            clear_total,obs_total,obs_confuse_total,obsness,100.0*obs_final);
}

struct OccGridEntryBuilder {
  OccGridEntryBuildState *state;

  void add_loc(MapPoint *pt,vector2f loc);
};

void OccGridEntryBuilder::add_loc(MapPoint *pt,vector2f loc)
{
  state->add(pt,loc);
}

void LModel::calc_occ_grid_cell(OccGridEntry *cell,double x1,double y1,double x2,double y2)
{
  OccGridEntryBuilder build_proc;
  OccGridEntryBuildState ogebs;

  ogebs.init(cur_time,debug_prints_occ_grid);

  build_proc.state = &ogebs;
  query_simple(vector2f(x1,y1),vector2f(x2,y2),build_proc);  

  ogebs.calc(cell);
}

void LModel::calc_occupancy(OccGridEntry *cell,vector2f ego_basis,vector2f ego_center,vector2f range,bool black_obs)
{
  OccGridEntryBuilder build_proc;
  OccGridEntryBuildState ogebs;

  ogebs.init(cur_time,debug_prints_occ_grid);

  build_proc.state = &ogebs;
  query_full(ego_basis,ego_center,range,build_proc);  

  ogebs.calc(cell,black_obs);
}

struct OccGridBuilder {
  OccGridEntryBuildState *states;
  int x1,y1,x2,y2;

  void add_loc(MapPoint *pt,vector2f loc);
};

void OccGridBuilder::add_loc(MapPoint *pt,vector2f loc)
{
  float x,y;
  int x_cell,y_cell;

  x = loc.x;
  y = loc.y;

  x_cell = (int)floor( x                       /OccGrid::CellSize) + OccGrid::EntryCenterX;
  x_cell = bound(x_cell,x1,x2); // shouldn't happen
  y_cell = (int)floor((y+OccGrid::CellSize/2.0)/OccGrid::CellSize) + OccGrid::EntryCenterY;
  y_cell = bound(y_cell,y1,y2); // shouldn't happen

  states[x_cell*OccGrid::EntriesVert+y_cell].add(pt,loc);
}

void LModel::calc_occ_grid_cells(int x1,int y1,int x2,int y2)
{
  for(int x_cell=x1; x_cell<=x2; x_cell++){
    for(int y_cell=y1; y_cell<=y2; y_cell++){
      occ_grid.grid_build[x_cell*OccGrid::EntriesVert+y_cell].init(cur_time,false);
    }
  }

  OccGridBuilder build_proc;
  build_proc.states = occ_grid.grid_build;
  build_proc.x1 = x1;
  build_proc.y1 = y1;
  build_proc.x2 = x2;
  build_proc.y2 = y2;

  float xb1,xb2,yb1,yb2;
  xb1 = OccGrid::CellSize * (x1     - OccGrid::EntryCenterX);
  xb2 = OccGrid::CellSize * (x2 + 1 - OccGrid::EntryCenterX);
  yb1 = OccGrid::CellSize * (y1     - OccGrid::EntryCenterY) - OccGrid::CellSize/2.0;
  yb2 = OccGrid::CellSize * (y2 + 1 - OccGrid::EntryCenterY) - OccGrid::CellSize/2.0;
  query_simple(vector2f(xb1,yb1),vector2f(xb2,yb2),build_proc);  

  for(int x_cell=x1; x_cell<=x2; x_cell++){
    for(int y_cell=y1; y_cell<=y2; y_cell++){
      OccGridEntry *cell = &occ_grid.grid[x_cell][y_cell];
      occ_grid.grid_build[x_cell*OccGrid::EntriesVert+y_cell].calc(cell);
    }
  }
}

const OccGridEntry *LModel::get_occ_grid_cell(int x_cell,int y_cell)
{
  OccGridEntry *cell;

  cell = &occ_grid.grid[x_cell][y_cell];
  if(cell->time != cur_time){
    double x1,x2,y1,y2;
    x1 = OccGrid::CellSize*(x_cell - OccGrid::EntryCenterX    );
    x2 = OccGrid::CellSize*(x_cell - OccGrid::EntryCenterX + 1);
    y1 = OccGrid::CellSize*(y_cell - OccGrid::EntryCenterY    )-OccGrid::CellSize/2.0;
    y2 = OccGrid::CellSize*(y_cell - OccGrid::EntryCenterY + 1)-OccGrid::CellSize/2.0;

    calc_occ_grid_cell(cell,x1,y1,x2,y2);
  }

  return cell;
}

void LModel::debug_support(float x,float y)
{
#ifdef PLATFORM_LINUX
  int x_cell,y_cell;

  x_cell = (int)floor( x                       /OccGrid::CellSize) + OccGrid::EntryCenterX;
  y_cell = (int)floor((y+OccGrid::CellSize/2.0)/OccGrid::CellSize) + OccGrid::EntryCenterY;

  printf("x_cell=%02d y_cell=%02d\n",x_cell,y_cell);

  double x1,x2,y1,y2;
  x1 = OccGrid::CellSize*(x_cell - OccGrid::EntryCenterX    );
  x2 = OccGrid::CellSize*(x_cell - OccGrid::EntryCenterX + 1);
  y1 = OccGrid::CellSize*(y_cell - OccGrid::EntryCenterY    )-OccGrid::CellSize/2.0;
  y2 = OccGrid::CellSize*(y_cell - OccGrid::EntryCenterY + 1)-OccGrid::CellSize/2.0;

  //bool old_val=debug_prints_occ_grid;
  //debug_prints_occ_grid=true;
  //calc_occ_grid_cell(&occ_grid.grid[x_cell][y_cell],x1,y1,x2,y2);
  //debug_prints_occ_grid=old_val;

  {
    bool old_val=debug_prints_separator;
    debug_prints_separator=true;
    SeparatorEntry entry;
    calc_separator(&entry,vector2f(1.0,0.0),vector2f(x1+x2,y1+y2)/2.0,vector2f(OccGrid::CellSize,OccGrid::CellSize));
    debug_prints_separator=old_val;
  }
#endif
}

void LModel::process_vision_update(const RadialObjectMap *vis_map,ulong timestamp)
{
  if(!EnableLocalModel)
    return;
  
  cur_time = timestamp;

  static ulong add_cnt=0UL;
  static ulong add_cnt_clear=0UL;

  MapPoint *pt;
  int ang_idx;
  double angle;

  perform_map_rotation(timestamp);

  static bool ang_dirs_done=false;
  static vector2f ang_dirs[NUM_ANGLES];

  // cache direction vectors for different angles
  if(!ang_dirs_done){
    for(ang_idx=0, angle=RadialObjectMap::idxToAngle(0); ang_idx<NUM_ANGLES; ang_idx++, angle+=RadialAngleSpacing){
      ang_dirs[ang_idx].set(cos(angle),sin(angle));
    }
    ang_dirs_done = true;
  }
  
  // add clear area to model
  for(ang_idx=0, angle=RadialObjectMap::idxToAngle(0); ang_idx<NUM_ANGLES; ang_idx++, angle+=RadialAngleSpacing){
    const RadialObjectReading *reading1,*reading2;
    vector2f ang_dir;

    reading1 = &vis_map->robjects[ROBJECT_FIELD_CLEAR_START][ang_idx];
    reading2 = &vis_map->robjects[ROBJECT_FIELD_CLEAR_END  ][ang_idx];
    if(reading1->confidence < 0.5 || reading2->confidence < 0.5)
      continue;

    ang_dir = ang_dirs[ang_idx];
    
    static const float DistSep = SamplingFrac*OccGrid::CellSize;
    static const float MaxFieldDist = 2.0e3;

    float min_dist,max_dist;
    min_dist = reading1->distance;
    max_dist = reading2->distance;
    if(max_dist > MaxFieldDist)
      max_dist = MaxFieldDist;

    bool done=false;
    bool small_step=true;
    float dist=min_dist;
    if(GlobalRandom.uniform0to1() < 0.67){
      dist += GlobalRandom.uniform0to1()*DistSep;
      small_step = false;
    }
    while(!done){
      vector2f loc;
      loc = ang_dir;
      loc *= dist;
      
      loc = loc.project(robot_heading);
      loc += robot_loc;

      //printf("adding field clear at loc (%g,%g) dist %g based on readings %g and %g\n",
      //       V2COMP(loc),dist,reading1->distance,reading2->distance);
      
      pt = &pt_storage[end_storage_loc];
      end_storage_loc = increment_storage_loc(end_storage_loc);
      pt->pos = loc;
      pt->obj_id = ROBJECT_FIELD_CLEAR_START;
      pt->timestamp = timestamp;
      maps[cur_map].add(pt);
      
      add_cnt++;
      add_cnt_clear++;

      done = (dist==max_dist);
      if(small_step){
        small_step = false;
        dist += GlobalRandom.uniform0to1()*DistSep;
      }else{
        dist += DistSep;
      }
      if(dist > max_dist){
        dist = max_dist;
        done = true;
      }
    }
  }

  // add all observations to model
  for(int obj_idx=ROBJECT_WALL; obj_idx<NUM_ROBJECTS; obj_idx++){
    double angle;
    int ang_idx;
    for(ang_idx=0, angle=RadialObjectMap::idxToAngle(0); ang_idx<NUM_ANGLES; ang_idx++, angle+=RadialAngleSpacing){
      const RadialObjectReading *reading;

      reading = &vis_map->robjects[obj_idx][ang_idx];
      // if don't see it or it is farther than 2m, ignore it
      if(reading->confidence < 0.5 || reading->distance>2.0e3)
        continue;

      vector2f loc;
      loc = ang_dirs[ang_idx];
      loc *= reading->distance;

      loc = loc.project(robot_heading);
      loc += robot_loc;

      pt = &pt_storage[end_storage_loc];
      end_storage_loc = increment_storage_loc(end_storage_loc);
      pt->pos = loc;
      pt->obj_id = obj_idx;
      pt->timestamp = timestamp;
      maps[cur_map].add(pt);

      add_cnt++;

      //printf("adding object, idx=%d ang_idx=%d angle=%g dist=%g loc=(%g,%g)\n",
      //       obj_idx,ang_idx,angle,reading->distance,V2COMP(loc));
    }
  }

#ifdef PLATFORM_LINUX
  printf("add count %lu, count clear %lu\n",add_cnt,add_cnt_clear);
#endif

#ifdef PLATFORM_LINUX
  if(DumpDebugMaps){
    dump_map();

    //STDumpProcessor dump;
    //vector2f center;

    //dump.file = stdout;
    //printf("robot_loc=(%g,%g) robot_heading=(%g,%g)\n",V2COMP(robot_loc),V2COMP(robot_heading));
    //center = robot_loc + robot_heading*100.0;
    //maps[cur_map].query(robot_heading,center,vector2f(200.0,100.0),dump);
  }
#endif
}

#ifdef PLATFORM_LINUX
const char *LModel::make_debug_filename()
{
  static char debug_filename[256];
  static int next_idx=0;

  sprintf(debug_filename,"map_data/m%04d.dat",next_idx);
  next_idx++;

  return debug_filename;
}

void LModel::dump_map()
{
  const char *dump_filename;
  FILE *dump_file;
  STDumpProcessor proc;

  dump_filename = make_debug_filename();
  dump_file = fopen(dump_filename,"w");

  proc.file = dump_file;
  maps[cur_map].process_locs(proc);

  fclose(dump_file);
}

void DumpProcessor::add_loc(MapPoint *pt,vector2f loc)
{
  fprintf(file,"%lu %d %g %g\n",pt->timestamp,pt->obj_id,V2COMP(loc));
}

void STDumpProcessor::add_loc(MapPoint *loc)
{
  fprintf(file,"%lu %d %g %g\n",loc->timestamp,loc->obj_id,V2COMP(loc->pos));
}
#endif

};