/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+
 */

#include <mrpt/core/round.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/kinematics/CVehicleSimul_DiffDriven.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/nav/reactive/CReactiveNavigationSystem3D.h>
#include <mrpt/nav/reactive/CRobot2NavInterfaceForSimulator.h>
#include <mrpt/opengl.h>
#include <mrpt/opengl/CPlanarLaserScan.h>
#include <mrpt/system/CObserver.h>

#include "map2_1.xpm"
#include "map2_2.xpm"
#include "map2_3.xpm"

using namespace mrpt;
using namespace mrpt::nav;
using namespace mrpt::opengl;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::gui;
using namespace mrpt::system;
using namespace mrpt::poses;
using namespace mrpt::kinematics;

class MyObserver : public mrpt::system::CObserver
{
 protected:
  void OnEvent(const mrptEvent& e)
  {
    if (e.isOfType<mrptEventMouseDown>())
    {
      mouse_click = 1;
    }
  }

 public:
  bool mouse_click;
};

struct TRobotLaser
{
  CObservation2DRangeScan m_scan;
  int m_level;
  int m_segments;
};

class CRobotKinects
{
 public:
  CSimplePointsMap m_points;
  float m_xrel;
  float m_yrel;
  float m_zrel;
  float m_phi;
  int m_level;
  float m_min_range;
  float m_max_range;
  float m_fov_v;
  float m_fov_h;
  float m_pitch_angle;
  float m_roll_angle;
  unsigned int m_rows;
  unsigned int m_columns;
  float m_std_error;

  void CorrectFloorPoints(const mrpt::poses::CPose3D& kinectrelpose)
  {
    using namespace mrpt::math;
    TSegment3D ray;
    TPoint3D p1, p2, pint(0, 0, 0);
    TObject3D pintobj;
    TPlane ground(0, 0, 1, 0);
    std::vector<float> x, y, z;

    m_points.getAllPoints(x, y, z, 1);
    p2.x = kinectrelpose[0];
    p2.y = kinectrelpose[1];
    p2.z = kinectrelpose[2];
    ray.point2 = p2;

    for (unsigned int i = 0; i < m_points.size(); i++)
    {
      if (z[i] < 0)
      {
        p1.x = x[i];
        p1.y = y[i];
        p1.z = z[i];
        ray.point1 = p1;
        intersect(ray, ground, pintobj);
        ASSERT_(pintobj.isPoint());
        pintobj.getPoint(pint);
        x[i] = pint.x;
        y[i] = pint.y;
        z[i] = pint.z;
      }
    }
    m_points.setAllPoints(x, y, z);
  }

  void CorrectCeiling(const mrpt::poses::CPose3D& kinectrelpose, float height)
  {
    using namespace mrpt::math;
    TSegment3D ray;
    TPoint3D p1, p2, pint(0, 0, 0);
    TObject3D pintobj;
    TPlane ceiling(0, 0, 1, -height);
    std::vector<float> x, y, z;

    m_points.getAllPoints(x, y, z, 1);

    p2.x = kinectrelpose[0];
    p2.y = kinectrelpose[1];
    p2.z = kinectrelpose[2];
    ray.point2 = p2;

    for (unsigned int i = 0; i < m_points.size(); i++)
    {
      if (z[i] > height)
      {
        p1.x = x[i];
        p1.y = y[i];
        p1.z = z[i];
        ray.point1 = p1;
        intersect(ray, ceiling, pintobj);
        ASSERT_(pintobj.isPoint());
        pintobj.getPoint(pint);
        x[i] = pint.x;
        y[i] = pint.y;
        z[i] = pint.z;
      }
    }
    m_points.setAllPoints(x, y, z);
  }

  void CorrectRanges(const mrpt::poses::CPose3D& kinectrelpose)
  {
    using namespace std;
    vector<float> x, y, z;
    vector<bool> deletion;

    m_points.getAllPoints(x, y, z, 1);

    for (unsigned int i = 0; i < m_points.size(); i++)
    {
      if ((kinectrelpose.distance3DTo(x[i], y[i], z[i]) < m_min_range) ||
          (kinectrelpose.distance3DTo(x[i], y[i], z[i]) > m_max_range))
      {
        deletion.push_back(1);
      }
      else
      {
        deletion.push_back(0);
      }
    }
    m_points.applyDeletionMask(deletion);
  }

