/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#ifndef INCLUDED_LocalModel_h
#define INCLUDED_LocalModel_h

#include "../headers/system_config.h"

#ifdef PLATFORM_LINUX
#include <stdio.h>
#endif

#include <map>

using std::map;

#include "../headers/DogTypes.h"
#include "../headers/fast_alloc.h"
#include "../headers/Geometry.h"
#include "../headers/Utility.h"
#include "../Vision/VisionInterface.h"
#include "spatial_tree.h"

#define PROC_TEMP template<class Processor>
#define PROC_ARGS Processor

namespace Motion {
  class MotionLocalizationUpdate;
};

namespace VisionInterface {
  class RadialObjectMap;
};

namespace LocalModel {
  class MapPoint {
  public:
    vector2f pos;
    int obj_id;
    ulong timestamp;

    // used by SpatialTree when in tree
    MapPoint *next;
  };

  // stores map point relative to robot
  class RelPoint {
  public:
    vector2f loc;
    MapPoint *map_pt;
  };

#ifdef PLATFORM_LINUX
  struct STDumpProcessor {
    FILE *file;

    void add_loc(MapPoint *loc);
  };

  struct DumpProcessor {
    FILE *file;

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

  PROC_TEMP
  struct TranslateProcessor {
    Processor *proc;
    vector2f robot_loc; // position of neck
    vector2f robot_heading;

    TranslateProcessor(Processor *_proc,vector2f _robot_loc,vector2f _robot_heading) :
      proc(_proc),
      robot_loc(_robot_loc),
      robot_heading(_robot_heading)
    {}

    void add_loc(MapPoint *pt);
  };

  struct SeparatorEntry {
    ulong time; // time at which this entry was last updated

    vector2f plane_n; // plane normal
    float orig_dist; // distance of plane from origin
  };

  // state of the occ grid entry being constructed
  class SeparatorEntryBuildState {
  private:
    struct SepPt {
      static const uchar LEFT  = 1;
      static const uchar RIGHT = 2;

      vector2f loc;
      ulong time;
      bool obs;
      uchar sides;
    };

    static const uchar NONE  = 0;
    static const uchar OBS   = 1;
    static const uchar CLEAR = 2;
    static const uchar BOTH  = OBS | CLEAR;

    typedef std::multimap<ulong,SepPt> SepPtMap;

    ulong cur_time;
    SepPtMap obs_pts;
    SepPtMap clear_pts;
    bool debug_prints_separator;

    void filterBehind(vector2f ref_pt);

  public:
    static const float SamplingFrac = 1.5; // number of free cells per free point added

    void init(ulong _cur_time,bool debug);
    void add(const MapPoint *map_pt,vector2f loc);
    void calc(SeparatorEntry *entry,vector2f ego_basis,vector2f ego_center,vector2f range);
  };

  struct OccGridEntry {
    static const float ObsPresentThreshold = 0.3; // obs greater than this is occupied
    static const float KnownThreshold = 0.0; // evidence greater than this is seen

    ulong time; // time at which this entry was last updated

    // obstacle information
    float obs; // [0.0,1.0]; 0.0 = clear, 1.0 = obstacle
    float evidence; // amount of evidence for this conclusion [0.0,???], 0.0=no evidence
    
    // object information
    float wall_conf;
    float unknown_obs_conf;
    float red_robot_conf;
    float blue_robot_conf;
    float ball_conf;
    float cyan_goal_conf;
    float yellow_goal_conf;
    float stripe_conf;
  };

  // state of the occ grid entry being constructed
  class OccGridEntryBuildState {
  private:
    static const uchar NONE  = 0;
    static const uchar OBS   = 1;
    static const uchar CLEAR = 2;
    static const uchar BOTH  = OBS | CLEAR;

    double values[VisionInterface::NUM_ROBJECTS];
    map<ulong,uchar> seen_state;
    map<ulong,int> clear_cnt;
    ulong cur_time;
    bool debug_prints_occ_grid;
  public:
    static const float SamplingFrac = 1.5; // number of free cells per free point added

    void init(ulong _cur_time,bool debug);
    void add(const MapPoint *map_pt,vector2f loc);
    void calc(OccGridEntry *cell, bool black_obs=false);
  };

  // occupancy grid
  struct OccGrid {
  public:
    static const float CellSize = 200.0;
    static const float RangeX = 2000.0; // half range
    static const float RangeY = RangeX - CellSize/2.0;

    // 0,0 is in lower left corner (i.e. behind and to right of robot)
    static const int EntriesHoriz=(int)(2.0*RangeX/CellSize+.5);
    static const int EntriesVert =(int)(2.0*RangeY/CellSize+.5);
    static const int EntryCenterX=EntriesHoriz/2; // x coordinate of square immediately in front of robot
    static const int EntryCenterY=EntriesVert /2; // y coordinate of square immediately in front of robot

