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

#include "maps-precomp.h"  // Precomp header
//
#include <mrpt/core/round.h>  // round()
#include <mrpt/img/color_maps.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/maps/CGasConcentrationGridMap2D.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/obs/CObservationGasSensors.h>
#include <mrpt/opengl/CArrow.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/datetime.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::io;
using namespace mrpt::poses;
using namespace mrpt::img;
using namespace std;
using namespace mrpt::math;

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
    "mrpt::maps::CGasConcentrationGridMap2D,gasGrid", mrpt::maps::CGasConcentrationGridMap2D)

CGasConcentrationGridMap2D::TMapDefinition::TMapDefinition()

    = default;

void CGasConcentrationGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(
    const mrpt::config::CConfigFileBase& source, const std::string& sectionNamePrefix)
{
  // [<sectionNamePrefix>+"_creationOpts"]
  const std::string sSectCreation = sectionNamePrefix + string("_creationOpts");
  MRPT_LOAD_CONFIG_VAR(min_x, float, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(max_x, float, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(min_y, float, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(max_y, float, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(resolution, float, source, sSectCreation);
  mapType = source.read_enum<CGasConcentrationGridMap2D::TMapRepresentation>(
      sSectCreation, "mapType", mapType);

  insertionOpts.loadFromConfigFile(source, sectionNamePrefix + string("_insertOpts"));
}

void CGasConcentrationGridMap2D::TMapDefinition::dumpToTextStream_map_specific(
    std::ostream& out) const
{
  out << mrpt::format(
      "MAP TYPE                                  = %s\n",
      mrpt::typemeta::TEnumType<CGasConcentrationGridMap2D::TMapRepresentation>::value2name(mapType)
          .c_str());
  LOADABLEOPTS_DUMP_VAR(min_x, float);
  LOADABLEOPTS_DUMP_VAR(max_x, float);
  LOADABLEOPTS_DUMP_VAR(min_y, float);
  LOADABLEOPTS_DUMP_VAR(max_y, float);
  LOADABLEOPTS_DUMP_VAR(resolution, float);

  this->insertionOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap::Ptr CGasConcentrationGridMap2D::internal_CreateFromMapDefinition(
    const mrpt::maps::TMetricMapInitializer& _def)
{
  const CGasConcentrationGridMap2D::TMapDefinition& def =
      *dynamic_cast<const CGasConcentrationGridMap2D::TMapDefinition*>(&_def);
  auto obj = CGasConcentrationGridMap2D::Create(
      def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution);
  obj->insertionOptions = def.insertionOpts;
  return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CGasConcentrationGridMap2D, CRandomFieldGridMap2D, mrpt::maps)

// Short-cut:
#define LUT_TABLE (*(LUT.table))

/*---------------------------------------------------------------
            Constructor
  ---------------------------------------------------------------*/
CGasConcentrationGridMap2D::CGasConcentrationGridMap2D(
    TMapRepresentation mapType,
    float x_min,
    float x_max,
    float y_min,
    float y_max,
    float resolution) :
    CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution), insertionOptions()
{
  // Override defaults:
  insertionOptions.GMRF_saturate_min = 0;
  insertionOptions.GMRF_saturate_max = 1;
  insertionOptions.GMRF_lambdaObsLoss = 1.0;

  // Set the grid to initial values (and adjust covariance matrices,...)
  //  Also, calling clear() is mandatory to end initialization of our base
  //  class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
  CMetricMap::clear();

  // Create WindGrids with same dimensions that the original map
  windGrid_module.setSize(x_min, x_max, y_min, y_max, resolution);
  windGrid_direction.setSize(x_min, x_max, y_min, y_max, resolution);

  // initialize counter for advection simulation
  timeLastSimulated = mrpt::Clock::now();
}

CGasConcentrationGridMap2D::~CGasConcentrationGridMap2D() = default;
/*---------------------------------------------------------------
            clear
  ---------------------------------------------------------------*/
void CGasConcentrationGridMap2D::internal_clear()
{
  // Just do the generic clear:
  CRandomFieldGridMap2D::internal_clear();

  // Anything else special for this derived class?

  if (insertionOptions.useWindInformation)
  {
    // Set default values of the wind grid
    windGrid_module.fill(insertionOptions.default_wind_speed);
    windGrid_direction.fill(insertionOptions.default_wind_direction);

    // Generate Look-Up Table of the Gaussian weights due to wind advection.
    if (!build_Gaussian_Wind_Grid())
    {
      // mrpt::system::pause();
      THROW_EXCEPTION("Problem with LUT wind table");
    }
  }
}

/*---------------------------------------------------------------
            insertObservation
  ---------------------------------------------------------------*/
bool CGasConcentrationGridMap2D::internal_insertObservation(
    const CObservation& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  MRPT_START

  CPose2D robotPose2D;
  CPose3D robotPose3D;

  if (robotPose)
  {
    robotPose2D = CPose2D(*robotPose);
    robotPose3D = (*robotPose);
  }
  else
  {
    // Default values are (0,0,0)
  }

  if (IS_CLASS(obs, CObservationGasSensors))
  {
    /********************************************************************
          OBSERVATION TYPE: CObservationGasSensors
    ********************************************************************/
    const auto& o = dynamic_cast<const CObservationGasSensors&>(obs);

    if (o.sensorLabel.compare(insertionOptions.gasSensorLabel) == 0)
    {
      float sensorReading;
      CPose2D sensorPose;

      if (o.sensorLabel.compare("MCEnose") == 0 || o.sensorLabel.compare("Full_MCEnose") == 0)
      {
        ASSERT_(o.m_readings.size() > insertionOptions.enose_id);
        const CObservationGasSensors::TObservationENose* it =
            &o.m_readings[insertionOptions.enose_id];

        // Compute the 3D sensor pose in world coordinates:
        sensorPose = CPose2D(CPose3D(robotPose2D) + CPose3D(it->eNosePoseOnTheRobot));

        // Compute the sensor reading value (Volts):
        if (insertionOptions.gasSensorType == 0x0000)
        {  // compute the mean
          sensorReading = d2f(math::mean(it->readingsVoltage));
        }
        else
        {
          // Look for the correct sensor type
          size_t i;
          for (i = 0; i < it->sensorTypes.size(); i++)
          {
            if (it->sensorTypes.at(i) == int(insertionOptions.gasSensorType)) break;
          }

          if (i < it->sensorTypes.size())
          {
            sensorReading = it->readingsVoltage[i];
          }
          else
          {
            cout << "Sensor especified not found, compute default "
                    "mean value"
                 << endl;
            sensorReading = d2f(math::mean(it->readingsVoltage));
          }
        }
      }
      else  //"GDM, RAE_PID, ENOSE_SIMUL
      {
        const CObservationGasSensors::TObservationENose* it = &o.m_readings[0];
        // Compute the 3D sensor pose in world coordinates:
        sensorPose = CPose2D(CPose3D(robotPose2D) + CPose3D(it->eNosePoseOnTheRobot));
        sensorReading = it->readingsVoltage[0];
      }

      // Normalization:
      sensorReading = (sensorReading - insertionOptions.R_min) /
                      (insertionOptions.R_max - insertionOptions.R_min);

      // Update the gross estimates of mean/vars for the whole reading
      // history (see IROS2009 paper):
      m_average_normreadings_mean =
          (sensorReading + m_average_normreadings_count * m_average_normreadings_mean) /
          (1 + m_average_normreadings_count);
      m_average_normreadings_var = (square(sensorReading - m_average_normreadings_mean) +
                                    m_average_normreadings_count * m_average_normreadings_var) /
                                   (1 + m_average_normreadings_count);
      m_average_normreadings_count++;

      // Finally, do the actual map update with that value:
      this->insertIndividualReading(
          sensorReading, mrpt::math::TPoint2D(sensorPose.x(), sensorPose.y()));
      return true;  // Done!
    }               // endif correct "gasSensorLabel"
  }                 // end if "CObservationGasSensors"

  return false;
  MRPT_END
}

/*---------------------------------------------------------------
            computeObservationLikelihood
  ---------------------------------------------------------------*/
double CGasConcentrationGridMap2D::internal_computeObservationLikelihood(
    [[maybe_unused]] const CObservation& obs, [[maybe_unused]] const CPose3D& takenFrom) const
{
  THROW_EXCEPTION("Not implemented yet!");
}

uint8_t CGasConcentrationGridMap2D::serializeGetVersion() const { return 5; }
void CGasConcentrationGridMap2D::serializeTo(mrpt::serialization::CArchive& out) const
{
  dyngridcommon_writeToStream(out);

  // To assure compatibility: The size of each cell:
  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
  out << n;

  // Save the map contents:
  n = static_cast<uint32_t>(m_map.size());
  out << n;

// Save the "m_map": This requires special handling for big endian systems:
#if MRPT_IS_BIG_ENDIAN
  for (uint32_t i = 0; i < n; i++)
  {
    out << m_map[i].kf_mean() << m_map[i].dm_mean() << m_map[i].dmv_var_mean;
  }
#else
  // Little endian: just write all at once:
  out.WriteBuffer(&m_map[0],
                  sizeof(m_map[0]) * m_map.size());  // TODO: Do this endianness safe!!
#endif

  // Version 1: Save the insertion options:
  out << uint8_t(m_mapType) << m_cov << m_stackedCov;

  out << insertionOptions.sigma << insertionOptions.cutoffRadius << insertionOptions.R_min
      << insertionOptions.R_max << insertionOptions.KF_covSigma
      << insertionOptions.KF_initialCellStd << insertionOptions.KF_observationModelNoise
      << insertionOptions.KF_defaultCellMeanValue << insertionOptions.KF_W_size;

  // New in v3:
  out << m_average_normreadings_mean << m_average_normreadings_var
      << uint64_t(m_average_normreadings_count);

  out << genericMapParams;  // v4
}

// Aux struct used below (must be at global scope for STL):
struct TOldCellTypeInVersion1
{
  float mean, std;
  float w, wr;
};

void CGasConcentrationGridMap2D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    {
      dyngridcommon_readFromStream(in, version < 5);

      // To assure compatibility: The size of each cell:
      uint32_t n;
      in >> n;

      if (version < 2)
      {  // Converter from old versions <=1
        ASSERT_(n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
        // Load the map contents in an aux struct:
        in >> n;
        vector<TOldCellTypeInVersion1> old_map(n);
        in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());

        // Convert to newer format:
        m_map.resize(n);
        for (size_t k = 0; k < n; k++)
        {
          m_map[k].kf_mean() = (old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
          m_map[k].kf_std() = (old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
        }
      }
      else
      {
        ASSERT_EQUAL_(n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
        // Load the map contents:
        in >> n;
        m_map.resize(n);

// Read the note in writeToStream()
#if MRPT_IS_BIG_ENDIAN
        for (uint32_t i = 0; i < n; i++)
          in >> m_map[i].kf_mean() >> m_map[i].dm_mean() >> m_map[i].dmv_var_mean;
#else
        // Little endian: just read all at once:
        in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
#endif
      }

      // Version 1: Insertion options:
      if (version >= 1)
      {
        uint8_t i;
        in >> i;
        m_mapType = TMapRepresentation(i);

        in >> m_cov >> m_stackedCov;

        in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >> insertionOptions.R_min >>
            insertionOptions.R_max >> insertionOptions.KF_covSigma >>
            insertionOptions.KF_initialCellStd >> insertionOptions.KF_observationModelNoise >>
            insertionOptions.KF_defaultCellMeanValue >> insertionOptions.KF_W_size;
      }

      if (version >= 3)
      {
        uint64_t N;
        in >> m_average_normreadings_mean >> m_average_normreadings_var >> N;
        m_average_normreadings_count = N;
      }

      if (version >= 4) in >> genericMapParams;

      m_hasToRecoverMeanAndCov = true;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
          TInsertionOptions
 ---------------------------------------------------------------*/
CGasConcentrationGridMap2D::TInsertionOptions::TInsertionOptions() :

    gasSensorLabel("MCEnose"),
    // By default use the mean between all e-nose sensors
    windSensorLabel("windSensor")

{
}

void CGasConcentrationGridMap2D::TInsertionOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [CGasConcentrationGridMap2D::TInsertionOptions] "
         "------------ \n\n";
  out << "[TInsertionOptions.Common] ------------ \n\n";
  internal_dumpToTextStream_common(out);  // Common params to all random fields maps:

  out << "[TInsertionOptions.GasSpecific] ------------ \n\n";
  out << mrpt::format("gasSensorLabel							= %s\n", gasSensorLabel.c_str());
  out << mrpt::format("enose_id								= %u\n", (unsigned)enose_id);
  out << mrpt::format("gasSensorType							= %u\n", (unsigned)gasSensorType);
  out << mrpt::format("windSensorLabel							= %s\n", windSensorLabel.c_str());
  out << mrpt::format("useWindInformation						= %u\n", useWindInformation);

  out << mrpt::format("advectionFreq							= %f\n", advectionFreq);
  out << mrpt::format("default_wind_direction					= %f\n", default_wind_direction);
  out << mrpt::format("default_wind_speed						= %f\n", default_wind_speed);
  out << mrpt::format("std_windNoise_phi						= %f\n", std_windNoise_phi);
  out << mrpt::format("std_windNoise_mod						= %f\n", std_windNoise_mod);

  out << "\n";
}

/*---------------------------------------------------------------
          loadFromConfigFile
  ---------------------------------------------------------------*/
void CGasConcentrationGridMap2D::TInsertionOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
  // Common data fields for all random fields maps:
  internal_loadFromConfigFile_common(iniFile, section);

  // Specific data fields for gasGridMaps
  gasSensorLabel = iniFile.read_string(section.c_str(), "gasSensorLabel", "Full_MCEnose", true);
  enose_id = iniFile.read_int(section.c_str(), "enoseID", enose_id);
  // Read sensor type in hexadecimal
  {
    std::string sensorType_str = iniFile.read_string(section.c_str(), "gasSensorType", "-1", true);
    int tmpSensorType;
    stringstream convert(sensorType_str);
    convert >> std::hex >> tmpSensorType;

    if (tmpSensorType >= 0)
    {
      // Valid number found:
      gasSensorType = tmpSensorType;
    }
    else
    {  // fall back to old name, or default to current value:
      gasSensorType = iniFile.read_int(section.c_str(), "KF_sensorType", gasSensorType, true);
    }
  }
  windSensorLabel = iniFile.read_string(section.c_str(), "windSensorLabel", "Full_MCEnose", true);

  // Indicates if wind information must be used for Advection Simulation
  useWindInformation = iniFile.read_bool(section.c_str(), "useWindInformation", "false", true);

  //(rad) The initial/default value of the wind direction
  default_wind_direction = iniFile.read_float(section.c_str(), "default_wind_direction", 0, false);
  //(m/s) The initial/default value of the wind speed
  default_wind_speed = iniFile.read_float(section.c_str(), "default_wind_speed", 0, false);

  //(rad) The noise in the wind direction
  std_windNoise_phi = iniFile.read_float(section.c_str(), "std_windNoise_phi", 0, false);
  //(m/s) The noise in the wind strenght
  std_windNoise_mod = iniFile.read_float(section.c_str(), "std_windNoise_mod", 0, false);

  //(m/s) The noise in the wind strenght
  advectionFreq = iniFile.read_float(section.c_str(), "advectionFreq", 1, true);
}

void CGasConcentrationGridMap2D::getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const
{
  MRPT_START
  if (!genericMapParams.enableSaveAs3DObject) return;
  CRandomFieldGridMap2D::getVisualizationInto(o);
  MRPT_END
}

/*---------------------------------------------------------------
            getAs3DObject
---------------------------------------------------------------*/
void CGasConcentrationGridMap2D::getAs3DObject(
    opengl::CSetOfObjects& meanObj, opengl::CSetOfObjects& varObj) const
{
  MRPT_START
  if (!genericMapParams.enableSaveAs3DObject) return;
  CRandomFieldGridMap2D::getAs3DObject(meanObj, varObj);
  MRPT_END
}

/*---------------------------------------------------------------
            getWindAs3DObject
---------------------------------------------------------------*/
void CGasConcentrationGridMap2D::getWindAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& windObj) const
{
  // Return an arrow map of the wind state (module(color) and direction).
  float scale = 0.2f;
  size_t arrow_separation = 5;  // distance between arrows, expresed as times the cell resolution

  // map limits
  float x_min = d2f(getXMin());
  float x_max = d2f(getXMax());
  float y_min = d2f(getYMin());
  float y_max = d2f(getYMax());
  float resol = d2f(getResolution());

  // Ensure map dimensions match with wind map
  unsigned int wind_map_size = windGrid_direction.getSizeX() * windGrid_direction.getSizeY();
  ASSERT_(wind_map_size == windGrid_module.getSizeX() * windGrid_module.getSizeY());
  if (m_map.size() != wind_map_size)
  {
    cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND MAP " << endl;
    // mrpt::system::pause();
  }

  size_t cx, cy;
  vector<float> xs, ys;

  // xs: array of X-axis values
  xs.resize(floor((x_max - x_min) / (arrow_separation * resol)));
  for (cx = 0; cx < xs.size(); cx++) xs[cx] = x_min + arrow_separation * resol * cx;

  // ys: array of X-axis values
  ys.resize(floor((y_max - y_min) / (arrow_separation * resol)));
  for (cy = 0; cy < ys.size(); cy++) ys[cy] = y_min + arrow_separation * resol * cy;

  for (cy = 0; cy < ys.size(); cy++)
  {
    for (cx = 0; cx < xs.size(); cx++)
    {
      // Cell values [0,inf]:
      double dir_xy = *windGrid_direction.cellByPos(xs[cx], ys[cy]);
      double mod_xy = *windGrid_module.cellByPos(xs[cx], ys[cy]);

      auto obj = mrpt::opengl::CArrow::Create(
          xs[cx], ys[cy], 0.f, xs[cx] + scale * (float)cos(dir_xy),
          ys[cy] + scale * (float)sin(dir_xy), 0.f, 1.15f * scale, 0.3f * scale, 0.35f * scale);

      float r, g, b;
      jet2rgb(mod_xy, r, g, b);
      obj->setColor(r, g, b);

      windObj->insert(obj);
    }
  }
}

/*---------------------------------------------------------------
            increaseUncertainty
---------------------------------------------------------------*/
void CGasConcentrationGridMap2D::increaseUncertainty(const double STD_increase_value)
{
  // Increase cell variance
  //	unsigned int cx,cy;
  //	double memory_retention;

  m_hasToRecoverMeanAndCov = true;
  for (size_t it = 0; it < m_map.size(); it++)
  {
    m_stackedCov(it, 0) = m_stackedCov(it, 0) + STD_increase_value;
  }

  // Update m_map.kf_std
  recoverMeanAndCov();
}

/*---------------------------------------------------------------
            simulateAdvection
---------------------------------------------------------------*/
bool CGasConcentrationGridMap2D::simulateAdvection(double STD_increase_value)
{
  /* 1- Ensure we can use Wind Information
  -------------------------------------------------*/
  if (!insertionOptions.useWindInformation) return false;

  // Get time since last simulation
  double At = mrpt::system::timeDifference(timeLastSimulated, mrpt::Clock::now());
  cout << endl << " - At since last simulation = " << At << "seconds" << endl;
  // update time of last updated.
  timeLastSimulated = mrpt::Clock::now();

  /* 3- Build Transition Matrix (SA)
  This Matrix contains the probabilities of each cell
  to "be displaced" to other cells by the wind effect.
  ------------------------------------------------------*/
  mrpt::system::CTicTac tictac;
  size_t i = 0, c = 0;
  int cell_i_cx, cell_i_cy;
  float mu_phi, mu_r, mu_modwind;
  const size_t N = m_map.size();
  mrpt::math::CMatrixF A(N, N);
  A.fill(0.0);
  // std::vector<double> row_sum(N,0.0);
  auto* row_sum = (double*)calloc(N, sizeof(double));

  try
  {
    // Ensure map dimensions match with wind map
    unsigned int wind_map_size = windGrid_direction.getSizeX() * windGrid_direction.getSizeY();
    ASSERT_(wind_map_size == windGrid_module.getSizeX() * windGrid_module.getSizeY());
    if (N != wind_map_size)
    {
      cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND INFORMATION " << endl;
      // mrpt::system::pause();
    }

    tictac.Tic();

    // Generate Sparse Matrix of the wind advection SA
    for (i = 0; i < N; i++)
    {
      // Cell_i indx and coordinates
      idx2cxcy(i, cell_i_cx, cell_i_cy);

      // Read dirwind value of cell i
      mu_phi = *windGrid_direction.cellByIndex(cell_i_cx, cell_i_cy);  //[0,2*pi]
      unsigned int phi_indx = round(mu_phi / LUT.phi_inc);

      // Read modwind value of cell i
      mu_modwind = *windGrid_module.cellByIndex(cell_i_cx, cell_i_cy);  //[0,inf)
      mu_r = mu_modwind * At;
      if (mu_r > LUT.max_r) mu_r = LUT.max_r;
      unsigned int r_indx = round(mu_r / LUT.r_inc);

      // Evaluate LUT
      ASSERT_(phi_indx < LUT.phi_count);
      ASSERT_(r_indx < LUT.r_count);

      // define label
      vector<TGaussianCell>& cells_to_update = LUT_TABLE[phi_indx][r_indx];

      // Generate Sparse Matrix with the wind weights "SA"
      for (auto& ci : cells_to_update)
      {
        int final_cx = cell_i_cx + ci.cx;
        int final_cy = cell_i_cy + ci.cy;
        // Check if affected cells is within the map
        if ((final_cx >= 0) && (final_cx < (int)getSizeX()) && (final_cy >= 0) &&
            (final_cy < (int)getSizeY()))
        {
          int final_idx = final_cx + final_cy * getSizeX();

          // Add Value to SA Matrix
          if (ci.value != 0.0)
          {
            A(final_idx, i) = ci.value;
            row_sum[final_idx] += ci.value;
          }
        }
      }  // end-for ci
    }    // end-for cell i

    cout << " - SA matrix computed in " << tictac.Tac() << "s" << endl << endl;
  }
  catch (const std::exception& e)
  {
    cout << " #########  EXCEPTION computing Transition Matrix (A) "
            "##########\n: "
         << e.what() << endl;
    cout << "on cell i= " << i << "  c=" << c << endl << endl;
    return false;
  }

  /* Update Mean + Variance as a Gaussian Mixture
  ------------------------------------------------*/
  try
  {
    tictac.Tic();
    // std::vector<double> new_means(N,0.0);
    auto* new_means = (double*)calloc(N, sizeof(double));
    // std::vector<double> new_variances(N,0.0);
    auto* new_variances = (double*)calloc(N, sizeof(double));

    for (size_t it_i = 0; it_i < N; it_i++)
    {
      //--------
      // mean
      //--------
      for (size_t it_j = 0; it_j < N; it_j++)
      {
        if (m_map[it_j].kf_mean() != 0 && A(it_i, it_j) != 0)
        {
          if (row_sum[it_i] >= 1)
            new_means[it_i] += (A(it_i, it_j) / row_sum[it_i]) * m_map[it_j].kf_mean();
          else
            new_means[it_i] += A(it_i, it_j) * m_map[it_j].kf_mean();
        }
      }

      //----------
      // variance
      //----------
      // Consider special case (borders cells)
      if (row_sum[it_i] < 1)
        new_variances[it_i] = (1 - row_sum[it_i]) * square(insertionOptions.KF_initialCellStd);

      for (size_t it_j = 0; it_j < N; it_j++)
      {
        if (A(it_i, it_j) != 0)
        {
          if (row_sum[it_i] >= 1)
            new_variances[it_i] +=
                (A(it_i, it_j) / row_sum[it_i]) *
                (m_stackedCov(it_j, 0) + square(m_map[it_j].kf_mean() - new_means[it_i]));
          else
            new_variances[it_i] +=
                A(it_i, it_j) *
                (m_stackedCov(it_j, 0) + square(m_map[it_j].kf_mean() - new_means[it_i]));
        }
      }
    }

    // Update means and Cov of the Kalman filter state
    for (size_t it_i = 0; it_i < N; it_i++)
    {
      m_map[it_i].kf_mean() = new_means[it_i];  // means

      // Variances
      // Scale the Current Covariances with the new variances
      for (size_t it_j = 0; it_j < N; it_j++)
      {
        m_stackedCov(it_i, it_j) = (m_stackedCov(it_i, it_j) / m_stackedCov(it_i, it_i)) *
                                   new_variances[it_i];  // variances
        m_stackedCov(it_j, it_i) = m_stackedCov(it_i, it_j);
      }
    }
    m_hasToRecoverMeanAndCov = true;
    recoverMeanAndCov();

    cout << " - Mean&Var updated in " << tictac.Tac() << "s" << endl;

    // Free Memory
    free(row_sum);
    free(new_means);
    free(new_variances);
  }
  catch (const std::exception& e)
  {
    cout << " #########  EXCEPTION Updating Covariances ##########\n: " << e.what() << endl;
    cout << "on row i= " << i << "  column c=" << c << endl << endl;
    return false;
  }

  // cout << " Increasing general STD..." << endl;
  increaseUncertainty(STD_increase_value);

  return true;
}

/*---------------------------------------------------------------
            build_Gaussian_Wind_Grid
---------------------------------------------------------------*/

bool CGasConcentrationGridMap2D::build_Gaussian_Wind_Grid()
/** Builds a LookUp table with the values of the Gaussian Weights result of the
wind advection
*   for a specific condition.
*
*	The LUT contains the values of the Gaussian Weigths and the references to
the cell indexes to be applied.
*	Since the LUT is independent of the wind direction and angle, it generates
the Gaussian Weights for different configurations
*	of wind angle and module values.
*
*	To increase precission, each cell of the grid is sub-divided in subcells of
smaller size.

*	cell_i --> Cell origin (We consider our reference system in the bottom left
corner of cell_i ).
         Is the cell that contains the gas measurement which will be
propagated by the wind.
         The wind propagates in the shape of a 2D Gaussian with center in
the target cell (cell_j)
*	cell_j --> Target cell. Is the cell where falls the center of the Gaussian
that models the propagation of the gas comming from cell_i.
*/

{
  cout << endl << "---------------------------------" << endl;
  cout << " BUILDING GAUSSIAN WIND WEIGHTS " << endl;
  cout << "---------------------------------" << endl << endl;

  //-----------------------------
  //          PARAMS
  //-----------------------------
  LUT.resolution = getResolution();                  // resolution of the grid-cells (m)
  LUT.std_phi = insertionOptions.std_windNoise_phi;  // Standard Deviation in wind Angle (cte)
  LUT.std_r = insertionOptions.std_windNoise_mod /
              insertionOptions.advectionFreq;  // Standard Deviation in wind module (cte)
  std::string filename = format(
      "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz", LUT.resolution, LUT.std_phi,
      LUT.std_r);

  // Fixed Params:
  LUT.phi_inc = M_PIf / 8;                            // Increment in the wind Angle. (rad)
  LUT.phi_count = round(2 * M_PI / LUT.phi_inc) + 1;  // Number of angles to generate
  LUT.r_inc = 0.1f;                                   // Increment in the wind Module. (m)
  LUT.max_r = 2;                                      // maximum distance (m) to simulate
  LUT.r_count = round(LUT.max_r / LUT.r_inc) + 1;     // Number of wind modules to simulate

  LUT.table = new vector<vector<vector<TGaussianCell>>>(
      LUT.phi_count, vector<vector<TGaussianCell>>(LUT.r_count, vector<TGaussianCell>()));

  // LUT.table = new
  // vector<vector<vector<vector<vector<TGaussianCell>>>>>(LUT.subcell_count,
  // vector<vector<vector<vector<TGaussianCell>>>>(LUT.subcell_count,
  // vector<vector<vector<TGaussianCell>>>(LUT.phi_count,
  // vector<vector<TGaussianCell>>(LUT.r_count,vector<TGaussianCell>()) ) ) );

  //-----------------------------
  //    Check if file exists
  //-----------------------------

  cout << "Looking for file: " << filename.c_str() << endl;

  if (mrpt::system::fileExists(filename.c_str()))
  {
    // file exists. Load lookUptable from file
    cout << "LookUp table found for this configuration. Loading..." << endl;
    return load_Gaussian_Wind_Grid_From_File();
  }
  else
  {
    // file does not exists. Generate LookUp table.
    cout << "LookUp table NOT found. Generating table..." << endl;

    bool debug = true;
    FILE* debug_file;

    if (debug)
    {
      debug_file = fopen("simple_LUT.txt", "w");
      fprintf(debug_file, " phi_inc = %.4f \n r_inc = %.4f \n", LUT.phi_inc, LUT.r_inc);
      fprintf(debug_file, " std_phi = %.4f \n std_r = %.4f \n", LUT.std_phi, LUT.std_r);
      fprintf(debug_file, "[ phi ] [ r ] ---> (cx,cy)=Value\n");
      fprintf(debug_file, "----------------------------------\n");
    }

    // For the different possible angles (phi)
    for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
    {
      // mean of the phi value
      float phi = phi_indx * LUT.phi_inc;

      // For the different and possibe wind modules (r)
      for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
      {
        // mean of the radius value
        float r = r_indx * LUT.r_inc;

        if (debug)
        {
          fprintf(debug_file, "\n[%.2f] [%.2f] ---> ", phi, r);
        }

        // Estimates Cell_i_position
        // unsigned int cell_i_cx = 0;
        // unsigned int cell_i_cy = 0;
        float cell_i_x = LUT.resolution / 2.0;
        float cell_i_y = LUT.resolution / 2.0;

        // Estimate target position according to the mean value of wind.
        // float x_final = cell_i_x + r*cos(phi);
        // float y_final = cell_i_y + r*sin(phi);

        // Determine cell_j coordinates respect to origin_cell
        // int cell_j_cx = static_cast<int>(floor(
        // (x_final)/LUT.resolution ));
        // int cell_j_cy = static_cast<int>(floor(
        // (y_final)/LUT.resolution ));
        // Center of cell_j
        // float cell_j_x = (cell_j_cx+0.5f)*LUT.resolution;
        // float cell_j_y = (cell_j_cy+0.5f)*LUT.resolution;
        // left bottom corner of cell_j
        // float cell_j_xmin = cell_j_x - LUT.resolution/2.0;
        // float cell_j_ymin = cell_j_y - LUT.resolution/2.0;

        /* ---------------------------------------------------------------------------------
          Generate bounding-box  (+/- 3std) to determine which cells
        to update
        ---------------------------------------------------------------------------------*/
        std::vector<double> vertex_x, vertex_y;
        vertex_x.resize(14);
        vertex_y.resize(14);
        // Bounding-Box initialization
        double minBBox_x = 1000;
        double maxBBox_x = -1000;
        double minBBox_y = 1000;
        double maxBBox_y = -1000;

        // Consider special case for high uncertainty in PHI. The shape
        // of the polygon is a donut.
        double std_phi_BBox = LUT.std_phi;
        if (std_phi_BBox > M_PI / 3)
        {
          std_phi_BBox = M_PI / 3;  // To avoid problems generating
                                    // the bounding box. For std>pi/3
                                    // the shape is always a donut.
        }

        // Calculate bounding box limits
        size_t indx = 0;
        int sr = 3;
        for (int sd = (-3); sd <= (3); sd++)
        {
          vertex_x[indx] = cell_i_x + (r + sr * LUT.std_r) * cos(phi + sd * std_phi_BBox);
          if (vertex_x[indx] < minBBox_x) minBBox_x = vertex_x[indx];
          if (vertex_x[indx] > maxBBox_x) maxBBox_x = vertex_x[indx];

          vertex_y[indx] = cell_i_y + (r + sr * LUT.std_r) * sin(phi + sd * std_phi_BBox);
          if (vertex_y[indx] < minBBox_y) minBBox_y = vertex_y[indx];
          if (vertex_y[indx] > maxBBox_y) maxBBox_y = vertex_y[indx];

          indx++;
        }
        sr = -3;
        for (int sd = (3); sd >= (-3); sd--)
        {
          vertex_x[indx] = cell_i_x + (r + sr * LUT.std_r) * cos(phi + sd * std_phi_BBox);
          if (vertex_x[indx] < minBBox_x) minBBox_x = vertex_x[indx];
          if (vertex_x[indx] > maxBBox_x) maxBBox_x = vertex_x[indx];

          vertex_y[indx] = cell_i_y + (r + sr * LUT.std_r) * sin(phi + sd * std_phi_BBox);
          if (vertex_y[indx] < minBBox_y) minBBox_y = vertex_y[indx];
          if (vertex_y[indx] > maxBBox_y) maxBBox_y = vertex_y[indx];

          indx++;
        }

        /* ------------------------------------------------------------------------
           Determine range of cells to update according to the
        Bounding-Box limits.
           Origin cell is cx=cy= 0   x[0,res), y[0,res)
        ---------------------------------------------------------------------------*/
        int min_cx = static_cast<int>(floor(minBBox_x / LUT.resolution));
        int max_cx = static_cast<int>(floor(maxBBox_x / LUT.resolution));
        int min_cy = static_cast<int>(floor(minBBox_y / LUT.resolution));
        int max_cy = static_cast<int>(floor(maxBBox_y / LUT.resolution));

        int num_cells_affected = (max_cx - min_cx + 1) * (max_cy - min_cy + 1);

        if (num_cells_affected == 1)
        {
          // Concentration of cell_i moves to cell_a (cx,cy)
          TGaussianCell gauss_info;
          gauss_info.cx = min_cx;  // since max_cx == min_cx
          gauss_info.cy = min_cy;
          gauss_info.value = 1;  // prob = 1

          // Add cell volume to LookUp Table
          LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);

          if (debug)
          {
            // Save to file (debug)
            fprintf(debug_file, "(%d,%d)=%.4f", gauss_info.cx, gauss_info.cy, gauss_info.value);
          }
        }
        else
        {
          // Estimate volume of the Gaussian under each affected cell

          float subcell_pres = LUT.resolution / 10;
          // Determine the number of subcells inside the Bounding-Box
          const int BB_x_subcells = (int)(floor((maxBBox_x - minBBox_x) / subcell_pres) + 1);
          const int BB_y_subcells = (int)(floor((maxBBox_y - minBBox_y) / subcell_pres) + 1);

          double subcell_pres_x = (maxBBox_x - minBBox_x) / BB_x_subcells;
          double subcell_pres_y = (maxBBox_y - minBBox_y) / BB_y_subcells;

          // Save the W value of each cell using a map
          std::map<std::pair<int, int>, float> w_values;
          std::map<std::pair<int, int>, float>::iterator it;
          float sum_w = 0;

          for (int scy = 0; scy < BB_y_subcells; scy++)
          {
            for (int scx = 0; scx < BB_x_subcells; scx++)
            {
              // P-Subcell coordinates (center of the p-subcell)
              float subcell_a_x = minBBox_x + (scx + 0.5f) * subcell_pres_x;
              float subcell_a_y = minBBox_y + (scy + 0.5f) * subcell_pres_y;

              // distance and angle between cell_i and subcell_a
              float r_ia = sqrt(square(subcell_a_x - cell_i_x) + square(subcell_a_y - cell_i_y));
              float phi_ia = atan2(subcell_a_y - cell_i_y, subcell_a_x - cell_i_x);

              // Volume Approximation of subcell_a (Gaussian
              // Bivariate)
              float w = (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
                        exp(-0.5 * (square(r_ia - r) / square(LUT.std_r) +
                                    square(phi_ia - phi) / square(LUT.std_phi)));
              w += (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
                   exp(-0.5 * (square(r_ia - r) / square(LUT.std_r) +
                               square(phi_ia + 2 * M_PI - phi) / square(LUT.std_phi)));
              w += (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
                   exp(-0.5 * (square(r_ia - r) / square(LUT.std_r) +
                               square(phi_ia - 2 * M_PI - phi) / square(LUT.std_phi)));

              // Since we work with a cell grid, approximate the
              // weight of the gaussian by the volume of the
              // subcell_a
              if (r_ia != 0.0) w = (w * (subcell_pres_x * subcell_pres_y) / r_ia);

              // Determine cell index of the current subcell
              int cell_cx = static_cast<int>(floor(subcell_a_x / LUT.resolution));
              int cell_cy = static_cast<int>(floor(subcell_a_y / LUT.resolution));

              // Save w value
              it = w_values.find(std::make_pair(cell_cx, cell_cy));
              if (it != w_values.end())  // already exists
                w_values[std::make_pair(cell_cx, cell_cy)] += w;
              else
                w_values[std::make_pair(cell_cx, cell_cy)] = w;

              sum_w = sum_w + w;
            }  // end-for scx
          }    // end-for scy

          // SAVE to LUT
          for (it = w_values.begin(); it != w_values.end(); it++)
          {
            float w_final = (it->second) / sum_w;  // normalization to 1

            if (w_final >= 0.001)
            {
              // Save the weight of the gaussian volume for cell_a
              // (cx,cy)
              TGaussianCell gauss_info;
              gauss_info.cx = it->first.first;
              gauss_info.cy = it->first.second;
              gauss_info.value = w_final;

              // Add cell volume to LookUp Table
              LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);

              if (debug)
              {
                // Save to file (debug)
                fprintf(
                    debug_file, "(%d,%d)=%.6f    ", gauss_info.cx, gauss_info.cy, gauss_info.value);
              }
            }
          }

        }  // end-if only one affected cell

      }  // end-for r
    }    // end-for phi

    if (debug) fclose(debug_file);

    // Save LUT to File
    return save_Gaussian_Wind_Grid_To_File();

  }  // end-if table not available
}

bool CGasConcentrationGridMap2D::save_Gaussian_Wind_Grid_To_File()
{
  // Save LUT to file
  cout << "Saving to File ....";

  CFileGZOutputStream fo(format(
      "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz", LUT.resolution, LUT.std_phi,
      LUT.std_r));
  if (!fo.fileOpenCorrectly())
  {
    return false;
    cout << "WARNING: Gaussian_Wind_Weights file NOT SAVED" << endl;
  }
  auto f = mrpt::serialization::archiveFrom(fo);

  try
  {
    // Save params first
    f << LUT.resolution;  // cell resolution used
    f << LUT.std_phi;     // std_phi used
    f << LUT.std_r;

    f << LUT.phi_inc;  // rad
    f << (float)LUT.phi_count;
    f << LUT.r_inc;  // m
    f << LUT.max_r;  // maximum distance (m)
    f << (float)LUT.r_count;

    // Save Multi-table
    // vector< vector< vector<TGaussianCell>>>>> *table;

    for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
    {
      for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
      {
        // save all cell values.
        size_t N = LUT_TABLE[phi_indx][r_indx].size();
        f << (float)N;

        for (size_t i = 0; i < N; i++)
        {
          f << (float)LUT_TABLE[phi_indx][r_indx][i].cx;
          f << (float)LUT_TABLE[phi_indx][r_indx][i].cy;
          f << LUT_TABLE[phi_indx][r_indx][i].value;
        }
      }
    }
    cout << "DONE" << endl;
    return true;
  }
  catch (const std::exception& e)
  {
    cout << endl << "------------------------------------------------------------" << endl;
    cout << "EXCEPTION WHILE SAVING LUT TO FILE" << endl;
    cout << "Exception = " << e.what() << endl;
    return false;
  }
}

bool CGasConcentrationGridMap2D::load_Gaussian_Wind_Grid_From_File()
{
  // LOAD LUT from file
  cout << "Loading from File ....";

  try
  {
    CFileGZInputStream fi(format(
        "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz", LUT.resolution, LUT.std_phi,
        LUT.std_r));
    if (!fi.fileOpenCorrectly())
    {
      cout << "WARNING WHILE READING FROM: Gaussian_Wind_Weights" << endl;
      return false;
    }
    auto f = mrpt::serialization::archiveFrom(fi);

    float t_float;
    unsigned int t_uint;
    // Ensure params from file are correct with the specified in the ini
    // file
    f >> t_float;
    ASSERT_(LUT.resolution == t_float);

    f >> t_float;
    ASSERT_(LUT.std_phi == t_float);

    f >> t_float;
    ASSERT_(LUT.std_r == t_float);

    f >> t_float;
    ASSERT_(LUT.phi_inc == t_float);

    f >> t_float;
    t_uint = (unsigned int)t_float;
    ASSERT_(LUT.phi_count == t_uint);

    f >> t_float;
    ASSERT_(LUT.r_inc == t_float);

    f >> t_float;
    ASSERT_(LUT.max_r == t_float);

    f >> t_float;
    t_uint = (unsigned int)t_float;
    ASSERT_(LUT.r_count == t_uint);

    // Load Multi-table
    // vector< vector< vector<TGaussianCell>>>>> *table;

    for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
    {
      for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
      {
        // Number of cells to update
        size_t N;
        f >> t_float;
        N = (size_t)t_float;

        for (size_t i = 0; i < N; i++)
        {
          TGaussianCell gauss_info;
          f >> t_float;
          gauss_info.cx = (int)t_float;

          f >> t_float;
          gauss_info.cy = (int)t_float;

          f >> gauss_info.value;

          // Add cell volume to LookUp Table
          LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
        }
      }
    }
    cout << "DONE" << endl;
    return true;
  }
  catch (const std::exception& e)
  {
    cout << endl << "------------------------------------------------------------" << endl;
    cout << "EXCEPTION WHILE LOADING LUT FROM FILE" << endl;
    cout << "Exception = " << e.what() << endl;
    return false;
  }
}