  void KinectScan(
      const std::vector<mrpt::maps::COccupancyGridMap2D>& m_maps,
      const std::vector<double>& heights,
      const mrpt::poses::CPose3D& robotpose,
      const mrpt::poses::CPose3D& kinectrelpose)
  {
    unsigned int acc_factor = std::max(1, mrpt::round<double>(80.0 / m_columns));
    float h = 0, incrz;
    CObservation2DRangeScan m_auxlaser;
    mrpt::poses::CPose2D scanpose2d;
    mrpt::math::TPoint3D point;
    CSimplePointsMap row_points;
    row_points.insertionOptions.minDistBetweenLaserPoints = 0;
    m_points.clear();

    scanpose2d.x(robotpose[0]);
    scanpose2d.y(robotpose[1]);
    scanpose2d.phi(robotpose[3]);
    m_auxlaser.setSensorPose(kinectrelpose);
    m_auxlaser.aperture = m_fov_h;
    // m_auxlaser.beamAperture = 0.01;  //Optional

    // For each map
    for (unsigned int k = 0; k < m_maps.size(); k++)
    {
      // acc_factor is used to get a higher resolution
      m_maps[k].laserScanSimulator(
          m_auxlaser, scanpose2d, 0.5f, acc_factor * m_columns, m_std_error, 1, 0);
      row_points.insertObservation(m_auxlaser);

      for (unsigned int i = 0; i < m_rows; i++)
      {
        for (unsigned int j = 0; j < m_columns; j++)
        {
          if (row_points.size() > acc_factor * j)
          {
            row_points.getPoint(acc_factor * j, point.x, point.y, point.z);
            incrz = kinectrelpose.distance3DTo(point.x, point.y, point.z) *
                    tan((float(i) / (m_rows - 1) - 0.5) * m_fov_v + m_pitch_angle) *
                    cos((float(j) / (m_columns - 1) - 0.5) * m_fov_h);
            point.z = point.z + incrz;

            // Points which belong to their height level are
            // inserted. Otherwise they are deleted.
            if (m_maps.size() == 1)
            {
              m_points.insertPoint(point);
            }
            else
            {
              if (k == 0)
              {
                if (point.z < heights[k])
                {
                  m_points.insertPoint(point);
                }
              }
              else if (k == m_maps.size() - 1)
              {
                if (point.z >= h)
                {
                  m_points.insertPoint(point);
                }
              }
              else
              {
                if ((point.z >= h) && (point.z < h + heights[k]))
                {
                  m_points.insertPoint(point);
                }
              }
            }
          }
        }
      }
      row_points.clear();
      h = h + heights[k];
    }

    CorrectFloorPoints(kinectrelpose);
    CorrectCeiling(kinectrelpose, 3);  // Default: ceiling height = 3 meters
    CorrectRanges(kinectrelpose);
  }
};

class CShortTermMemory
{
 public:
  bool is_active;
  std::vector<mrpt::maps::COccupancyGridMap2D> obsgrids;
  float vision_limit;
  float likelihood_incr;
  float likelihood_decr;
  float occupancy_threshold;
  math::TPoint2D robot_ingrid;
  CSimplePointsMap grid_points;

  float remainder(float dividend, float divisor)
  {
    while (dividend > divisor) dividend -= divisor;
    return dividend;
  }