    // +y axis points to left of robot

    OccGridEntryBuildState grid_build[EntriesHoriz*EntriesVert];
    OccGridEntry grid[EntriesHoriz][EntriesVert];
  };

  class LModel {
  private:
    static const ulong MapLifespan       = 4*1000*1000UL; // 4 sec
    static const int NumMaps = 4;
    static const ulong MapGenerationTime = ((MapLifespan+NumMaps-1) / NumMaps);

    static const int NumPtsPerSec  = 7*10*25; // max number of points per second to allow
    static const int PtStorageSize = NumPtsPerSec * (MapLifespan/(1000*1000));

    static const float BaseRange = 2000.0;

    static const float SamplingFrac = OccGridEntryBuildState::SamplingFrac; // number of free cells per free point added

    typedef SpatialTree<MapPoint> MapTree;

    MapTree maps[NumMaps];
    ulong birth_time[NumMaps];
    int cur_map;
    ulong cur_map_birth_time;
    ulong cur_time;

    vector2f robot_loc;
    vector2f robot_body_loc;
    vector2f robot_heading;

    MapPoint pt_storage[PtStorageSize];
    // the current valid range of points is [start_storage_loc,end_storage_loc)
    // the index of the first valid point
    int start_storage_loc;
    // the index of the next pt_storage index to store into, i.e. the first invalid pt
    int end_storage_loc;

    OccGrid occ_grid;

    // for debugging
    bool debug_prints_occ_grid;
    bool debug_prints_separator;

    int increment_storage_loc(int storage_loc)
    { return (storage_loc+1)%PtStorageSize; }

    void perform_map_rotation(ulong cur_time);

#ifdef PLATFORM_LINUX
    const char *make_debug_filename();
    void dump_map();
#endif

    void calc_occ_grid_cell(OccGridEntry *cell,double x1,double y1,double x2,double y2);
    void create_occ_grid();

  public:
    LModel();

    void init();

    void process_motion_update(const Motion::MotionLocalizationUpdate *mot_update,ulong timestamp);
    void process_vision_update(const VisionInterface::RadialObjectMap *vis_map,ulong timestamp);
    
    // general query interface
    // basis - unit vector in x direction relative to robot
    // center - center of query relative to robot
    // range - major,minor size of query in basis reference frame
    PROC_TEMP
    void query_full(vector2f ego_basis,vector2f ego_center,vector2f range,Processor &proc);

    // easy robot centric interface for rectangles
    // minv - mininum values for robot relative bounding box
    // maxv - maximum values for robot relative bounding box
    PROC_TEMP
    void query_simple(vector2f ego_minv,vector2f ego_maxv,Processor &proc);

    void calc_separator(SeparatorEntry *entry,vector2f ego_basis,vector2f ego_center,vector2f range);

    void calc_occ_grid_cells(int x1,int y1,int x2,int y2);
    const OccGridEntry *get_occ_grid_cell(int x_cell,int y_cell);
    void calc_occupancy(OccGridEntry *cell,vector2f ego_basis,vector2f ego_center,vector2f range,bool black_obs=false);

    // for debugging
    // run the debugging code for this (x,y) location
    void debug_support(float x,float y);
  };

  PROC_TEMP
  void LModel::query_full(vector2f ego_basis,vector2f ego_center,vector2f range,Processor &proc)
  {
    TranslateProcessor<PROC_ARGS> trans_proc(&proc,robot_loc,robot_heading);

    vector2f basis;
    vector2f center;

    basis  = ego_basis.project(robot_heading);
    center = robot_loc + ego_center.project(robot_heading);
    
    for(int i=0; i<NumMaps; i++){
      int map_idx;
      map_idx = (cur_map-i + NumMaps)%NumMaps;

      maps[map_idx].query(basis,center,range,trans_proc);
    }
  }

  PROC_TEMP
  void LModel::query_simple(vector2f ego_minv,vector2f ego_maxv,Processor &proc)
  {
    vector2f ego_basis,ego_center,range;

    ego_basis.set(1.0,0.0);
    ego_center = (ego_minv + ego_maxv)/2.0;
    range = ego_maxv - ego_minv;

    query_full(ego_basis,ego_center,range,proc);
  }

  PROC_TEMP
  void TranslateProcessor<PROC_ARGS>::add_loc(MapPoint *pt)
  {
    vector2f loc;

    loc = pt->pos;
    loc -= robot_loc;
    loc = loc.rebase(robot_heading);

    proc->add_loc(pt,loc);
  }
};

#undef PROC_TEMP
#undef PROC_ARGS

#endif