  void updateObsGrids(
      float incrx,
      float incry,
      float phi,
      const std::vector<CRobotKinects>& kinects,
      const std::vector<double>& heights)
  {
    using namespace std;
    using mrpt::square;
    // First, move the robot respect to the grid and adjust the likelihood
    // values in the grid according to that movement
    //-----------------------------------------------------------------------------------------------------------------

    if ((abs(robot_ingrid.x + incrx) < obsgrids[0].getResolution()) &&
        (abs(robot_ingrid.y + incry) < obsgrids[0].getResolution()))
    // The grid doesn't have to be diplaced
    {
      robot_ingrid.x = robot_ingrid.x + incrx;
      robot_ingrid.y = robot_ingrid.y + incry;
    }
    else if (sqrt(square(incrx) + square(incry)) > 2.6 * obsgrids[0].getXMax())
    // The displacement is too big so the grid is reset
    {
      for (unsigned int i = 0; i < obsgrids.size(); i++)
      {
        obsgrids[i].setSize(
            obsgrids[0].getXMin(), obsgrids[0].getXMax(), obsgrids[0].getYMin(),
            obsgrids[0].getXMax(), obsgrids[0].getResolution(), 0.5);
      }
    }
    else
    // The grid is displaced according to the robot movement
    {
      int despx = obsgrids[0].x2idx(robot_ingrid.x + incrx) - obsgrids[0].x2idx(robot_ingrid.x);
      int despy = obsgrids[0].y2idx(robot_ingrid.y + incry) - obsgrids[0].y2idx(robot_ingrid.y);
      // int despxpos = abs(despx);
      // int despypos = abs(despy);
      float despxmeters = despx * obsgrids[0].getResolution();
      float despymeters = despy * obsgrids[0].getResolution();

      float xcel, ycel;
      vector<float> cells_newval;

      // For each of the "n" grids
      for (unsigned int n = 0; n < obsgrids.size(); n++)
      {
        cells_newval.clear();

        // Cell values are stored
        for (unsigned int i = 0; i < obsgrids[n].getSizeX(); i++)
        {
          for (unsigned int j = 0; j < obsgrids[n].getSizeY(); j++)
          {
            xcel = obsgrids[n].idx2x(i) + despxmeters;
            ycel = obsgrids[n].idx2y(j) + despymeters;
            if ((abs(xcel) > obsgrids[n].getXMax()) || (abs(ycel) > obsgrids[n].getYMax()))
              cells_newval.push_back(-1);
            else
              cells_newval.push_back(obsgrids[n].getCell(i + despx, j + despy));
          }
        }

        // Cell values are updated in their new "positions"
        for (unsigned int i = 0; i < obsgrids[n].getSizeX(); i++)
        {
          for (unsigned int j = 0; j < obsgrids[n].getSizeY(); j++)
          {
            if (cells_newval[j + obsgrids[n].getSizeY() * i] == -1)
              obsgrids[n].setCell(i, j, 0.5);
            else
              obsgrids[n].setCell(i, j, cells_newval[j + obsgrids[n].getSizeY() * i]);
          }
        }
      }

      robot_ingrid.x = sign<float>(robot_ingrid.x + incrx) *
                       remainder(abs(robot_ingrid.x + incrx), obsgrids[0].getResolution());
      robot_ingrid.y = sign<float>(robot_ingrid.y + incry) *
                       remainder(abs(robot_ingrid.y + incry), obsgrids[0].getResolution());
    }

    // Second, update the likelihood values according to kinect scan
    //-------------------------------------------------------------

    float angrot = -phi;
    float aux_xpass;
    float incr_grid_reactive = 0.2 / obsgrids[0].getResolution();  // This number marks distance in
                                                                   // meters (but it's transformed
                                                                   // into an index)
    mrpt::math::TPoint3D paux;
    unsigned int index;
    unsigned int lim_visionxn = obsgrids[0].x2idx(-vision_limit + robot_ingrid.x);
    unsigned int lim_visionxp = obsgrids[0].x2idx(vision_limit + robot_ingrid.x);
    unsigned int lim_visionyn = obsgrids[0].x2idx(-vision_limit + robot_ingrid.y);
    unsigned int lim_visionyp = obsgrids[0].x2idx(vision_limit + robot_ingrid.y);
    unsigned int num_col = obsgrids[0].getSizeX();
    float xylim = obsgrids[0].getXMax();

    float level_height = 0.0;
    vector<bool> obs_in;
    obs_in.resize(square(num_col), 0);
    grid_points.clear();

    for (unsigned int n = 0; n < obsgrids.size(); n++)
    {
      obs_in.assign(square(num_col), 0);

      // Vector obs_in is filled with 0 or 1 depending on the presence of
      // any obstacle at each cell (of the grid)
      for (unsigned int i = 0; i < kinects[0].m_points.size(); i++)
      {
        kinects[0].m_points.getPoint(i, paux);

        // Points rotation and translation
        aux_xpass = paux.x * cos(angrot) + paux.y * sin(angrot) + robot_ingrid.x;
        paux.y = -paux.x * sin(angrot) + paux.y * cos(angrot) + robot_ingrid.y;
        paux.x = aux_xpass;

        // Set binary occupancy of the cells (1 - there is at least one
        // point, 0 - there isn't any point)
        if ((paux.x >= -xylim) && (paux.x <= xylim) && (paux.y >= -xylim) && (paux.y <= xylim) &&
            (paux.z < level_height + heights[n]) && (paux.z > level_height + 0.000001))
        {
          index = obsgrids[n].x2idx(paux.x) + num_col * obsgrids[n].y2idx(paux.y);
          obs_in[index] = 1;
        }
      }

      // The likelihood values of the grid are updated
      float angle_cell, dif_angle, rango = M_PIf / 6 - 0.05f;

      for (unsigned int i = 0; i < num_col; i++)
      {
        for (unsigned int j = 0; j < num_col; j++)
        {
          if (obs_in[i + num_col * j] == 1)
            obsgrids[n].updateCell(i, j, likelihood_incr);
          else if (
              ((i < lim_visionxn) || (i > lim_visionxp)) ||
              ((j < lim_visionyn) || (j > lim_visionyp)))
          {
            // The angle between the advance direction of the robot
            // and the cell is calculated
            angle_cell =
                atan2(obsgrids[n].idx2y(j) - robot_ingrid.y, obsgrids[n].idx2x(i) - robot_ingrid.x);
            dif_angle = abs(angle_cell - phi);
            if (dif_angle > M_PI) dif_angle = dif_angle - 2 * M_PI;
            if (abs(dif_angle) < rango) obsgrids[n].updateCell(i, j, likelihood_decr);
          }

          // Transform the cell with high occupancy likelihood into 3D
          // points
          if (((i >= lim_visionxn - incr_grid_reactive) &&
               (i <= lim_visionxp + incr_grid_reactive)) &&
              ((j >= lim_visionyn - incr_grid_reactive) &&
               (j <= lim_visionyp + incr_grid_reactive)))
          {
            if (obsgrids[n].getCell(i, j) > occupancy_threshold)
            {
              paux.x = (obsgrids[n].idx2x(i) - robot_ingrid.x) * cos(-angrot) +
                       (obsgrids[n].idx2y(j) - robot_ingrid.y) * sin(-angrot);
              paux.y = -(obsgrids[n].idx2x(i) - robot_ingrid.x) * sin(-angrot) +
                       (obsgrids[n].idx2y(j) - robot_ingrid.y) * cos(-angrot);
              paux.z = level_height + 0.5 * heights[n];
              grid_points.insertPoint(paux.x, paux.y, paux.z);
              // Include points in their level for reactive
              // navigation
              // obstacles_inlevels[n].insertPoint(paux.x, paux.y,
              // paux.z);
            }
          }
        }
      }
      level_height += heights[n];
    }
  }
};

class CMyReactInterface : public mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven
{
 public:
  CVehicleSimul_DiffDriven robotSim;

  CMyReactInterface() : CRobot2NavInterfaceForSimulator_DiffDriven(this->robotSim) {}

  CPose2D new_pose;
  CPose2D last_pose;
  CPose2D target;
  mrpt::nav::TRobotShape robotShape;
  std::vector<mrpt::maps::COccupancyGridMap2D> maps;
  std::vector<TRobotLaser> lasers;
  std::vector<CRobotKinects> kinects;
  CShortTermMemory stm;
  gui::CDisplayWindow3D window;
  Scene::Ptr scene;

  bool getCurrentPoseAndSpeeds(
      mrpt::math::TPose2D& curPose,
      mrpt::math::TTwist2D& curVel,
      mrpt::system::TTimeStamp& timestamp,
      mrpt::math::TPose2D& odomPose,
      std::string& pose_frame_id) override
  {
    curPose = robotSim.getCurrentGTPose();
    curVel = robotSim.getCurrentGTVel();
    timestamp = mrpt::Clock::now();
    odomPose = robotSim.getCurrentOdometricPose();
    return true;
  }

  bool senseObstacles(
      mrpt::maps::CSimplePointsMap& obstacles, mrpt::system::TTimeStamp& timestamp) override
  {
    last_pose = new_pose;
    new_pose = CPose2D(robotSim.getCurrentGTPose());
    CPose3D robotpose3d(new_pose[0], new_pose[1], 0, new_pose[2], 0, 0);
    CPose3D kinectrelpose(0, 0, 0, 0, 0, 0);

    obstacles.clear();

    // Laser scans
    for (unsigned int i = 0; i < lasers.size(); i++)
    {
      maps[lasers[i].m_level - 1].laserScanSimulator(
          lasers[i].m_scan, new_pose, 0.5f, lasers[i].m_segments, lasers[i].m_scan.stdError, 1, 0);

      obstacles.insertObservation(lasers[i].m_scan);
    }
    timestamp = mrpt::Clock::now();

    // Depth scans
    for (unsigned int i = 0; i < kinects.size(); i++)
    {
      kinectrelpose.x(kinects[i].m_xrel);
      kinectrelpose.y(kinects[i].m_yrel);
      kinectrelpose.z(kinects[i].m_zrel);
      kinectrelpose.setYawPitchRoll(kinects[i].m_phi, 0, 0);
      kinects[i].KinectScan(maps, robotShape.getHeights(), robotpose3d, kinectrelpose);
      obstacles.insertAnotherMap(&kinects[i].m_points, CPose3D(0, 0, 0, 0, 0, 0));
    }

    // Process depth scans in the STM
    if (stm.is_active)
    {
      float incrx = new_pose[0] - last_pose[0];
      float incry = new_pose[1] - last_pose[1];
      stm.updateObsGrids(incrx, incry, float(new_pose.phi()), kinects, robotShape.getHeights());
      obstacles.insertAnotherMap(&stm.grid_points, CPose3D(0, 0, 0, 0, 0, 0));
    }

    return true;
  }

  void loadMaps(const mrpt::config::CConfigFileBase& ini)
  {
    COccupancyGridMap2D grid;
    mrpt::img::CImage myImg;
    // int family = ini.read_int("MAP_CONFIG","FAMILY", 1, true);
    float resolution = ini.read_float("MAP_CONFIG", "MAP_RESOLUTION", 0.02f, true);
    // int num_maps = ini.read_int("MAP_CONFIG","NUM_MAPS", 1, true);

    // Maps are loaded here. Different maps can be loaded changing these
    // lines  and including them above (#define...)
    bool loadOk = myImg.loadFromXPM(map2_1_xpm);
    ASSERT_(loadOk);
    grid.loadFromBitmap(myImg, resolution);
    maps.push_back(grid);

    loadOk = myImg.loadFromXPM(map2_2_xpm);
    ASSERT_(loadOk);
    grid.loadFromBitmap(myImg, resolution);
    maps.push_back(grid);

    loadOk = myImg.loadFromXPM(map2_3_xpm);
    ASSERT_(loadOk);
    grid.loadFromBitmap(myImg, resolution);
    maps.push_back(grid);

    std::cout << std::endl << "Maps have been loaded successfully.";
  }

  void loadConfiguration(const mrpt::config::CConfigFileBase& ini)
  {
    unsigned int num_lasers, num_kinects, num_levels;
    std::vector<double> lasercoord, xaux, yaux;

    // Read lasers params
    num_lasers = ini.read_int("LASER_CONFIG", "N_LASERS", 1, true);
    lasers.resize(num_lasers);
    for (unsigned int i = 1; i <= num_lasers; i++)
    {
      ini.read_vector(
          "LASER_CONFIG", format("LASER%d_POSE", i), std::vector<double>(0), lasercoord, true);
      mrpt::obs::CObservation2DRangeScan& scan = lasers[i - 1].m_scan;
      scan.maxRange = ini.read_float("LASER_CONFIG", format("LASER%d_MAX_RANGE", i), 50, true);
      scan.aperture = ini.read_float("LASER_CONFIG", format("LASER%d_APERTURE", i), M_PIf, true);
      scan.stdError = ini.read_float("LASER_CONFIG", format("LASER%d_STD_ERROR", i), 0.05f, true);
      scan.sensorPose.setFromValues(
          lasercoord[0], lasercoord[1], lasercoord[2], lasercoord[3], lasercoord[4], lasercoord[5]);
      lasers[i - 1].m_level = ini.read_int("LASER_CONFIG", format("LASER%d_LEVEL", i), 1, true);
      lasers[i - 1].m_segments =
          ini.read_int("LASER_CONFIG", format("LASER%d_SEGMENTS", i), 181, true);
    }

    // Read kinects params
    num_kinects = ini.read_int("KINECT_CONFIG", "N_KINECTS", 1, true);
    kinects.resize(num_kinects);
    for (unsigned int i = 1; i <= num_kinects; i++)
    {
      kinects[i - 1].m_level = ini.read_int("KINECT_CONFIG", format("KINECT%d_LEVEL", i), 1, true);
      kinects[i - 1].m_xrel = ini.read_float("KINECT_CONFIG", format("KINECT%d_X", i), 0, true);
      kinects[i - 1].m_yrel = ini.read_float("KINECT_CONFIG", format("KINECT%d_Y", i), 0, true);
      kinects[i - 1].m_zrel = ini.read_float("KINECT_CONFIG", format("KINECT%d_Z", i), 0, true);
      kinects[i - 1].m_phi =
          DEG2RAD(ini.read_float("KINECT_CONFIG", format("KINECT%d_PHI", i), 0, true));
      kinects[i - 1].m_min_range =
          ini.read_float("KINECT_CONFIG", format("KINECT%d_MINRANGE", i), 0, true);
      kinects[i - 1].m_max_range =
          ini.read_float("KINECT_CONFIG", format("KINECT%d_MAXRANGE", i), 0, true);
      kinects[i - 1].m_fov_v =
          DEG2RAD(ini.read_float("KINECT_CONFIG", format("KINECT%d_FOV_V", i), 60, true));
      kinects[i - 1].m_fov_h =
          DEG2RAD(ini.read_float("KINECT_CONFIG", format("KINECT%d_FOV_H", i), 60, true));
      kinects[i - 1].m_pitch_angle =
          DEG2RAD(ini.read_float("KINECT_CONFIG", format("KINECT%d_PITCH", i), 0, true));
      kinects[i - 1].m_rows = ini.read_int("KINECT_CONFIG", format("KINECT%d_ROWS", i), 10, true);
      kinects[i - 1].m_columns =
          ini.read_int("KINECT_CONFIG", format("KINECT%d_COLUMNS", i), 10, true);
      kinects[i - 1].m_std_error =
          ini.read_float("KINECT_CONFIG", format("KINECT%d_STD_ERROR", i), 0.05f, true);
    }

    // Read config params which describe the robot shape
    const auto sect = "CReactiveNavigationSystem3D";
    num_levels = ini.read_int(sect, "HEIGHT_LEVELS", 1, true);
    robotShape.resize(num_levels);
    for (unsigned int i = 1; i <= num_levels; i++)
    {
      robotShape.setHeight(i - 1, ini.read_double(sect, format("LEVEL%d_HEIGHT", i), 1.0, true));
      robotShape.setRadius(i - 1, ini.read_double(sect, format("LEVEL%d_RADIUS", i), 0.5, false));
      ini.read_vector(sect, format("LEVEL%d_VECTORX", i), std::vector<double>(0), xaux, false);
      ini.read_vector(sect, format("LEVEL%d_VECTORY", i), std::vector<double>(0), yaux, false);
      ASSERT_(xaux.size() == yaux.size());
      for (unsigned int j = 0; j < xaux.size(); j++)
      {
        robotShape.polygon(i - 1).AddVertex(xaux[j], yaux[j]);
      }
    }

    // Read other params associated with the robot model and its navigation
    // CRobot2NavInterface_DiffDriven::loadConfigFile(ini,
    // "ReactiveParams");
    float x_ini = ini.read_float("MAP_CONFIG", "X0", 0, true);
    float y_ini = ini.read_float("MAP_CONFIG", "Y0", 0, true);
    float phi_ini = DEG2RAD(ini.read_float("MAP_CONFIG", "PHI0", 0, true));
    robotSim.resetStatus();
    robotSim.setOdometryErrors(0);
    robotSim.setCurrentGTPose(mrpt::math::TPose2D(x_ini, y_ini, phi_ini));

    // Read the "short term memory" parameters
    stm.is_active = ini.read_bool("STM_CONFIG", "Stm_active", 0, 1);
    float grid_length = ini.read_float("STM_CONFIG", "Obs_grid_length", 0.8f, 1);
    float grid_resolution = ini.read_float("STM_CONFIG", "Obs_grid_resolution", 0.1f, 1);
    stm.vision_limit = ini.read_float("STM_CONFIG", "Vision_limit", 0.6f, 1);
    stm.likelihood_incr = ini.read_float("STM_CONFIG", "Pos_likelihood_incr", 0.55f, 1);
    stm.likelihood_decr = ini.read_float("STM_CONFIG", "Neg_likelihood_incr", 0.45f, 1);
    stm.occupancy_threshold = ini.read_float("STM_CONFIG", "Occupancy_threshold", 0.8f, 1);

    if (stm.is_active)
    {
      stm.robot_ingrid.x = 0;
      stm.robot_ingrid.y = 0;
      stm.obsgrids.resize(num_levels);
      for (unsigned int i = 0; i < stm.obsgrids.size(); i++)
      {
        stm.obsgrids[i].setSize(
            -grid_length, grid_length, -grid_length, grid_length, grid_resolution, 0.5);
      }
    }
  }

  void initializeScene()
  {
    CPose3D robotpose3d = CPose3D(CPose2D(robotSim.getCurrentGTPose()));

    // The display window is created
    mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE(10000);
    window.setWindowTitle("Reactive Navigation. Robot motion simulation");
    window.resize(1800, 980);
    window.setPos(50, 0);
    window.setCameraZoom(35);
    window.setCameraAzimuthDeg(20);
    window.setCameraElevationDeg(60);
    scene = window.get3DSceneAndLock();

    // Maps are inserted
    for (unsigned int i = 0; i < maps.size(); i++) scene->insert(maps[i].getVisualization());

    // A CornerXYZ object is inserted as an absolute frame of reference
    {
      auto obj = opengl::stock_objects::CornerXYZ();
      obj->setLocation(0, 0, 0);
      scene->insert(obj);
    }

    // The target is inserted
    {
      auto obj = opengl::CDisk::Create(0.4f, 0.3f);
      obj->setLocation(0, 0, 0.02);
      obj->setColor(0.2, 0.3, 0.9);
      scene->insert(obj);
    }

    // The robot is inserted
    {
      float h;
      for (unsigned int i = 0; i < robotShape.size(); i++)
      {
        if (i == 0)
        {
          h = 0;
        }
        else
        {
          h = robotShape.getHeight(i - 1) + h;
        }

        robotpose3d.z(h);
        auto obj =
            opengl::CPolyhedron::CreateCustomPrism(robotShape.polygon(i), robotShape.getHeight(i));
        obj->setName(format("Level%d", i + 1));
        obj->setPose(robotpose3d);
        obj->setColor(0.2, 0.5, 0.2, 1);
        // obj->setWireframe(true);
        obj->setLineWidth(2);
        scene->insert(obj);
      }
    }

    // One scan is simulated
    CSimplePointsMap auxpoints;
    mrpt::system::TTimeStamp auxpoints_time;
    this->senseObstacles(auxpoints, auxpoints_time);

    robotpose3d.z() = 0;
    // The laserscans are inserted
    {
      std::vector<CPlanarLaserScan::Ptr> gl_scan;
      CPlanarLaserScan::Ptr gl_scanind;

      for (unsigned int i = 0; i < lasers.size(); i++)
      {
        gl_scan.push_back(gl_scanind);
        gl_scan[i] = CPlanarLaserScan::Create();

        gl_scan[i]->enableLine(true);
        gl_scan[i]->enableSurface(false);
        gl_scan[i]->enablePoints(true);
        gl_scan[i]->setName(format("Laser%d", i + 1));
        gl_scan[i]->setScan(lasers[i].m_scan);
        gl_scan[i]->setPose(robotpose3d);
        scene->insert(gl_scan[i]);
      }
    }

    // The Kinectscans are inserted
    {
      robotpose3d.z(0);
      mrpt::math::TPoint3D point;
      std::vector<CPointCloud::Ptr> obj;
      CPointCloud::Ptr indobj;

      for (unsigned int i = 0; i < kinects.size(); i++)
      {
        obj.push_back(indobj);
        obj[i] = opengl::CPointCloud::Create();
        obj[i]->setPose(robotpose3d);
        obj[i]->setName(format("Kinect%d", i + 1));
        scene->insert(obj[i]);
        obj[i]->setColor(0, 0, 1);
        obj[i]->setPointSize(4.0);
        for (unsigned int j = 0; j < kinects[i].m_points.size(); j++)
        {
          kinects[i].m_points.getPoint(j, point);
          obj[i]->insertPoint(point.x, point.y, point.z);
        }
      }
    }

    // Virtual obstacles from STM are inserted
    {
      auto obj = CPointCloud::Create();
      obj->setPose(robotpose3d);
      obj->setPointSize(5.0);
      obj->setColor(0, 1, 0);
      obj->loadFromPointsMap<CSimplePointsMap>(&stm.grid_points);
      scene->insert(obj);
    }

    window.unlockAccess3DScene();

    window.addTextMessage(5, 180, getLegendString(window.getRenderingFPS()));
    window.repaint();
  }

  static std::string getLegendString(double fps)
  {
    std::string legend =
        "--------------------------------------------\n"
        "| m - Move the target \t|\n"
        "| p - Pause navigation \t\t|\n"
        "| r - Resume navigation \t|\n"
        "| e - Exit \t\t\t\t|\n"
        "--------------------------------------------\n";
    legend += format("\n        %.02fFPS", fps);
    return legend;
  }

  void updateScene()
  {
    scene = window.get3DSceneAndLock();
    CPose3D robotpose3d = CPose3D(CPose2D(robotSim.getCurrentGTPose()));

    // The robot pose is updated
    {
      float h = 0;
      for (unsigned int i = 0; i < robotShape.size(); i++)
      {
        CRenderizable::Ptr obj;
        obj = scene->getByName(format("Level%d", i + 1));

        if (i == 0)
        {
          h = 0;
        }
        else
        {
          h = robotShape.getHeight(i - 1) + h;
        }

        robotpose3d.z(h);
        obj->setPose(robotpose3d);
      }
    }

    robotpose3d.z() = 0;
    // The laserscan is inserted
    {
      CPlanarLaserScan::Ptr lasobj;

      for (unsigned int i = 0; i < lasers.size(); i++)
      {
        lasobj = scene->getByClass<CPlanarLaserScan>(i);
        lasobj->setScan(lasers[i].m_scan);
        lasobj->setPose(robotpose3d);
      }
    }

    // The Kinectscan is inserted
    {
      robotpose3d.z(0);
      mrpt::math::TPoint3D point;
      CPointCloud::Ptr pc;

      for (unsigned int i = 0; i < kinects.size(); i++)
      {
        pc = scene->getByClass<CPointCloud>(i);
        pc->clear();
        pc->setPose(robotpose3d);

        for (unsigned int j = 0; j < kinects[i].m_points.size(); j++)
        {
          kinects[i].m_points.getPoint(j, point);
          pc->insertPoint(point.x, point.y, point.z);
        }
      }
    }

    // Virtual obstacles from STM are inserted
    {
      CPointCloud::Ptr obj;
      obj = scene->getByClass<CPointCloud>(kinects.size());
      obj->setPose(robotpose3d);
      obj->clear();
      obj->loadFromPointsMap<CSimplePointsMap>(&stm.grid_points);
    }

    window.unlockAccess3DScene();

    window.addTextMessage(5, 180, getLegendString(window.getRenderingFPS()));
    window.repaint();
  }

  CAbstractNavigator::TNavigationParams createNewTarget(
      float x, float y, float targetAllowedDistance, bool targetIsRelative = false)
  {
    CAbstractNavigator::TNavigationParams navparams;
    navparams.target.target_coords = mrpt::math::TPose2D(x, y, 0);
    navparams.target.targetAllowedDistance = targetAllowedDistance;
    navparams.target.targetIsRelative = targetIsRelative;
    if (!targetIsRelative)
      target = CPose2D(x, y, 0);
    else
    {
      CPose2D robotpose = CPose2D(robotSim.getCurrentGTPose());
      target = CPose2D(x, y, 0) + robotpose;
    }
    return navparams;
  }
};
