/**
 * After running build_vocab_tree, this code will turn the resulting descriptors
 * into a bag of words (BOW)
 */

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/contrib/contrib.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include <cstdio>

using namespace cv;
using std::string;

static cv::Mat LoadVocabFile(const std::string &vocab_file) {
  //Hardcoded vocab data dir
  string data_dir = "./";
  string vocab_path = data_dir + vocab_file;

  //load/generate vocab
  printf("Loading Vocabulary: %s\n", vocab_path.c_str());
  FileStorage fs;
  fs.open(vocab_path.c_str(), FileStorage::READ);

  Mat vocab;
  fs["Vocabulary"] >> vocab;
  if (vocab.empty()) {
    printf("Vocabulary not found\n");
    return cv::Mat();
  }
  printf("Vocabulary loaded\n");
  fs.release();

  return vocab;
}

static bool WriteTrainingData(string filename,
    const vector<Mat> visualword_training) {
  cv::Mat training_mapdata;
  vconcat(visualword_training, training_mapdata);
  printf("Saving training data...\n");
  FileStorage fs(filename, FileStorage::WRITE);
  if (fs.isOpened()) {
    fs << "BOWImageDescs" << training_mapdata;
    return true;
  }
  return false;
}

static void help()
{
  printf("\n\n"
  "Usage:\n"
  "   ./build_training_map <vocab file name> <directory to put logged images into> [<camera device-id>]\n"
  "<camera-device-id> is optional - should be a non-negative integer. default camera is zero"
  "\n"
  "Press space collect each image to put into the map\n"
  "ESC will end\n"
     "\n\n");
}

static void make_img_directory(string *img_save_dir) {
  if ((*img_save_dir)[img_save_dir->length() - 1] != '/') {
      *img_save_dir += "/";
    }
    boost::filesystem::path dir(*img_save_dir);
    if (boost::filesystem::exists(dir)) {
	  printf("Sequence directory already exists. Change dir name. Aborting.\n");
	  exit(-1);
    }
    if (boost::filesystem::create_directory(dir)) {
	  printf("Directory '%s' created\n", img_save_dir->c_str());
    }
}

int main(int argc, char *argv[]) {
  string vocab_file;
  string img_save_dir;
  int cam_deviceid = 0;
  if (argc == 1) {
    help();
    return 0;
  } else if (argc == 2) {
    if(!strcmp(argv[1],"-h") || !strcmp(argv[1],"--help")) {
      help();
      return(0);
    }
    vocab_file = string(argv[1]);
    img_save_dir = "./";
  } else if (argc == 3) {
    vocab_file = string(argv[1]);
    img_save_dir = string(argv[2]);
    make_img_directory(&img_save_dir);
  } else if (argc == 4) {
    vocab_file = string(argv[1]);
    img_save_dir = string(argv[2]);
    make_img_directory(&img_save_dir);
    cam_deviceid = boost::lexical_cast<int>(argv[3]);
  }

  cv::Mat vocabulary = LoadVocabFile(vocab_file);
  if (vocabulary.empty()) {
    printf("aborting\n");
    exit(-1);
  }

  Ptr<FeatureDetector> detector =
    new DynamicAdaptedFeatureDetector(
      AdjusterAdapter::create("SURF"), 100, 130, 5);
  Ptr<DescriptorExtractor> extractor =
   // DescriptorExtractor::create("SIFT");
     new SurfDescriptorExtractor(1000, 4, 2, false, true);
  Ptr<DescriptorMatcher> matcher =
    DescriptorMatcher::create("FlannBased");

  BOWImgDescriptorExtractor bide(extractor, matcher);
  bide.setVocabulary(vocabulary);
  vector<Mat> visualword_training;

  // read images from camera
  cv::VideoCapture cap(cam_deviceid);
  if (!cap.isOpened()) {
    printf("Opening the default camera did not succeed\n");
    return -1;
  }

  int framenum = 0;
  for (;;) {
    Mat frame;
    cap >> frame;
    imshow("input RGB image", frame);
    int keyp = waitKey(30);
    if (keyp == 27) {
      printf("Done capturing.\n");
      break;
    } else if (keyp == 'h') {
      help();
    } else if (keyp == 32) {
      Mat visualword_descriptor;
      vector<KeyPoint> kpts;
      detector->detect(frame, kpts);
      bide.compute(frame, kpts, visualword_descriptor);

      visualword_training.push_back(visualword_descriptor);

      printf("Saving frame\n");
      string frame_filename;

      // TODO: Hardcoded prefix
      frame_filename = img_save_dir + "training_%06d.png";
      cv::imwrite(cv::format(frame_filename.c_str(), framenum), frame);
      framenum++;
    }
  }

  string filename = "training_data.yml";
  if (!WriteTrainingData(filename, visualword_training)) {
    printf("Error: file %s cannot to opened to write\n", filename.c_str());
    exit(-1);
  }

  return 0;
}


#ifndef STAN_SERVICES_UTIL_CREATE_RNG_HPP
#define STAN_SERVICES_UTIL_CREATE_RNG_HPP

#include <boost/random/additive_combine.hpp>

namespace stan {
namespace services {
namespace util {

/**
 * Creates a pseudo random number generator from a random seed
 * and a chain id by initializing the PRNG with the seed and
 * then advancing past pow(2, 50) times the chain ID draws to
 * ensure different chains sample from different segments of the
 * pseudo random number sequence.
 *
 * Chain IDs should be kept to larger values than one to ensure
 * that the draws used to initialized transformed data are not
 * duplicated.
 *
 * @tparam RngType type of RNG to return, default is `boost::ecuyer1988`
 * @param[in] seed the random seed
 * @param[in] chain the chain id
 * @return an RNG instance
 */
template <typename RngType = boost::ecuyer1988>
inline RngType create_rng(unsigned int seed, unsigned int chain) {
  using boost::uintmax_t;
  static constexpr uintmax_t DISCARD_STRIDE = static_cast<uintmax_t>(1) << 50;
  RngType rng(seed);
  rng.discard(DISCARD_STRIDE * chain);
  return rng;
}

}  // namespace util
}  // namespace services
}  // namespace stan
#endif


// Copyright 2009-2010 Green Code LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <algorithm>
#include <cmath>
#include <iostream>

#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>

#include "tpsdemo/linalg3d-double.h"

#include "DisjointRegions.h"
#include "Geometry3D.h"
#include "Grid.h"
#include "IPointVector.h"
#include "LineIndent.h"
#include "ProgressBar.h"
#include "RasterSurface.h"

namespace mcc
{

  DisjointRegions::DisjointRegions()
    : iterationState_(RegionIteration_Done)  {
  }

  //---------------------------------------------------------------------------

  const unsigned int desiredPtsPerRegion = 12;  // Default in ArcGIS Spline

  //---------------------------------------------------------------------------

  // Represents a point that's in a neighboring region.
  struct NeighborPoint
  {
    const IPoint * point;
    double distance;

    NeighborPoint(const IPoint *             neighborPt,
                  const CoordinateInterval & xInterval,
                  const CoordinateInterval & yInterval)
      : point(neighborPt)
    {
      // Compute distance from region specified by x and y intervals.
      double xDistance = distanceToInterval(point->x(), xInterval);
      double yDistance = distanceToInterval(point->y(), yInterval);
      distance = std::max(xDistance, yDistance);
    }

    double distanceToInterval(Coordinate                 value,
                              const CoordinateInterval & interval)
    {
      if (value < interval.lowerBound())
        return interval.lowerBound() - value;
      else if (interval.upperBound() < value)
        return value - interval.upperBound();
      else
        return 0.0;
    }

    bool operator<(const NeighborPoint & rhs) const
    {
      return distance < rhs.distance;
    }
  };

  //---------------------------------------------------------------------------

  // Functor class for adding a neighbor point to a region's point list.
  class AppendPoint
  {
    public:
      AppendPoint(std::vector<const IPoint *> & points)
        : points_(points)
      {
      }

      void operator()(const NeighborPoint & neighbor) const
      {
        points_.push_back(neighbor.point);
      }

    private:
      std::vector<const IPoint *> & points_;
  };

  //---------------------------------------------------------------------------

  // Location of a cell's neighbor relative to the cell itself.
  struct RelativeLocation
  {
    int rowOffset;
    int columnOffset;

    RelativeLocation(int rOffset,
                     int cOffset)
      : rowOffset(rOffset),
        columnOffset(cOffset)
    {
    }

    RelativeLocation & operator+=(const RelativeLocation & relLoc)
    {
      rowOffset += relLoc.rowOffset;
      columnOffset += relLoc.columnOffset;
      return *this;
    }
  };

  //---------------------------------------------------------------------------

  // Get the neighboring region to a particular region (specified by its cell).
  // If the relative location of the neighbor is outside the grid of regions,
  // 0 is returned.

  const InterpolationRegion * getNeighbor(Grid<InterpolationRegion> &  regions,
                                          const Cell &                 cell,
                                          const RelativeLocation       neighborLocation)
  {
    long row = cell.row() + neighborLocation.rowOffset;
    if (row < 0 || row >= (long) regions.rows())
      return 0;
    long column = cell.column() + neighborLocation.columnOffset;
    if (column < 0 || column >= (long) regions.columns())
      return 0;
    return &(regions((unsigned int) row, (unsigned int) column));
  }

  //---------------------------------------------------------------------------

  // Get points from the regions in the outermost ring of a region's
  // neighborhood.  The ring is traversed in clock-wise order:
  //
  //   +---+---+---+---+---+
  //   | P | A | B | C | D |
  //   +---+---+---+---+---+
  //   | O | 8 | 1 | 2 | E |
  //   +---+---+---+---+---+
  //   | N | 7 | * | 3 | F |
  //   +---+---+---+---+---+
  //   | M | 6 | 5 | 4 | G |
  //   +---+---+---+---+---+
  //   | L | K | J | I | H |
  //   +---+---+---+---+---+
  //
  //  For the region marked "*" in the diagram above, if its neighborhood is
  //  3x3, then the eight regions marked 1 to 8 are traversed.  If the
  //  neighborhood is 5x5, then the regions marked "A" to "P" are traversed.
  //
  //  The points in the traversed neighbors are appended to the "neighborPts"
  //  parameter.
  void getPointsFromOuterRing(Grid<InterpolationRegion> &  regions,
                              const Cell &                 cell,
                              int                          neighborhoodSize,
                              std::vector<NeighborPoint> & neighborPts)
  {
    CoordinateInterval xInterval = regions.getXInterval(cell.column()); // = cell.xInterval();
    CoordinateInterval yInterval = regions.getYInterval(cell.row());    // = cell.yInterval();

    RelativeLocation traverseRingAlongEdges[] = {
      RelativeLocation( 0,       right(1) ),  // top edge, left to right
      RelativeLocation( down(1), 0        ),  // right edge, top to bottom
      RelativeLocation( 0,       left(1)  ),  // bottom edge, right to left
      RelativeLocation( up(1),   0        ),  // left edge, bottom to top
    };

    // The upper left cell in current neighborhood ring
    int neighborhoodRadius = (neighborhoodSize - 1) / 2;
    RelativeLocation neighborLocation(up(neighborhoodRadius), left(neighborhoodRadius));

    int nCellsPerEdge = neighborhoodSize - 1;

    BOOST_FOREACH(const RelativeLocation & locOfNextNeighborAlongEdge, traverseRingAlongEdges) {
      for (int i = 0; i < nCellsPerEdge; ++i) {
        neighborLocation += locOfNextNeighborAlongEdge;
        const InterpolationRegion * neighbor = getNeighbor(regions, cell, neighborLocation);
        if (neighbor) {
          BOOST_FOREACH(const IPoint * point, neighbor->pts) {
            neighborPts.push_back(NeighborPoint(point, xInterval, yInterval));
          }
        }
      }
    }
  }

  //---------------------------------------------------------------------------

  int DisjointRegions::subdivide(const IPointVector &  points,
                                 PointSelector         pointSelector,
                                 const RasterSurface & raster)
  {
    // Subdivide the raster's area into non-overlapping regions.  The goal is
    // to have approximately the same # of points in each region.

    int desiredNumRegions = points.count() / desiredPtsPerRegion;
      // Rounding down because one less region means possibly more pts per
      // region.

    double rasterHeight = raster.rows() * raster.cellSize();
    double rasterWidth = raster.columns() * raster.cellSize();
    double rasterArea = rasterHeight * rasterWidth;
    double desiredRegionArea = rasterArea / desiredNumRegions;

    // The size of a region if it was the optimal shape = square
    double desiredRegionSize = std::sqrt(desiredRegionArea);

    // But the desired region size is not guaranteed to divide both the height
    // and width of the raster evenly.  So stretch the regions' heights and
    // widths until they evenly divide the raster's height and width,
    // respectively.  Stretching ensures that regions do not lose points.  The
    // resulting region shape is likely not to be square.
    int nRows = int(std::floor(rasterHeight / desiredRegionSize));
	if (nRows > int(raster.rows())){
		nRows = int(raster.rows());
	}
    double regionHeight = rasterHeight / nRows;

    int nColumns = int(std::floor(rasterWidth / desiredRegionSize));
	if (nColumns > int(raster.columns())){
		nColumns = int(raster.columns());
	}
    double regionWidth = rasterWidth / nColumns;

    // Create the 2-d arry of InterpolationRegion (nRows, nColumns)
    regions_ = boost::make_shared< Grid<InterpolationRegion> >(nRows, nColumns, raster.lowerLeft(),
                                                               Coordinate(regionHeight),
                                                               Coordinate(regionWidth));

    LineIndent indent("  ");

    // Sort points into the regions
    BOOST_FOREACH(const IPoint & point, points) {
      Cell cell = regions_->getCell(point.x(), point.y());
      if ((*pointSelector)(point))
        (*regions_)[cell].pts.push_back(& point);
      else
        (*regions_)[cell].nPtsNotSelected++;
    }

    // Determine the cell block for each region.
    int regionRow = regions_->topRow();
    Coordinate regionRow_minY = regions_->getYInterval(regionRow).lowerBound();
    BOOST_FOREACH(unsigned int rasterRow, raster.topToBottom()) {
      // If first (top) row, then scan across cell columns computing widths of
      // cell blocks.
      if (rasterRow == raster.topRow()) {
          const unsigned int regionTopRow = regions_->topRow();
          unsigned int regionColumn = regions_->leftColumn();
          InterpolationRegion * currentRegion = &( (*regions_)(regionTopRow, regionColumn) );
          Coordinate currentRegion_maxX = regions_->getXInterval(regionColumn).upperBound();
          BOOST_FOREACH(unsigned int rasterColumn, raster.leftToRight()) {
            Cell cell = raster.getCell(raster.topRow(), rasterColumn);
            if (rasterColumn == raster.leftColumn()) {
              currentRegion->cellBlock = CellBlock(cell /* upperLeft */);  // cell block height and width = 1
            }
            else if (cell.x() < currentRegion_maxX) {
              // The cell is in the current region, so increment the cell
              // block's width.
              currentRegion->cellBlock.width += 1;
            } else {
              // We've moved right into a new region.
              regionColumn += right(1);
              currentRegion = &( (*regions_)(regionTopRow, regionColumn) );
              currentRegion->cellBlock = CellBlock(cell /* upperLeft */);  // cell block height and width = 1
              currentRegion_maxX = regions_->getXInterval(regionColumn).upperBound();
            }
          }
      } else {
        // Current raster row is not the top row.  So two possibilities:
        //   A) the centers of the current row's cells are in the current
        //      region row, or
        //   B) the current row's cells in the next lower region row
        Cell leftMostCell = raster.getCell(rasterRow, raster.leftColumn());
        if (leftMostCell.y() > regionRow_minY) {
          // Case (A) - still in current region row, so go through the regions
          // on the current region row, and increment their cell block heights.
          BOOST_FOREACH(unsigned int regionColumn, regions_->leftToRight()) {
            (*regions_)(regionRow, regionColumn).cellBlock.height += 1;
          }
        } else {
          // Case (B) - moved down into new region row, so scan across region
          // columns, copying the cell block widths from the regions in the
          // top region row into their corresponding regions of the current
          // region row.
          regionRow += down(1);
          regionRow_minY = regions_->getYInterval(regionRow).lowerBound();
          BOOST_FOREACH(unsigned int regionColumn, regions_->leftToRight()) {
            const InterpolationRegion & correspondingRegionInTopRow = (*regions_)(regions_->topRow(), regionColumn);
            int upperLeftCell_column = correspondingRegionInTopRow.cellBlock.upperLeftCell.column();

            CellBlock & currentRegion_cellBlock = (*regions_)(regionRow, regionColumn).cellBlock;
            currentRegion_cellBlock.upperLeftCell = raster.getCell(rasterRow, upperLeftCell_column);
            currentRegion_cellBlock.height = 1;
            currentRegion_cellBlock.width = correspondingRegionInTopRow.cellBlock.width;
          }
        }  // else Case (B)
      }  // else current raster row not top row
    }  // for each raster row

    iterationState_ = RegionIteration_Initialized;
    raster_ = &(raster);

    return nRows * nColumns;
  }

  const Cell* DisjointRegions::getNextCell()
  {
    switch (iterationState_) {
      case RegionIteration_Initialized :
        // Return the upper left region
        currentRegionRow_ = regions_->topRow();
        currentRegionColumn_ = regions_->leftColumn();
        iterationState_ = RegionIteration_InProgress;
        break;

      case RegionIteration_InProgress :
        // Advance to next cell in row major order
        if (currentRegionColumn_ != regions_->rightColumn())
          currentRegionColumn_ += right(1);
        else {
          // Advance to next row
          if (currentRegionRow_ != regions_->bottomRow()) {
            currentRegionRow_ += down(1);
            currentRegionColumn_ = regions_->leftColumn();
          } else {
            iterationState_ = RegionIteration_Done;
            return 0;
          }
        }
        break;

      default :
        assert(iterationState_ == RegionIteration_Done);
        return 0;
    }

    return new Cell(regions_->getCell(currentRegionRow_, currentRegionColumn_));

  }

  const InterpolationRegion * DisjointRegions::getRegionForCell(const Cell *cell)
  {
    unsigned int row = cell->row();
    unsigned int column = cell->column();

    return &( (*regions_)(row, column) );

  }

    //---------------------------------------------------------------------------

  void addNeighborPointsToRegionWithCell(Grid<InterpolationRegion> &  regions,
		  const Cell & cell, std::vector<const IPoint *> &points, int nPoints,
    int &indexNextAvailableNeighbor, std::vector<NeighborPoint> &neighborPts,
    int &neighborhoodSize, int &nPointsLeftInOuterRing)
  {
    while (nPoints > 0) {
      while (nPointsLeftInOuterRing == 0) {
        // Expand the neighborhood, and get points from the new outer ring.
        neighborPts.clear();
        neighborhoodSize += 2;  // first 3x3, then 5x5, 7x7, ...
        getPointsFromOuterRing(regions, cell, neighborhoodSize, neighborPts);
        nPointsLeftInOuterRing = neighborPts.size();
        std::sort(neighborPts.begin(), neighborPts.begin());
        indexNextAvailableNeighbor = 0;
      }

      int nPtsToAdd = (nPointsLeftInOuterRing < nPoints) ? nPointsLeftInOuterRing : nPoints;
      std::for_each(neighborPts.begin() + indexNextAvailableNeighbor,
                    neighborPts.begin() + indexNextAvailableNeighbor + nPtsToAdd,
                    AppendPoint(points));
      indexNextAvailableNeighbor += nPtsToAdd;
      nPointsLeftInOuterRing -= nPtsToAdd;
      nPoints -= nPtsToAdd;
    }
  }

  void DisjointRegions::getPointsAndCellsForCell(const Cell *cell, int nExtraPoints,
    std::vector<const IPoint *> &points, std::vector<Cell> &cells)
  {
    const InterpolationRegion *region = getRegionForCell(cell);
    points = region->pts;
    int neighborhoodSize = 1;
    std::vector<NeighborPoint> neighborPts;
    neighborPts.clear();
    int nPointsLeftInOuterRing = 0;
    int indexNextAvailableNeighbor = 0;
    unsigned int nSelectedPts = points.size();

    while (nSelectedPts < desiredPtsPerRegion) {

      addNeighborPointsToRegionWithCell(*regions_, *cell, points, desiredPtsPerRegion - nSelectedPts,
        indexNextAvailableNeighbor, neighborPts, neighborhoodSize, nPointsLeftInOuterRing);
      nSelectedPts = points.size();

    }

    if(nExtraPoints > 0) {
      addNeighborPointsToRegionWithCell(*regions_, *cell, points, nExtraPoints,indexNextAvailableNeighbor,
        neighborPts, neighborhoodSize, nPointsLeftInOuterRing);
    }

    cells.clear();
    const CellBlock & cellBlock = region->cellBlock;
    const Cell & upperLeftCell = cellBlock.upperLeftCell;
    unsigned int blockTop    = upperLeftCell.row();
    unsigned int blockBottom = blockTop + down(cellBlock.height - 1);
    unsigned int blockLeft   = upperLeftCell.column();
    unsigned int blockRight  = blockLeft + right(cellBlock.width - 1);

    BOOST_FOREACH(unsigned int row, Sequence<unsigned int>(blockTop, blockBottom)) {
      BOOST_FOREACH(unsigned int column, Sequence<unsigned int>(blockLeft, blockRight)) {
        cells.push_back(raster_->getCell(row, column));
      }
    }
  }

}


// SPDX-License-Identifier: BSD-3-Clause
// Copyright (c) 2021 Scipp contributors (https://github.com/scipp)
#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include <vector>

#include "fix_typed_test_suite_warnings.h"
#include "test_macros.h"

#include "scipp/core/element/math.h"
#include "scipp/variable/except.h"
#include "scipp/variable/misc_operations.h"
#include "scipp/variable/operations.h"
#include "scipp/variable/variable.h"

using namespace scipp;
using namespace scipp::core;
using namespace scipp::variable;

template <typename T>
class VariableScalarOperatorTest : public ::testing::Test {
public:
  Variable variable{makeVariable<T>(Dims{Dim::X}, Shape{1}, Values{10})};
  const T scalar{2};

  T value() const { return this->variable.template values<T>()[0]; }
};

using ScalarTypes = ::testing::Types<double, float, int64_t, int32_t>;
TYPED_TEST_SUITE(VariableScalarOperatorTest, ScalarTypes);

TYPED_TEST(VariableScalarOperatorTest, plus_equals) {
  this->variable += this->scalar * units::one;
  EXPECT_EQ(this->value(), 12);
}

TYPED_TEST(VariableScalarOperatorTest, minus_equals) {
  this->variable -= this->scalar * units::one;
  EXPECT_EQ(this->value(), 8);
}

TYPED_TEST(VariableScalarOperatorTest, times_equals) {
  this->variable *= this->scalar * units::one;
  EXPECT_EQ(this->value(), 20);
}

TYPED_TEST(VariableScalarOperatorTest, divide_equals) {
  if (this->variable.dtype() == dtype<double> ||
      this->variable.dtype() == dtype<float>) {
    this->variable /= this->scalar * units::one;
    EXPECT_EQ(this->value(), 5);
  } else {
    EXPECT_THROW(this->variable /= this->scalar * units::one,
                 except::TypeError);
  }
}

TEST(Variable, operator_unary_minus) {
  const auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m, Values{1.1, 2.2});
  const auto expected = makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m,
                                             Values{-1.1, -2.2});
  auto b = -a;
  EXPECT_EQ(b, expected);
}

TEST(VariableView, unary_minus) {
  const auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m, Values{1.1, 2.2});
  const auto expected =
      makeVariable<double>(Dims(), Shape(), units::m, Values{-2.2});
  auto b = -a.slice({Dim::X, 1});
  EXPECT_EQ(b, expected);
}

TEST(Variable, operator_plus_equal) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.1, 2.2});

  ASSERT_NO_THROW(a += a);
  EXPECT_EQ(a.values<double>()[0], 2.2);
  EXPECT_EQ(a.values<double>()[1], 4.4);
}

TEST(Variable, operator_plus_equal_automatic_broadcast_of_rhs) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.1, 2.2});

  auto fewer_dimensions = makeVariable<double>(Values{1.0});

  ASSERT_NO_THROW(a += fewer_dimensions);
  EXPECT_EQ(a.values<double>()[0], 2.1);
  EXPECT_EQ(a.values<double>()[1], 3.2);
}

TEST(Variable, operator_plus_equal_transpose) {
  auto a = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 2}, units::m,
                                Values{1.0, 2.0, 3.0, 4.0, 5.0, 6.0});
  auto transpose =
      makeVariable<double>(Dims{Dim::X, Dim::Y}, Shape{2, 3}, units::m,
                           Values{1.0, 3.0, 5.0, 2.0, 4.0, 6.0});

  EXPECT_NO_THROW(a += transpose);
  ASSERT_EQ(a, makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 2}, units::m,
                                    Values{2.0, 4.0, 6.0, 8.0, 10.0, 12.0}));
}

TEST(Variable, operator_plus_equal_different_dimensions) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.1, 2.2});

  auto different_dimensions =
      makeVariable<double>(Dims{Dim::Y}, Shape{2}, Values{1.1, 2.2});
  EXPECT_THROW_MSG(a += different_dimensions, std::runtime_error,
                   "Expected (x: 2) to include (y: 2).");
}

TEST(Variable, operator_plus_equal_different_unit) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.1, 2.2});

  auto different_unit = copy(a);
  different_unit.setUnit(units::m);
  EXPECT_THROW(a += different_unit, except::UnitError);
}

TEST(Variable, operator_plus_equal_non_arithmetic_type) {
  auto a = makeVariable<std::string>(Dims{Dim::X}, Shape{1},
                                     Values{std::string("test")});
  EXPECT_THROW(a += a, except::TypeError);
}

TEST(Variable, operator_plus_equal_time_type) {
  using time_point = scipp::core::time_point;
  auto a = makeVariable<time_point>(Shape{1}, units::Unit{units::ns},
                                    Values{time_point{2}});
  EXPECT_THROW(a += static_cast<float>(1.0) * units::ns, except::TypeError);
  EXPECT_NO_THROW(a += static_cast<int64_t>(1) * units::ns);
  EXPECT_NO_THROW(a += static_cast<int32_t>(2) * units::ns);
  EXPECT_EQ(a, makeVariable<time_point>(Shape{1}, units::Unit{units::ns},
                                        Values{time_point{5}}));
}

TEST(Variable, operator_minus_equal_time_type) {
  using time_point = scipp::core::time_point;
  auto a = makeVariable<time_point>(Shape{1}, units::Unit{units::ns},
                                    Values{time_point{10}});
  EXPECT_THROW(a -= static_cast<float>(1.0) * units::ns, except::TypeError);
  EXPECT_NO_THROW(a -= static_cast<int64_t>(1) * units::ns);
  EXPECT_NO_THROW(a -= static_cast<int32_t>(2) * units::ns);
  EXPECT_EQ(a, makeVariable<time_point>(Shape{1}, units::Unit{units::ns},
                                        Values{time_point{7}}));
}

TEST(Variable, operator_plus_equal_different_variables_different_element_type) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{1}, Values{1.0});
  auto b = makeVariable<int64_t>(Dims{Dim::X}, Shape{1}, Values{2});
  EXPECT_NO_THROW(a += b);
}

TEST(Variable, operator_plus_equal_different_variables_same_element_type) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{1}, Values{1.0});
  auto b = makeVariable<double>(Dims{Dim::X}, Shape{1}, Values{2.0});
  EXPECT_NO_THROW(a += b);
  EXPECT_EQ(a.values<double>()[0], 3.0);
}

TEST(Variable, operator_plus_equal_scalar) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.1, 2.2});

  EXPECT_NO_THROW(a += 1.0 * units::one);
  EXPECT_EQ(a.values<double>()[0], 2.1);
  EXPECT_EQ(a.values<double>()[1], 3.2);
}

TEST(Variable, operator_plus_equal_custom_type) {
  auto a = makeVariable<float>(Dims{Dim::X}, Shape{2}, Values{1.1f, 2.2f});

  EXPECT_NO_THROW(a += a);
  EXPECT_EQ(a.values<float>()[0], 2.2f);
  EXPECT_EQ(a.values<float>()[1], 4.4f);
}

TEST(Variable, operator_plus_unit_fail) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.0, 2.0},
                                Variances{3.0, 4.0});
  a.setUnit(units::m);
  auto b = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.0, 2.0},
                                Variances{3.0, 4.0});
  b.setUnit(units::s);
  ASSERT_ANY_THROW_DISCARD(a + b);
  b.setUnit(units::m);
  ASSERT_NO_THROW_DISCARD(a + b);
}

TEST(Variable, operator_plus_eigen_type) {
  const auto var = makeVariable<Eigen::Vector3d>(
      Dims{Dim::X}, Shape{2},
      Values{Eigen::Vector3d{1.0, 2.0, 3.0}, Eigen::Vector3d{0.1, 0.2, 0.3}});
  const auto expected = makeVariable<Eigen::Vector3d>(
      Dims(), Shape(), Values{Eigen::Vector3d{1.1, 2.2, 3.3}});

  const auto result = var.slice({Dim::X, 0}) + var.slice({Dim::X, 1});

  EXPECT_EQ(result, expected);
}

TEST(Variable, operator_times_equal) {
  auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m, Values{2.0, 3.0});

  EXPECT_EQ(a.unit(), units::m);
  EXPECT_NO_THROW(a *= a);
  EXPECT_EQ(a.values<double>()[0], 4.0);
  EXPECT_EQ(a.values<double>()[1], 9.0);
  EXPECT_EQ(a.unit(), units::m * units::m);
}

TEST(Variable, operator_times_equal_scalar) {
  auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m, Values{2.0, 3.0});

  EXPECT_EQ(a.unit(), units::m);
  EXPECT_NO_THROW(a *= 2.0 * units::one);
  EXPECT_EQ(a.values<double>()[0], 4.0);
  EXPECT_EQ(a.values<double>()[1], 6.0);
  EXPECT_EQ(a.unit(), units::m);
}

TEST(Variable, operator_plus_equal_unit_fail_integrity) {
  auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2},
                           units::Unit(units::m * units::m), Values{2.0, 3.0});
  const auto expected(a);

  ASSERT_THROW(a += a * a, std::runtime_error);
  EXPECT_EQ(a, expected);
}

TEST(Variable, operator_times_can_broadcast) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{0.5, 1.5});
  auto b = makeVariable<double>(Dims{Dim::Y}, Shape{2}, Values{2.0, 3.0});

  auto ab = a * b;
  auto reference = makeVariable<double>(Dims{Dim::X, Dim::Y}, Shape{2, 2},
                                        Values{1.0, 1.5, 3.0, 4.5});
  EXPECT_EQ(ab, reference);
}

TEST(Variable, operator_divide_equal) {
  auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{2.0, 3.0});
  auto b = makeVariable<double>(Values{2.0});
  b.setUnit(units::m);

  EXPECT_NO_THROW(a /= b);
  EXPECT_EQ(a.values<double>()[0], 1.0);
  EXPECT_EQ(a.values<double>()[1], 1.5);
  EXPECT_EQ(a.unit(), units::one / units::m);
}

TEST(Variable, operator_divide_equal_self) {
  auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m, Values{2.0, 3.0});

  EXPECT_EQ(a.unit(), units::m);
  EXPECT_NO_THROW(a /= a);
  EXPECT_EQ(a.values<double>()[0], 1.0);
  EXPECT_EQ(a.values<double>()[1], 1.0);
  EXPECT_EQ(a.unit(), units::one);
}

TEST(Variable, operator_divide_equal_scalar) {
  auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m, Values{2.0, 4.0});

  EXPECT_EQ(a.unit(), units::m);
  EXPECT_NO_THROW(a /= 2.0 * units::one);
  EXPECT_EQ(a.values<double>()[0], 1.0);
  EXPECT_EQ(a.values<double>()[1], 2.0);
  EXPECT_EQ(a.unit(), units::m);
}

TEST(Variable, operator_divide_scalar_double) {
  const auto a =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, units::m, Values{2.0, 4.0});
  const auto result = 1.111 * units::one / a;
  EXPECT_EQ(result.values<double>()[0], 1.111 / 2.0);
  EXPECT_EQ(result.values<double>()[1], 1.111 / 4.0);
  EXPECT_EQ(result.unit(), units::one / units::m);
}

TEST(Variable, operator_divide_scalar_float) {
  const auto a =
      makeVariable<float>(Dims{Dim::X}, Shape{2}, units::m, Values{2.0, 4.0});
  const auto result = 1.111f * units::one / a;
  EXPECT_EQ(result.values<float>()[0], 1.111f / 2.0f);
  EXPECT_EQ(result.values<float>()[1], 1.111f / 4.0f);
  EXPECT_EQ(result.unit(), units::one / units::m);
}

TEST(Variable, operator_allowed_types) {
  auto i32 = makeVariable<int32_t>(Values{10});
  auto i64 = makeVariable<int64_t>(Values{10});
  auto f = makeVariable<float>(Values{0.5f});
  auto d = makeVariable<double>(Values{0.5});

  /* Can operate on higher precision from lower precision */
  EXPECT_NO_THROW(i64 += i32);
  EXPECT_NO_THROW(d += f);

  /* Can operate on lower precision from higher precision */
  EXPECT_NO_THROW(i32 += i64);
  EXPECT_NO_THROW(f += d);

  /* Expect promotion to double if one parameter is double */
  EXPECT_EQ(dtype<double>, (f + d).dtype());
  EXPECT_EQ(dtype<double>, (d + f).dtype());
}

TEST(VariableView, minus_equals_failures) {
  auto var = makeVariable<double>(Dims{Dim::X, Dim::Y}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});

  EXPECT_THROW_MSG(var -= var.slice({Dim::X, 0, 1}), std::runtime_error,
                   "Expected (x: 2, y: 2) to include (x: 1, y: 2).");
}

TEST(VariableView, self_overlapping_view_operation) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});

  var -= var.slice({Dim::Y, 0});
  const auto data = var.values<double>();
  EXPECT_EQ(data[0], 0.0);
  EXPECT_EQ(data[1], 0.0);
  // This is the critical part: After subtracting for y=0 the view points to
  // data containing 0.0, so subsequently the subtraction would have no effect
  // if self-overlap was not taken into account by the implementation.
  EXPECT_EQ(data[2], 2.0);
  EXPECT_EQ(data[3], 2.0);
}

TEST(VariableView, minus_equals_slice_const_outer) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});
  const auto copy = variable::copy(var);

  var -= copy.slice({Dim::Y, 0});
  const auto data = var.values<double>();
  EXPECT_EQ(data[0], 0.0);
  EXPECT_EQ(data[1], 0.0);
  EXPECT_EQ(data[2], 2.0);
  EXPECT_EQ(data[3], 2.0);
  var -= copy.slice({Dim::Y, 1});
  EXPECT_EQ(data[0], -3.0);
  EXPECT_EQ(data[1], -4.0);
  EXPECT_EQ(data[2], -1.0);
  EXPECT_EQ(data[3], -2.0);
}

TEST(VariableView, minus_equals_slice_outer) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});
  auto copy = variable::copy(var);

  var -= copy.slice({Dim::Y, 0});
  const auto data = var.values<double>();
  EXPECT_EQ(data[0], 0.0);
  EXPECT_EQ(data[1], 0.0);
  EXPECT_EQ(data[2], 2.0);
  EXPECT_EQ(data[3], 2.0);
  var -= copy.slice({Dim::Y, 1});
  EXPECT_EQ(data[0], -3.0);
  EXPECT_EQ(data[1], -4.0);
  EXPECT_EQ(data[2], -1.0);
  EXPECT_EQ(data[3], -2.0);
}

TEST(VariableView, minus_equals_slice_inner) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});
  auto copy = variable::copy(var);

  var -= copy.slice({Dim::X, 0});
  const auto data = var.values<double>();
  EXPECT_EQ(data[0], 0.0);
  EXPECT_EQ(data[1], 1.0);
  EXPECT_EQ(data[2], 0.0);
  EXPECT_EQ(data[3], 1.0);
  var -= copy.slice({Dim::X, 1});
  EXPECT_EQ(data[0], -2.0);
  EXPECT_EQ(data[1], -1.0);
  EXPECT_EQ(data[2], -4.0);
  EXPECT_EQ(data[3], -3.0);
}

TEST(VariableView, minus_equals_slice_of_slice) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});
  auto copy = variable::copy(var);

  var -= copy.slice({Dim::X, 1}).slice({Dim::Y, 1});
  const auto data = var.values<double>();
  EXPECT_EQ(data[0], -3.0);
  EXPECT_EQ(data[1], -2.0);
  EXPECT_EQ(data[2], -1.0);
  EXPECT_EQ(data[3], 0.0);
}

TEST(VariableView, minus_equals_nontrivial_slices) {
  auto source = makeVariable<double>(
      Dims{Dim::Y, Dim::X}, Shape{3, 3},
      Values{11.0, 12.0, 13.0, 21.0, 22.0, 23.0, 31.0, 32.0, 33.0});
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2});
    target -= source.slice({Dim::X, 0, 2}).slice({Dim::Y, 0, 2});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], -11.0);
    EXPECT_EQ(data[1], -12.0);
    EXPECT_EQ(data[2], -21.0);
    EXPECT_EQ(data[3], -22.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2});
    target -= source.slice({Dim::X, 1, 3}).slice({Dim::Y, 0, 2});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], -12.0);
    EXPECT_EQ(data[1], -13.0);
    EXPECT_EQ(data[2], -22.0);
    EXPECT_EQ(data[3], -23.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2});
    target -= source.slice({Dim::X, 0, 2}).slice({Dim::Y, 1, 3});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], -21.0);
    EXPECT_EQ(data[1], -22.0);
    EXPECT_EQ(data[2], -31.0);
    EXPECT_EQ(data[3], -32.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2});
    target -= source.slice({Dim::X, 1, 3}).slice({Dim::Y, 1, 3});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], -22.0);
    EXPECT_EQ(data[1], -23.0);
    EXPECT_EQ(data[2], -32.0);
    EXPECT_EQ(data[3], -33.0);
  }
}

TEST(VariableView, slice_inner_minus_equals) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});

  var.slice({Dim::X, 0}) -= var.slice({Dim::X, 1});
  const auto data = var.values<double>();
  EXPECT_EQ(data[0], -1.0);
  EXPECT_EQ(data[1], 2.0);
  EXPECT_EQ(data[2], -1.0);
  EXPECT_EQ(data[3], 4.0);
}

TEST(VariableView, slice_outer_minus_equals) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                  Values{1.0, 2.0, 3.0, 4.0});

  var.slice({Dim::Y, 0}) -= var.slice({Dim::Y, 1});
  const auto data = var.values<double>();
  EXPECT_EQ(data[0], -2.0);
  EXPECT_EQ(data[1], -2.0);
  EXPECT_EQ(data[2], 3.0);
  EXPECT_EQ(data[3], 4.0);
}

TEST(VariableView, nontrivial_slice_minus_equals) {
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                       Values{11.0, 12.0, 21.0, 22.0});
    target.slice({Dim::X, 0, 2}).slice({Dim::Y, 0, 2}) -= source;
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], -11.0);
    EXPECT_EQ(data[1], -12.0);
    EXPECT_EQ(data[2], 0.0);
    EXPECT_EQ(data[3], -21.0);
    EXPECT_EQ(data[4], -22.0);
    EXPECT_EQ(data[5], 0.0);
    EXPECT_EQ(data[6], 0.0);
    EXPECT_EQ(data[7], 0.0);
    EXPECT_EQ(data[8], 0.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                       Values{11.0, 12.0, 21.0, 22.0});
    target.slice({Dim::X, 1, 3}).slice({Dim::Y, 0, 2}) -= source;
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], 0.0);
    EXPECT_EQ(data[1], -11.0);
    EXPECT_EQ(data[2], -12.0);
    EXPECT_EQ(data[3], 0.0);
    EXPECT_EQ(data[4], -21.0);
    EXPECT_EQ(data[5], -22.0);
    EXPECT_EQ(data[6], 0.0);
    EXPECT_EQ(data[7], 0.0);
    EXPECT_EQ(data[8], 0.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                       Values{11.0, 12.0, 21.0, 22.0});
    target.slice({Dim::X, 0, 2}).slice({Dim::Y, 1, 3}) -= source;
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], 0.0);
    EXPECT_EQ(data[1], 0.0);
    EXPECT_EQ(data[2], 0.0);
    EXPECT_EQ(data[3], -11.0);
    EXPECT_EQ(data[4], -12.0);
    EXPECT_EQ(data[5], 0.0);
    EXPECT_EQ(data[6], -21.0);
    EXPECT_EQ(data[7], -22.0);
    EXPECT_EQ(data[8], 0.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                       Values{11.0, 12.0, 21.0, 22.0});
    target.slice({Dim::X, 1, 3}).slice({Dim::Y, 1, 3}) -= source;
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], 0.0);
    EXPECT_EQ(data[1], 0.0);
    EXPECT_EQ(data[2], 0.0);
    EXPECT_EQ(data[3], 0.0);
    EXPECT_EQ(data[4], -11.0);
    EXPECT_EQ(data[5], -12.0);
    EXPECT_EQ(data[6], 0.0);
    EXPECT_EQ(data[7], -21.0);
    EXPECT_EQ(data[8], -22.0);
  }
}

TEST(VariableView, nontrivial_slice_minus_equals_slice) {
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source =
        makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 3},
                             Values{666.0, 11.0, 12.0, 666.0, 21.0, 22.0});
    target.slice({Dim::X, 0, 2}).slice({Dim::Y, 0, 2}) -=
        source.slice({Dim::X, 1, 3});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], -11.0);
    EXPECT_EQ(data[1], -12.0);
    EXPECT_EQ(data[2], 0.0);
    EXPECT_EQ(data[3], -21.0);
    EXPECT_EQ(data[4], -22.0);
    EXPECT_EQ(data[5], 0.0);
    EXPECT_EQ(data[6], 0.0);
    EXPECT_EQ(data[7], 0.0);
    EXPECT_EQ(data[8], 0.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source =
        makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 3},
                             Values{666.0, 11.0, 12.0, 666.0, 21.0, 22.0});
    target.slice({Dim::X, 1, 3}).slice({Dim::Y, 0, 2}) -=
        source.slice({Dim::X, 1, 3});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], 0.0);
    EXPECT_EQ(data[1], -11.0);
    EXPECT_EQ(data[2], -12.0);
    EXPECT_EQ(data[3], 0.0);
    EXPECT_EQ(data[4], -21.0);
    EXPECT_EQ(data[5], -22.0);
    EXPECT_EQ(data[6], 0.0);
    EXPECT_EQ(data[7], 0.0);
    EXPECT_EQ(data[8], 0.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source =
        makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 3},
                             Values{666.0, 11.0, 12.0, 666.0, 21.0, 22.0});
    target.slice({Dim::X, 0, 2}).slice({Dim::Y, 1, 3}) -=
        source.slice({Dim::X, 1, 3});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], 0.0);
    EXPECT_EQ(data[1], 0.0);
    EXPECT_EQ(data[2], 0.0);
    EXPECT_EQ(data[3], -11.0);
    EXPECT_EQ(data[4], -12.0);
    EXPECT_EQ(data[5], 0.0);
    EXPECT_EQ(data[6], -21.0);
    EXPECT_EQ(data[7], -22.0);
    EXPECT_EQ(data[8], 0.0);
  }
  {
    auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{3, 3});
    auto source =
        makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 3},
                             Values{666.0, 11.0, 12.0, 666.0, 21.0, 22.0});
    target.slice({Dim::X, 1, 3}).slice({Dim::Y, 1, 3}) -=
        source.slice({Dim::X, 1, 3});
    const auto data = target.values<double>();
    EXPECT_EQ(data[0], 0.0);
    EXPECT_EQ(data[1], 0.0);
    EXPECT_EQ(data[2], 0.0);
    EXPECT_EQ(data[3], 0.0);
    EXPECT_EQ(data[4], -11.0);
    EXPECT_EQ(data[5], -12.0);
    EXPECT_EQ(data[6], 0.0);
    EXPECT_EQ(data[7], -21.0);
    EXPECT_EQ(data[8], -22.0);
  }
}

TEST(VariableView, slice_minus_lower_dimensional) {
  auto target = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2});
  auto source = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1.0, 2.0});
  EXPECT_EQ(target.slice({Dim::Y, 1, 2}).dims(),
            (Dimensions{{Dim::Y, 1}, {Dim::X, 2}}));

  target.slice({Dim::Y, 1, 2}) -= source;

  const auto data = target.values<double>();
  EXPECT_EQ(data[0], 0.0);
  EXPECT_EQ(data[1], 0.0);
  EXPECT_EQ(data[2], -1.0);
  EXPECT_EQ(data[3], -2.0);
}

TEST(VariableView, slice_binary_operations) {
  auto v = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 2},
                                Values{1, 2, 3, 4});
  // Note: There does not seem to be a way to test whether this is using the
  // operators that convert the second argument to Variable (it should not), or
  // keep it as a view. See variable_benchmark.cpp for an attempt to verify
  // this.
  auto sum = v.slice({Dim::X, 0}) + v.slice({Dim::X, 1});
  auto difference = v.slice({Dim::X, 0}) - v.slice({Dim::X, 1});
  auto product = v.slice({Dim::X, 0}) * v.slice({Dim::X, 1});
  auto ratio = v.slice({Dim::X, 0}) / v.slice({Dim::X, 1});
  EXPECT_TRUE(equals(sum.values<double>(), {3, 7}));
  EXPECT_TRUE(equals(difference.values<double>(), {-1, -1}));
  EXPECT_TRUE(equals(product.values<double>(), {2, 12}));
  EXPECT_TRUE(equals(ratio.values<double>(), {1.0 / 2.0, 3.0 / 4.0}));
}

TEST(Variable, non_in_place_scalar_operations) {
  auto var = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{1, 2});

  auto sum = var + 1.0 * units::one;
  EXPECT_TRUE(equals(sum.values<double>(), {2, 3}));
  sum = 2.0 * units::one + var;
  EXPECT_TRUE(equals(sum.values<double>(), {3, 4}));

  auto diff = var - 1.0 * units::one;
  EXPECT_TRUE(equals(diff.values<double>(), {0, 1}));
  diff = 2.0 * units::one - var;
  EXPECT_TRUE(equals(diff.values<double>(), {1, 0}));

  auto prod = var * (2.0 * units::one);
  EXPECT_TRUE(equals(prod.values<double>(), {2, 4}));
  prod = 3.0 * units::one * var;
  EXPECT_TRUE(equals(prod.values<double>(), {3, 6}));

  auto ratio = var / (2.0 * units::one);
  EXPECT_TRUE(equals(ratio.values<double>(), {1.0 / 2.0, 1.0}));
  ratio = 3.0 * units::one / var;
  EXPECT_TRUE(equals(ratio.values<double>(), {3.0, 1.5}));
}

TEST(VariableView, scalar_operations) {
  auto var = makeVariable<double>(Dims{Dim::Y, Dim::X}, Shape{2, 3},
                                  Values{11, 12, 13, 21, 22, 23});

  var.slice({Dim::X, 0}) += 1.0 * units::one;
  EXPECT_TRUE(equals(var.values<double>(), {12, 12, 13, 22, 22, 23}));
  var.slice({Dim::Y, 1}) += 1.0 * units::one;
  EXPECT_TRUE(equals(var.values<double>(), {12, 12, 13, 23, 23, 24}));
  var.slice({Dim::X, 1, 3}) += 1.0 * units::one;
  EXPECT_TRUE(equals(var.values<double>(), {12, 13, 14, 23, 24, 25}));
  var.slice({Dim::X, 1}) -= 1.0 * units::one;
  EXPECT_TRUE(equals(var.values<double>(), {12, 12, 14, 23, 23, 25}));
  var.slice({Dim::X, 2}) *= 0.0 * units::one;
  EXPECT_TRUE(equals(var.values<double>(), {12, 12, 0, 23, 23, 0}));
  var.slice({Dim::Y, 0}) /= 2.0 * units::one;
  EXPECT_TRUE(equals(var.values<double>(), {6, 6, 0, 23, 23, 0}));
}

TEST(VariableTest, binary_op_with_variance) {
  const auto var = makeVariable<double>(
      Dims{Dim::X, Dim::Y}, Shape{2, 3}, Values{1.0, 2.0, 3.0, 4.0, 5.0, 6.0},
      Variances{0.1, 0.2, 0.3, 0.4, 0.5, 0.6});
  const auto sum = makeVariable<double>(
      Dims{Dim::X, Dim::Y}, Shape{2, 3}, Values{2.0, 4.0, 6.0, 8.0, 10.0, 12.0},
      Variances{0.2, 0.4, 0.6, 0.8, 1.0, 1.2});
  auto tmp = var + var;
  EXPECT_TRUE(tmp.hasVariances());
  EXPECT_EQ(tmp.variances<double>()[0], 0.2);
  EXPECT_EQ(var + var, sum);

  tmp = var * sum;
  EXPECT_EQ(tmp.variances<double>()[0], 0.1 * 2.0 * 2.0 + 0.2 * 1.0 * 1.0);
}

TEST(VariableTest, divide_with_variance) {
  // Note the 0.0: With a wrong implementation the resulting variance is INF.
  const auto a = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{2.0, 0.0},
                                      Variances{0.1, 0.1});
  const auto b = makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{3.0, 3.0},
                                      Variances{0.2, 0.2});
  const auto expected =
      makeVariable<double>(Dims{Dim::X}, Shape{2}, Values{2.0 / 3.0, 0.0},
                           Variances{(0.1 / (2.0 * 2.0) + 0.2 / (3.0 * 3.0)) *
                                         (2.0 / 3.0) * (2.0 / 3.0),
                                     /* (0.1 / (0.0 * 0.0) + 0.2 / (3.0
                                      * 3.0)) * (0.0 / 3.0) * (0.0 / 3.0)
                                      naively, but if we take the limit... */
                                     0.1 / (3.0 * 3.0)});
  const auto q = a / b;
  EXPECT_DOUBLE_EQ(q.values<double>()[0], expected.values<double>()[0]);
  EXPECT_DOUBLE_EQ(q.values<double>()[1], expected.values<double>()[1]);
  EXPECT_DOUBLE_EQ(q.variances<double>()[0], expected.variances<double>()[0]);
  EXPECT_DOUBLE_EQ(q.variances<double>()[1], expected.variances<double>()[1]);
}

TEST(VariableTest, boolean_or) {
  const auto a = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, true, false, true});
  const auto b = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, false, true, true});
  const auto expected = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                           Values{false, true, true, true});

  const auto result = a | b;

  EXPECT_EQ(result, expected);
}

TEST(VariableTest, boolean_or_equals) {
  auto a = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                              Values{false, true, false, true});
  const auto b = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, false, true, true});
  a |= b;
  const auto expected = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                           Values{false, true, true, true});

  EXPECT_EQ(a, expected);
}

TEST(VariableTest, boolean_and_equals) {
  auto a = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                              Values{false, true, false, true});
  const auto b = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, false, true, true});
  a &= b;
  const auto expected = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                           Values{false, false, false, true});

  EXPECT_EQ(a, expected);
}

TEST(VariableTest, boolean_and) {
  const auto a = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, true, false, true});
  const auto b = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, false, true, true});
  const auto expected = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                           Values{false, false, false, true});

  const auto result = a & b;

  EXPECT_EQ(result, expected);
}

TEST(VariableTest, boolean_xor_equals) {
  auto a = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                              Values{false, true, false, true});
  const auto b = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, false, true, true});
  a ^= b;
  const auto expected = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                           Values{false, true, true, false});

  EXPECT_EQ(a, expected);
}

TEST(VariableTest, boolean_xor) {
  const auto a = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, true, false, true});
  const auto b = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                    Values{false, false, true, true});
  const auto expected = makeVariable<bool>(Dims{Dim::X}, Shape{4},
                                           Values{false, true, true, false});
  const auto result = a ^ b;

  EXPECT_EQ(result, expected);
}

TEST(VariableTest, zip_positions) {
  const Variable x =
      makeVariable<double>(Dims{Dim::X}, Shape{3}, units::m, Values{1, 2, 3});
  auto positions = geometry::position(x, x, x);
  auto values = positions.values<Eigen::Vector3d>();
  EXPECT_EQ(values.size(), 3);
  EXPECT_EQ(values[0], (Eigen::Vector3d{1, 1, 1}));
  EXPECT_EQ(values[1], (Eigen::Vector3d{2, 2, 2}));
  EXPECT_EQ(values[2], (Eigen::Vector3d{3, 3, 3}));
}

TEST(VariableTest, rotate) {
  Eigen::Vector3d vec1(1, 2, 3);
  Eigen::Vector3d vec2(4, 5, 6);
  auto vec = makeVariable<Eigen::Vector3d>(Dims{Dim::X}, Shape{2}, units::m,
                                           Values{vec1, vec2});
  Eigen::Quaterniond rot1(1.1, 2.2, 3.3, 4.4);
  Eigen::Quaterniond rot2(5.5, 6.6, 7.7, 8.8);
  rot1.normalize();
  rot2.normalize();
  const Variable rot = makeVariable<Eigen::Matrix3d>(
      Dims{Dim::X}, Shape{2}, units::one,
      Values{rot1.toRotationMatrix(), rot2.toRotationMatrix()});
  auto vec_new = rot * vec;
  const auto rotated = makeVariable<Eigen::Vector3d>(
      Dims{Dim::X}, Shape{2}, units::m,
      Values{rot1.toRotationMatrix() * vec1, rot2.toRotationMatrix() * vec2});
  EXPECT_EQ(vec_new, rotated);
}

TEST(VariableTest, mul_vector) {
  Eigen::Vector3d vec1(1, 2, 3);
  Eigen::Vector3d vec2(2, 4, 6);
  auto vec = makeVariable<Eigen::Vector3d>(Dims{Dim::X}, Shape{1}, units::m,
                                           Values{vec1});
  auto expected_vec = makeVariable<Eigen::Vector3d>(Dims{Dim::X}, Shape{1},
                                                    units::m, Values{vec2});
  auto scale = makeVariable<double>(Dims{}, Shape{1}, units::one, Values{2.0});
  auto scale_with_variance = makeVariable<double>(Dims{}, Shape{1}, units::one,
                                                  Values{2.0}, Variances{1.0});

  auto left_scaled_vec = scale * vec;
  auto right_scaled_vec = vec * scale;

  EXPECT_THROW_DISCARD(vec * scale_with_variance, except::VariancesError);
  EXPECT_EQ(left_scaled_vec, expected_vec);
  EXPECT_EQ(right_scaled_vec, expected_vec);
}

TEST(VariableTest, divide_vector) {
  Eigen::Vector3d vec1(1, 2, 3);
  Eigen::Vector3d vec2(2, 4, 6);
  auto vec = makeVariable<Eigen::Vector3d>(Dims{Dim::X}, Shape{1}, units::m,
                                           Values{vec2});
  auto expected_vec = makeVariable<Eigen::Vector3d>(Dims{Dim::X}, Shape{1},
                                                    units::m, Values{vec1});
  auto scale = makeVariable<double>(Dims{}, Shape{1}, units::one, Values{2.0});

  auto scaled_vec = vec / scale;

  EXPECT_EQ(scaled_vec, expected_vec);
}

TEST(Variable, 6d) {
  ASSERT_EQ(core::NDIM_MAX, 6); // update this test if limit is increased
  auto a = makeVariable<double>(
      Dims{Dim("1"), Dim("2"), Dim("3"), Dim("4"), Dim("5"), Dim("6")},
      Shape{1, 2, 3, 4, 5, 6});
  auto b = makeVariable<double>(
      Dims{Dim("3"), Dim("2"), Dim("1"), Dim("4"), Dim("6"), Dim("5")},
      Shape{3, 2, 1, 4, 6, 5});
  copy(a, b);
  a += b;
}


/*
 * reachability_graph.hpp
 *
 *  Created on: May 12, 2011
 *      Author: stewie
 */

#ifndef REACHABILITY_GRAPH_HPP_
#define REACHABILITY_GRAPH_HPP_

#include <utility>
#include <map>
#include <functional>

#include <boost/concept_check.hpp>
#include <boost/range/concepts.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/ptr_container/indirect_fun.hpp>

#include "../core/Attractor.hpp"
#include "../core/BooleanDynamics.hpp"
#include "perturb_attractor.hpp"

namespace bn {

typedef boost::adjacency_list<boost::vecS, boost::listS, boost::bidirectionalS,
		Attractor, double> Graph;

typedef std::map<Attractor, Graph::vertex_descriptor> VertexMap;

template<class AttractorRange, class CycleFinder> Graph extended_reachability_graph(
		const AttractorRange& r, CycleFinder f) {
	BOOST_CONCEPT_ASSERT((boost::ForwardRangeConcept<AttractorRange>));
	Graph g;
	VertexMap vmap;
	for (typename boost::range_iterator<AttractorRange>::type it =
			boost::begin(r), end = boost::end(r); it != end; ++it) {
		vmap.insert(std::make_pair(*it, boost::add_vertex(*it, g)));
		//std::cout << "Initial: " << *it << std::endl;
	}
	Graph::vertex_iterator vit, vend;
	for (boost::tie(vit, vend) = boost::vertices(g); vit != vend; ++vit) {
		//std::cout << "From here: " << g[*vit] << std::endl;
		std::map<Attractor, double> m = perturb_attractor(g[*vit], f);
		for (std::map<Attractor, double>::const_iterator it = m.begin(), end =
				m.end(); it != end; ++it) {
			if (vmap.count(it->first)) {
				boost::add_edge(*vit, vmap[it->first], it->second, g);
			} else {
				//std::cout << "New: " << it->first << std::endl;
				Graph::vertex_descriptor v = boost::add_vertex(it->first, g);
				vmap.insert(std::make_pair(it->first, v));
				boost::add_edge(*vit, v, it->second, g);
			}
		}
	}
	return g;
}

} // namespace bn

#endif /* REACHABILITY_GRAPH_HPP_ */


#include <Eigen/Eigen>
#include <pcl/point_cloud.h>
#include <pcl/features/feature.h>

#include <string>
#include <climits>

#include <ndt_map/ndt_map_hmt.h>
#include <ndt_map/lazy_grid.h>
#include <ndt_map/cell_vector.h>

#include <cstring>
#include <cstdio>

namespace perception_oru
{

void NDTMapHMT::initializeGrids() {

    if(grids_init) return;

    LazyGrid *proto = dynamic_cast<LazyGrid*>(index_);
    if(proto == NULL) return;

    double centerX,centerY,centerZ;
    proto->getCenter(centerX, centerY, centerZ);
    double sizeX,sizeY,sizeZ;
    proto->getGridSizeInMeters(sizeX, sizeY, sizeZ);
    std::cout<<"inti grids: res="<<resolution<<" cen "<<centerX<<" "<<centerY<<" "<<centerZ<<" size "<<sizeX<<" "<<sizeY<<" "<<sizeZ<<std::endl;

    for(int i=-1; i<2; i++) {
	for(int j=-1; j<2; j++) {
	    if(i==0 && j==0) {
		//no need to allocate, this is us
		LazyGrid *lz = dynamic_cast<LazyGrid*>(index_);
		grid_[i+1][j+1] = lz;
	    } else {
		double cenX,cenY;
		cenX = centerX+(double)i*sizeX;
		cenY = centerY+(double)j*sizeY;
		std::cout<<i<<":"<<j<<" center "<<cenX<<" "<<cenY<<std::endl;
		NDTCell *ptCell = new NDTCell();
		LazyGrid *lz = new LazyGrid(resolution);
		lz->setCellType(ptCell);
		lz->setCenter(cenX,cenY,centerZ);
		lz->setSize(sizeX,sizeY,sizeZ);
		lz->initializeAll();
		grid_[i+1][j+1] = lz;
		delete ptCell;
	    }
	}
    }
    grids_init = true;

}

bool NDTMapHMT::tryLoadPosition(const Eigen::Vector3d &newPos) {
    //open meta file and read grid centers
    if(my_directory == "" || !grids_init)
    {
	std::cout<<"cannot load from directory!\n";
	return false;
    }

    LazyGrid *proto = dynamic_cast<LazyGrid*>(index_);
    if(proto == NULL) return false;
    double sizeX,sizeY,sizeZ;
    double centerX,centerY,centerZ;
    proto->getGridSizeInMeters(sizeX, sizeY, sizeZ);

    std::string meta = my_directory;
    meta += "/metadata.txt";
    //std::cerr<<"metadata file at "<<meta<<std::endl;
    FILE* meta_f = fopen(meta.c_str(),"a+");
    if(meta_f==0) return false;
    char *line = NULL;
    size_t len;
    bool found=false;
    //std::cerr<<"reading metadata file at "<<meta<<std::endl;
    //read in all metadata
    //NOTE: v 2.0 -> Added header:
    //VERSION X.x\n
    //SIZE X.x\n
    //each next line: centerx centery centerz filename\n
    if(getline(&line,&len,meta_f) >0 ) {
	char *tk = strtok(line," ");
	if(tk==NULL) return false;
	if (strncmp(tk,"VERSION",7) == 0) {
	    tk = strtok(NULL," ");
	    if(tk==NULL) return false;
	    if(strncmp(tk,"2.0",3) == 0) {
		if(!getline(&line,&len,meta_f) >0 ) {
		    return false;
		}
		tk = strtok(line," ");
		if(tk==NULL) return false;
		if(strncmp(tk,"SIZE",4) != 0) return false;
		tk = strtok(NULL," ");
		double sizeMeta = atof(tk);
		if(fabsf(sizeMeta - sizeX) > 0.01) {
		    std::cerr<<"cannot load map, different grid size used... reverting to empty map\n";
		    return false;
		}
	    }
	    //add cases ...

	} else {
	    //this is a version 1.0 file, close and re-open and go on
	    std::cerr<<"metafile version 1.0, no protection against different grid size\n";
	    fclose(meta_f);
	    meta_f = fopen(meta.c_str(),"a+");
	}
    }

    while(getline(&line,&len,meta_f) >0 )
    {
	pcl::PointXYZ cen;
	char *token = strtok(line," ");
	if(token==NULL) return -1;
	cen.x = atof(token);
	token = strtok(NULL," ");
	if(token==NULL) return -1;
	cen.y = atof(token);
	token = strtok(NULL," ");
	if(token==NULL) return -1;
	cen.z = atof(token);

	token = strtok(NULL," ");
	if(token==NULL) return -1;

	if(fabsf(newPos(0)-cen.x) < sizeX/2 &&
		fabsf(newPos(1)-cen.y) < sizeY/2 && fabsf(newPos(2)-cen.z) < sizeZ/2) {
	    found = true;
	    centerX = cen.x; centerY = cen.y; centerZ = cen.z;
	    //std::cerr<<"file already for "<<jffname<<std::endl;
	    break;
	}

    }
    fclose(meta_f);
    if(!found) {
	std::cerr<<"Map file not found!\n";
	return false;
    }

    //newPos is inside one of the maps, load that for the center
    LazyGrid *tmp_grid[3][3];
    for(int i=-1; i<2; i++) {
	for(int j=-1; j<2; j++) {
	    double cenX,cenY;
	    cenX = centerX+(double)i*sizeX;
	    cenY = centerY+(double)j*sizeY;
	    std::cout<<i<<":"<<j<<" NEW center "<<cenX<<" "<<cenY<<std::endl;
	    if(this->tryLoad(cenX,cenY,centerZ,tmp_grid[i+1][j+1])) {
		delete grid_[i+1][j+1];
		grid_[i+1][j+1] = tmp_grid[i+1][j+1];
	    } else {
		grid_[i+1][j+1]->setCenter(cenX,cenY,centerZ);
	    }
	}
    }
    return true;
}

void NDTMapHMT::setInsertPosition(const Eigen::Vector3d &newPos) {

    last_insert = newPos;
    pcl::PointXYZ newPosP;
    newPosP.x = newPos(0);
    newPosP.y = newPos(1);
    newPosP.z = newPos(2);
    //check if newPos is outside current grid
    if(grid_[1][1]->isInside(newPosP)) return;

    std::cout<<"We are outside the central grid, time to switch pointers\n";
    //if yes, save maps
    this->writeTo();

    //find new center grid
    int qi=0,qj=0;
    for(int i=-1; i<2; i++) {
	for(int j=-1; j<2; j++) {
	    if(grid_[i+1][j+1]->isInside(newPosP)) {
		//std::cout<<"switching to grid "<<i+1<<" "<<j+1<<std::endl;
		qi=i; qj=j;
	    }
	}
    }
    LazyGrid *tmp_grid[3][3];
    bool copy[3][3];

    double centerX,centerY,centerZ;
    grid_[qi+1][qj+1]->getCenter(centerX, centerY, centerZ);
    double sizeX,sizeY,sizeZ;
    grid_[qi+1][qj+1]->getGridSizeInMeters(sizeX, sizeY, sizeZ);
    for(int i=0; i<3; i++) {
	for(int j=0; j<3; j++) {
	    copy[i][j]=false;
	}
    }
    int oi,oj;
    for(int i=0; i<3; i++) {
	for(int j=0; j<3; j++) {
	    oi = i+qi;
	    oj = j+qj;
	    if(oi>=0 && oi<3 && oj>=0 &&oj<3) {
		//copy old pointer
		//std::cout<<"will copy: "<<oi<<" "<<oj<<" to "<<i<<" "<<j<<std::endl;
		copy[oi][oj] = true;
		tmp_grid[i][j] = grid_[oi][oj];
	    } else {
		//try load of previous map
		double cenX,cenY;
		cenX = centerX+(double)(i-1)*sizeX;
		cenY = centerY+(double)(j-1)*sizeY;
		if(!this->tryLoad(cenX,cenY,centerZ,tmp_grid[i][j])) {
		    //de-allocate old pointer, create new one
		    //std::cout<<"will allocate new at "<<i<<" "<<j<<std::endl;

		    //std::cout<<i<<":"<<j<<" center "<<cenX<<" "<<cenY<<std::endl;
		    NDTCell *ptCell = new NDTCell();
		    LazyGrid *lz = new LazyGrid(resolution);
		    lz->setCellType(ptCell);
		    lz->setCenter(cenX,cenY,centerZ);
		    lz->setSize(sizeX,sizeY,sizeZ);
		    lz->initializeAll();
		    tmp_grid[i][j] = lz;
		    delete ptCell;
		} else {
		    //std::cout<<"loading previous map\n";
		}
	    }
	}
    }

    //deallocate old border
    for(int i=0; i<3; i++) {
	for(int j=0; j<3; j++) {
	    if(!copy[i][j]) {
		//std::cout<<"DELETE old grid at "<<i<<" "<<j<<std::endl;
		delete grid_[i][j];
	    }
	    grid_[i][j] = tmp_grid[i][j];
	}
    }

}
/**
* loadPointCloud - You can call this if you are only interested in dealing with one scan
* without need for fusing several ones or representing empty space and occupancy
*
* Otherwise you should always call addPointCloud (or if you don't want occupancy then addPointCloudSimple)
*
* \param pc the PointCloud that is to be loaded
* \note every subsequent call will destroy the previous map!
*/
void NDTMapHMT::loadPointCloud(const pcl::PointCloud<pcl::PointXYZ> &pc, double range_limit)
{

    //std::cout<<"LOAD pc\n";
    typename pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();
    NDTCell *ptCell;

    while(it!=pc.points.end())
    {
        Eigen::Vector3d d;
        if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
        {
            it++;
            continue;
        }
        /*if(range_limit>0)
        {
            d << it->x, it->y, it->z;
            if(d.norm()>range_limit)
            {
                it++;
                continue;
            }
        }*/

	for(int i=0; i<3; ++i) {
	    for(int j=0; j<3; ++j) {
		if(grid_[i][j]->isInside(*it)) {
		    ptCell = (grid_[i][j]->addPoint(*it));
		    if(ptCell!=NULL) {
			update_set.insert(ptCell);
		    }
		    break;
		}
	    }
	}
	it++;
    }
    isFirstLoad_ = false;
}

/**
 * Adds a new cloud: NDT-OM update step
 */
void NDTMapHMT::addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<pcl::PointXYZ> &pc, double classifierTh, double maxz, double sensor_noise, double occupancy_limit)
{
    //std::cerr<<"ADDPointCloud\n";
    if(isFirstLoad_)
    {
	//std::cout<<"First add, loading pcs\n";
	this->loadPointCloud(pc);
	return;
    }

    this->setInsertPosition(origin);
    pcl::PointCloud<pcl::PointXYZ>::const_iterator it = pc.points.begin();

    LazyGrid *lz = grid_[1][1];

    double centerX, centerY, centerZ;
    double sizeXmeters, sizeYmeters, sizeZmeters;
    double eps = 0.001;
    Eigen::Vector3d diff2;
    lz->getGridSizeInMeters(sizeXmeters, sizeYmeters, sizeZmeters);
    lz->getCenter(centerX, centerY, centerZ);
    pcl::PointXYZ po, pt;
    Eigen::Vector3d pf;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    NDTCell* ptCell = NULL;

    std::vector< NDTCell*> cells;
    bool updatePositive = true;
    //double max_range = 200.;

    while(it!=pc.points.end())
    {
	if(std::isnan(it->x) ||std::isnan(it->y) ||std::isnan(it->z))
	{
	    it++;
	    continue;
	}

	Eigen::Vector3d diff;
	diff << it->x-origin(0), it->y-origin(1), it->z-origin(2);
	double l = diff.norm();

	if(l>max_range_)
	{
	    //fprintf(stderr,"Very long distance (%lf) :( \n",l);
	    it++;
	    continue;
	}

	cells.clear();
	if(!lz->traceLineWithEndpoint(origin,*it,diff,maxz,cells,pf)) {
	    it++;
	    continue;
	}
	//check if pf is within one resolution of diff
	diff2 << it->x-pf(0), it->y-pf(1), it->z-pf(2);
	if((diff2).norm() > eps) {
	    //line continues outside center map, need to trace more!
	    int i=0,j=0;
	    if(it->x > centerX+sizeXmeters/2-resolution/2) {
		i = 1;
	    }
	    if(it->x < centerX-sizeXmeters/2-resolution/2) {
		i = -1;
	    }
	    if(it->y > centerY+sizeYmeters/2-resolution/2) {
		j = 1;
	    }
	    if(it->y < centerY-sizeYmeters/2-resolution/2) {
		j = -1;
	    }
	    std::vector< NDTCell*> cells2;
	    if(!grid_[i+1][j+1]->traceLineWithEndpoint(pf,*it,diff2,maxz,cells,pf)) {
		std::cerr<<"ray outside central map: at "<<it->x<<" "<<it->y<<" "<<it->z<<" with diff "<<diff.transpose()<<" starts at " <<origin.transpose()
		    <<" stops at "<<pf.transpose()<<" diff2 is "<<diff2.transpose()<<" check grid "<<i<<" "<<j<<std::endl;
		it++;
		continue;
	    }
	    cells.insert(cells.begin(),cells2.begin(),cells2.end());
	    //TODO: we might need to make one more raytrace here
	}

	for(unsigned int i=0; i<cells.size(); i++)
	{
	    ptCell = cells[i];
	    if(ptCell != NULL)
	    {
		double l2target = 0;
		if(ptCell->hasGaussian_)
		{
		    Eigen::Vector3d out, pend,vpt;
		    pend << it->x,it->y,it->z;
		    double lik = ptCell->computeMaximumLikelihoodAlongLine(po, *it, out);
		    l2target = (out-pend).norm();

		    double dist = (origin-out).norm();
		    if(dist > l) continue; ///< don't accept points further than the measurement

		    l2target = (out-pend).norm(); ///<distance to endpoint

		    double sigma_dist = 0.5 * (dist/30.0); ///test for distance based sensor noise
		    double snoise = sigma_dist + sensor_noise;
		    double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); ///This is the probability of max lik point being endpoint
		    lik *= (1.0-thr);
		    if(lik<0.3) continue;
		    lik = 0.1*lik+0.5; ///Evidence value for empty - alpha * p(x);
		    double logoddlik = log( (1.0-lik)/(lik) );
		    ptCell->updateOccupancy(logoddlik, occupancy_limit);
		    if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false;
		}
		else
		{
		    ptCell->updateOccupancy(-0.2, occupancy_limit);
		    if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false;
		}
	    }
	}
	if(updatePositive) {
	    int i=0,j=0;
	    if(it->x > centerX+sizeXmeters/2-resolution/2) {
		i = 1;
	    }
	    if(it->x < centerX-sizeXmeters/2-resolution/2) {
		i = -1;
	    }
	    if(it->y > centerY+sizeYmeters/2-resolution/2) {
		j = 1;
	    }
	    if(it->y < centerY-sizeYmeters/2-resolution/2) {
		j = -1;
	    }
	    if(grid_[i+1][j+1]->isInside(*it)) {
		ptCell = (grid_[i+1][j+1]->addPoint(*it));
		if(ptCell!=NULL) {
		    update_set.insert(ptCell);
		}
	    }
	}
	it++;
    }
    isFirstLoad_ = false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Add new pointcloud to map - Updates the occupancy using the mean values of
* a local map generated from an observation
*
* Performs raytracing, updates conflicts and adds points to cells
* computeNDTCells must be called after calling this
*
* @param &origin is the position of the sensor, from where the scan has been taken from.
* @param &pc is the pointcloud to be added
* @param &localmapsize The dimensions of the local map used for computing the gaussians
* @param maxnumpoints Defines the forgetting factor (default 100000) the smaller the value the faster the adaptation
* @param occupancy_limit Clamping threshold for log-odds value
* @param maxz threshold for the maximum z-coordinate value for the measurement point_cloud
* @param sensor_noise The expected standard deviation of the sensor noise
*/
void NDTMapHMT::addPointCloudMeanUpdate(const Eigen::Vector3d &origin,
	const pcl::PointCloud<pcl::PointXYZ> &pc,
	const Eigen::Vector3d &localmapsize,
	unsigned int maxnumpoints, float occupancy_limit,double maxz, double sensor_noise){

    //std::cout<<localmapsize.transpose()<<" "<<maxnumpoints<<" "<<occupancy_limit<<" "<<maxz<<" "<<sensor_noise<<std::endl;
    //fprintf(stderr,"Mean nasty point cloud hmt\n");
    if(isFirstLoad_){
	//fprintf(stderr,"First load... ");
	loadPointCloud(pc);
	return;
    }
    this->setInsertPosition(origin);

    LazyGrid *lz = grid_[1][1];
    ///fprintf(stderr,"UPDATING\n");
    double sizeXmeters, sizeYmeters, sizeZmeters;
    double eps = 0.001;
    lz->getGridSizeInMeters(sizeXmeters, sizeYmeters, sizeZmeters);
    double centerX,centerY,centerZ;
    lz->getCenter(centerX, centerY, centerZ);
    int sizeX,sizeY,sizeZ;
    lz->getGridSize(sizeX, sizeY, sizeZ);

    Eigen::Vector3d old_centroid;
    old_centroid(0) = centerX;
    old_centroid(1) = centerY;
    old_centroid(2) = centerZ;

    ///Lets first create a local ndmap (this really works only if we have lazy grid as well)
    perception_oru::NDTMap ndlocal(new perception_oru::LazyGrid(resolution));
    ndlocal.loadPointCloudCentroid(pc,origin, old_centroid, localmapsize, max_range_);
    ndlocal.computeNDTCells();

    ///TODO: This returns now copies --- should be pointers?
    std::vector<perception_oru::NDTCell*> ndts;
    ndts = ndlocal.getAllCells();

    NDTCell* ptCell = NULL;
    std::vector< NDTCell*> cells;

    pcl::PointXYZ pt;
    pcl::PointXYZ po,mpoint;
    po.x = origin(0); po.y = origin(1); po.z = origin(2);
    //fprintf(stderr,"NUM = %d\n",ndts.size());
    bool updatePositive = true;
    int num_high = 0;
    for(unsigned int it=0;it<ndts.size();it++)
    {
	if(ndts[it] == NULL){
	    fprintf(stderr,"NDTMap::addPointCloudMeanUpdate::GOT NULL FROM MAP -- SHOULD NEVER HAPPEN!!!\n");
	    continue;
	}

	if(!ndts[it]->hasGaussian_){
	    fprintf(stderr,"NDTMap::addPointCloudMeanUpdate::NO GAUSSIAN!!!! -- SHOULD NEVER HAPPEN!!!\n");
	    continue;
	}

	int numpoints = ndts[it]->getN();

	if(numpoints<=0){
	    fprintf(stderr,"addPointCloudMeanUpdate::Number of points in distribution<=0!!");
	    continue;
	}
	Eigen::Vector3d diff,diff2,pf;
	Eigen::Vector3d m = ndts[it]->getMean();
	mpoint.x = m(0);
	mpoint.y = m(1);
	mpoint.z = m(2);
	diff = m-origin;
	double l = diff.norm();

	if(l>max_range_)
	{
	    //fprintf(stderr,"Very long distance (%lf) :( \n",l);
	    it++;
	    continue;
	}
	cells.clear();
	if(!lz->traceLineWithEndpoint(origin,mpoint,diff,maxz,cells,pf)) {
	    it++;
	    continue;
	}
	//check if pf is within one resolution of diff
	diff2 = m-pf;
	if((diff2).norm() > eps) {
	    //line continues outside center map, need to trace more!
	    int i=0,j=0;
	    if(mpoint.x > centerX+sizeXmeters/2-resolution/2) {
		i = 1;
	    }
	    if(mpoint.x < centerX-sizeXmeters/2-resolution/2) {
		i = -1;
	    }
	    if(mpoint.y > centerY+sizeYmeters/2-resolution/2) {
		j = 1;
	    }
	    if(mpoint.y < centerY-sizeYmeters/2-resolution/2) {
		j = -1;
	    }
	    std::vector< NDTCell*> cells2;
	    if(!grid_[i+1][j+1]->traceLineWithEndpoint(pf,mpoint,diff2,maxz,cells,pf)) {
		std::cerr<<"ray outside central map: at "<<mpoint.x<<" "<<mpoint.y<<" "<<mpoint.z<<" with diff "<<diff.transpose()<<" starts at " <<origin.transpose()
		    <<" stops at "<<pf.transpose()<<" diff2 is "<<diff2.transpose()<<" check grid "<<i<<" "<<j<<std::endl;
		it++;
		continue;
	    }
	    cells.insert(cells.begin(),cells2.begin(),cells2.end());
	    //TODO: we might need to make one more raytrace here
	}

	for(unsigned int i=0; i<cells.size(); i++)
	{
	    ptCell = cells[i];

	    if(ptCell != NULL){
		double l2target = 0;
		if(ptCell->hasGaussian_)
		{
		    Eigen::Vector3d out, pend,vpt;
		    pend = m;
		    double lik = ptCell->computeMaximumLikelihoodAlongLine(po, mpoint, out);
		    l2target = (out-pend).norm();

		    double dist = (origin-out).norm();
		    if(dist > l) continue; ///< don't accept points further than the measurement

		    l2target = (out-pend).norm(); ///<distance to endpoint

		    double sigma_dist = 0.5 * (dist/30.0); ///test for distance based sensor noise
		    double snoise = sigma_dist + sensor_noise;
		    double thr =exp(-0.5*(l2target*l2target)/(snoise*snoise)); ///This is the probability of max lik point being endpoint
		    lik *= (1.0-thr);
		    if(lik<0.3) continue;
		    lik = 0.1*lik+0.5; ///Evidence value for empty - alpha * p(x);
		    double logoddlik = log( (1.0-lik)/(lik) );
		    ptCell->updateOccupancy(numpoints*logoddlik, occupancy_limit);
		    if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false;
		}
		else
		{
		    ptCell->updateOccupancy(-0.2*numpoints, occupancy_limit);
		    if(ptCell->getOccupancy()<=0) ptCell->hasGaussian_ = false;
		}
	    }
	    else{
		std::cerr<<"PANIC!!\n";
		index_->addPoint(pt); ///Add fake point to initialize!
	    }
	}
	if(updatePositive){
	    Eigen::Matrix3d ucov = ndts[it]->getCov();
	    float r,g,b;
	    ndts[it]->getRGB(r,g,b);
	    addDistributionToCell(ucov, m, numpoints, r, g,b,maxnumpoints,occupancy_limit); ///<FIXME:: local implementation can be faster?
	}
    }
    //fprintf(stderr,"| Gaussians=%d Invalid=%d |", ndts.size(), num_high);
    for(unsigned int i=0;i<ndts.size();i++) delete ndts[i];
    isFirstLoad_ = false;

}

/**
 * Add a distribution to the map
 */
void NDTMapHMT::addDistributionToCell(const Eigen::Matrix3d &ucov, const Eigen::Vector3d &umean, unsigned int numpointsindistribution,
	float r, float g,float b,  unsigned int maxnumpoints, float max_occupancy)
{
    pcl::PointXYZ pt;
    pt.x = umean[0];
    pt.y = umean[1];
    pt.z = umean[2];

    double centerX,centerY,centerZ;
    grid_[1][1]->getCenter(centerX, centerY, centerZ);
    double sizeXmeters, sizeYmeters, sizeZmeters;
    grid_[1][1]->getGridSizeInMeters(sizeXmeters, sizeYmeters, sizeZmeters);

    int i=0, j=0;
    if(pt.x > centerX+sizeXmeters/2-resolution/2) {
	i = 1;
    }
    if(pt.x < centerX-sizeXmeters/2-resolution/2) {
	i = -1;
    }
    if(pt.y > centerY+sizeYmeters/2-resolution/2) {
	j = 1;
    }
    if(pt.y < centerY-sizeYmeters/2-resolution/2) {
	j = -1;
    }

    if(grid_[i+1][j+1]->isInside(pt)) {
	NDTCell *ptCell = NULL;
	grid_[i+1][j+1]->getNDTCellAt(pt,ptCell);
	if(ptCell != NULL)
	{
	    ptCell->updateSampleVariance(ucov, umean, numpointsindistribution, true, max_occupancy,maxnumpoints);
	    ptCell->setRGB(r,g,b);
	}
    }

}

/** Helper function, computes the  NDTCells
 */
void NDTMapHMT::computeNDTCells(int cellupdatemode, unsigned int maxnumpoints, float occupancy_limit, Eigen::Vector3d origin, double sensor_noise)
{
    conflictPoints.clear();
    typename std::set<NDTCell*>::iterator it = update_set.begin();
    while (it != update_set.end())
    {
        NDTCell *cell = *it;
        if(cell!=NULL)
        {
            cell->computeGaussian(cellupdatemode,maxnumpoints, occupancy_limit, origin,sensor_noise);
            ///Process the conflict points
            if(cell->points_.size()>0)
            {
                for(unsigned int i=0; i<cell->points_.size(); i++) conflictPoints.push_back(cell->points_[i]);
                cell->points_.clear();
            }
        }
        it++;
    }
    update_set.clear();
}

/** output methods for saving the map in the jff format
 */
int NDTMapHMT::writeTo()
{

    if(my_directory == "" || !grids_init)
    {
	std::cout<<"provide directory name\n";
	return -1;
    }

    //std::cerr<<"writing to "<<my_directory<<std::endl;
    char fname[500];
    std::string jffnames[3][3];
    bool already[3][3];
    double centx, centy, centz, sizeX, sizeY, sizeZ;

    for(int i=0; i<3; i++) {
	for(int j=0; j<3; j++) {
	    if(grid_[i][j]==NULL) return -1;
	    grid_[i][j]->getCenter(centx,centy,centz);
	    snprintf(fname,499,"lz_%05lf_%05lf_%05lf.jff",centx,centy,centz);
	    jffnames[i][j] = fname;
	    already[i][j] = false;
	    //std::cerr<<"fname "<<jffnames[i][j]<<std::endl;
	}
    }
    grid_[1][1]->getGridSizeInMeters(sizeX, sizeY, sizeZ);
    //open metadata file
    std::string meta = my_directory;
    meta += "/metadata.txt";

    //std::cerr<<"metadata file at "<<meta<<std::endl;
    FILE* meta_f = fopen(meta.c_str(),"a+");
    if(meta_f==0) return -1;
    //read in all metadata
    //each line: centerx centery centerz filename\n
    char *line = NULL;
    size_t len;
    bool first=true;
    size_t length = 0;
    double epsilon = 1e-5;
    //std::cerr<<"reading metadata file at "<<meta<<std::endl;
    //------------------------------------------------------------------------//
    //read in all metadata
    //NOTE: v 2.0 -> Added header:
    //VERSION X.x\n
    //SIZE X.x\n
    //each next line: centerx centery centerz filename\n
    if(getline(&line,&len,meta_f) >0 ) {
	char *tk = strtok(line," ");
	if(tk==NULL) return false;
	if (strncmp(tk,"VERSION",7) == 0) {
	    tk = strtok(NULL," ");
	    if(tk==NULL) return false;
	    if(strncmp(tk,"2.0",3) == 0) {
		if(!getline(&line,&len,meta_f) >0 ) {
		    return false;
		}
		tk = strtok(line," ");
		if(tk==NULL) return false;
		if(strncmp(tk,"SIZE",4) != 0) return false;
		tk = strtok(NULL," ");
		double sizeMeta = atof(tk);
		if(fabsf(sizeMeta - sizeX) > 0.01) {
		    std::cerr<<"cannot write map, different grid size used...\n";
		    char ndir[500];
		    snprintf(ndir,499,"%s_%5d",my_directory.c_str(),rand());
		    my_directory = ndir;
		    std::cerr<<"SWITCHING DIRECTORY! "<<my_directory<<std::endl;
		    int res = mkdir(my_directory.c_str(),S_IRWXU);
		    if(res <0 ) return false;
		    fclose(meta_f);
		    meta   = my_directory+"/metadata.txt";
		    meta_f = fopen(meta.c_str(),"a+");
		    fprintf(meta_f,"VERSION 2.0\nSIZE %lf\n",sizeX);
		}
	    }
	    //add cases ...

	} else {
	    //this is a version 1.0 file, close and re-open and go on
	    std::cerr<<"metafile version 1.0, no protection against different grid size\n";
	    fclose(meta_f);
	    meta_f = fopen(meta.c_str(),"a+");
	}
    } else {
	//empty metadata file -> write in version and grid sizes!
	fprintf(meta_f,"VERSION 2.0\nSIZE %lf\n",sizeX);
    }
    //------------------------------------------------------------------------//
    while(getline(&line,&len,meta_f) >0 )
    {
	pcl::PointXYZ cen;
	char *token = strtok(line," ");
	if(token==NULL) return -1;
	cen.x = atof(token);
	token = strtok(NULL," ");
	if(token==NULL) return -1;
	cen.y = atof(token);
	token = strtok(NULL," ");
	if(token==NULL) return -1;
	cen.z = atof(token);

	token = strtok(NULL," ");
	if(token==NULL) return -1;

	for(int i=0; i<3; i++) {
	    for(int j=0; j<3; j++) {
		if(grid_[i][j]==NULL) return -1;
		grid_[i][j]->getCenter(centx,centy,centz);
		if(fabsf(cen.x-centx) < epsilon &&
		   fabsf(cen.y-centy) < epsilon && fabsf(cen.z-centz) < epsilon) {
		    already[i][j] = true;
		    token[strlen(token)-1]='\0';
		    jffnames[i][j] = token;
		    //std::cerr<<"file already for "<<jffnames[i][j]<<std::endl;
		}
	    }
	}

    }
    //fclose(meta_f);

    //now open for appending, in case we need to add new metadata
    //meta_f = fopen(meta.c_str(),"a");
    //std::cerr<<"appending metadata file at "<<meta<<std::endl;
    for(int i=0; i<3; i++) {
	for(int j=0; j<3; j++) {
	    if(grid_[i][j]==NULL) return -1;
	    grid_[i][j]->getCenter(centx,centy,centz);
	    if(!already[i][j]) {
		fprintf(meta_f,"%05lf %05lf %05lf %s\n",centx,centy,centz,jffnames[i][j].c_str());
		//std::cerr<<"appending metadata for "<<jffnames[i][j]<<std::endl;
	    }
	}
    }
    fclose(meta_f);

    //now pass through all grids and write to disk
    std::string sep = "/";
    for(int i=0; i<3; i++) {
	for(int j=0; j<3; j++) {
	    std::string path = my_directory+sep+jffnames[i][j];
	    //std::cout<<"saving "<<path<<std::endl;
	    FILE * jffout = fopen(path.c_str(), "w+b");
	    fwrite(_JFFVERSION_, sizeof(char), strlen(_JFFVERSION_), jffout);
	    int indexType[1] = {3};
	    fwrite(indexType, sizeof(int), 1, jffout);
	    double sizeXmeters, sizeYmeters, sizeZmeters;
	    double cellSizeX, cellSizeY, cellSizeZ;
	    double centerX, centerY, centerZ;
	    LazyGrid *ind = grid_[i][j];
	    ind->getGridSizeInMeters(sizeXmeters, sizeYmeters, sizeZmeters);
	    ind->getCellSize(cellSizeX, cellSizeY, cellSizeZ);
	    ind->getCenter(centerX, centerY, centerZ);
	    double lazyGridData[9] = { sizeXmeters, sizeYmeters, sizeZmeters,
		cellSizeX,   cellSizeY,   cellSizeZ,
		centerX,     centerY,     centerZ
	    };
	    fwrite(lazyGridData, sizeof(double), 9, jffout);
	    fwrite(ind->getProtoType(), sizeof(NDTCell), 1, jffout);
	    // loop through all active cells
	    typename SpatialIndex::CellVectorItr it = ind->begin();
	    while (it != ind->end())
	    {
		NDTCell *cell = (*it);
		if(cell!=NULL)
		{
		    if(cell->writeToJFF(jffout) < 0)	return -1;
		}
		else
		{
		    // do nothing
		}
		it++;
	    }
	    fclose(jffout);
	}
    }

    return 0;
}

bool NDTMapHMT::tryLoad(const double &cx, const double &cy, const double &cz, LazyGrid *&grid) {
std::cout<<"trying to load at "<<cx<<" "<<cy<<" "<<cz<<std::endl;
    if(my_directory == "" || !grids_init)
    {
	std::cout<<"provide directory name\n";
	return false;
    }

    std::string jffname;
    std::string meta = my_directory;
    meta += "/metadata.txt";
    //std::cerr<<"metadata file at "<<meta<<std::endl;
    FILE* meta_f = fopen(meta.c_str(),"a+");
    if(meta_f==0) return -1;
    //read in all metadata
    //each line: centerx centery centerz filename\n
    char *line = NULL;
    size_t len;
    bool found=false;
    size_t length = 0;
    double epsilon = 1e-2;
    //std::cerr<<"reading metadata file at "<<meta<<std::endl;
    // ---------------------------------------------------------------------------- //
    double sizeX,sizeY,sizeZ;
    grid_[1][1]->getGridSizeInMeters(sizeX, sizeY, sizeZ);
    //read in all metadata
    //NOTE: v 2.0 -> Added header:
    //VERSION X.x\n
    //SIZE X.x\n
    //each next line: centerx centery centerz filename\n
    if(getline(&line,&len,meta_f) >0 ) {
	char *tk = strtok(line," ");
	if(tk==NULL) return false;
	if (strncmp(tk,"VERSION",7) == 0) {
	    tk = strtok(NULL," ");
	    if(tk==NULL) return false;
	    if(strncmp(tk,"2.0",3) == 0) {
		if(!getline(&line,&len,meta_f) >0 ) {
		    return false;
		}
		tk = strtok(line," ");
		if(tk==NULL) return false;
		if(strncmp(tk,"SIZE",4) != 0) return false;
		tk = strtok(NULL," ");
		double sizeMeta = atof(tk);
		if(fabsf(sizeMeta - sizeX) > 0.01) {
		    std::cerr<<"cannot load map, different grid size used... reverting to empty map\n";
		    return false;
		}
	    }
	    //add cases ...

	} else {
	    //this is a version 1.0 file, close and re-open and go on
	    std::cerr<<"metafile version 1.0, no protection against different grid size\n";
	    fclose(meta_f);
	    meta_f = fopen(meta.c_str(),"a+");
	}
    }
    // ---------------------------------------------------------------------------- //

    while(getline(&line,&len,meta_f) >0 )
    {
	pcl::PointXYZ cen;
	char *token = strtok(line," ");
	if(token==NULL) return -1;
	cen.x = atof(token);
	token = strtok(NULL," ");
	if(token==NULL) return -1;
	cen.y = atof(token);
	token = strtok(NULL," ");
	if(token==NULL) return -1;
	cen.z = atof(token);

	token = strtok(NULL," ");
	if(token==NULL) return -1;

	if(fabsf(cen.x-cx) < epsilon &&
		fabsf(cen.y-cy) < epsilon && fabsf(cen.z-cz) < epsilon) {
	    token[strlen(token)-1]='\0';
	    jffname = token;
	    found = true;
	    //std::cerr<<"file already for "<<jffname<<std::endl;
	    break;
	}

    }
    fclose(meta_f);
    if(!found) return false;

    FILE * jffin;
    std::string sep = "/";
    std::string path = my_directory+sep+jffname;
    std::cout<<"reading file "<<path<<std::endl;
    jffin = fopen(path.c_str(),"r+b");
    if(jffin==NULL) return false;

    char versionBuf[16];
    if(fread(&versionBuf, sizeof(char), strlen(_JFFVERSION_), jffin) <= 0)
    {
	std::cerr<<"reading version failed";
	return false;
    }
    versionBuf[strlen(_JFFVERSION_)] = '\0';
    int indexType;
    if(fread(&indexType, sizeof(int), 1, jffin) <= 0)
    {
	std::cerr<<"reading index type failed";
	return false;
    }

    NDTCell *ptCell = new NDTCell();
    grid = new LazyGrid(resolution);
    grid->setCellType(ptCell);
    delete ptCell;
    if(grid->loadFromJFF(jffin) < 0)
    {
	std::cerr<<"loading grid failed";
	return false;
    }
    fclose(jffin);

    return true;
}

/** method to load NDT maps from .jff files
USAGE:	create NDTMap with desired index and PointType (index type is
checked, but Point type is NOT checked) via e.g.

lslgeneric::NDTMap<pcl::PointXYZ> nd1(
new lslgeneric::LazyGrid<pcl::PointXYZ>(0.4)); --> (*)

and then call

nd1.loadFromJFF("map0027.jff");

 *) use this constructor so index is not initialized and attributes
 can be set manually
 */
int NDTMapHMT::loadFrom()
{

    if(my_directory == "" || !grids_init)
    {
	std::cout<<"provide directory name\n";
	return -1;
    }

    //read in metadata
    //find the grid centered at center and the grids around
    //read them in
   /*

    if(filename == NULL)
    {
        JFFERR("problem outputing to jff");
    }

    FILE * jffin;
    jffin = fopen(filename,"r+b");
    char versionBuf[16];
    if(fread(&versionBuf, sizeof(char), strlen(_JFFVERSION_), jffin) <= 0)
    {
        JFFERR("reading version failed");
    }
    versionBuf[strlen(_JFFVERSION_)] = '\0';
    int indexType;
    if(fread(&indexType, sizeof(int), 1, jffin) <= 0)
    {
        JFFERR("reading version failed");
    }
    if(gr->loadFromJFF(jffin) < 0)
    {
	JFFERR("Error loading LazyGrid");
    }
    NDTCell *ptCell = new NDTCell();
    index_->setCellType(ptCell);
    delete ptCell;
    fclose(jffin);

   // std::cout << "map loaded successfully " << versionBuf << std::endl;

    isFirstLoad_ = false;

    return 0;
    */
}

///Get the cell for which the point fall into (not the closest cell)
bool NDTMapHMT::getCellAtPoint(const pcl::PointXYZ &refPoint, NDTCell *&cell)
{
    if(grid_[1][1]->isInside(refPoint)) {
	grid_[1][1]->getNDTCellAt(refPoint,cell);
    } else {
	for(int i=0; i<3; ++i) {
	    for(int j=0; j<3; ++j) {
		if(grid_[i][j]->isInside(refPoint)) {
		    grid_[i][j]->getNDTCellAt(refPoint,cell);
		    break;
		}
	    }
    	}
    }
    return (cell != NULL);
}

//computes the *negative log likelihood* of a single observation
double NDTMapHMT::getLikelihoodForPoint(pcl::PointXYZ pt)
{
    double uniform=0.00100;
    NDTCell* ndCell = NULL;
    this->getCellAtPoint(pt,ndCell);
    if(ndCell == NULL) return uniform;
    double prob = ndCell->getLikelihood(pt);
    prob = (prob<0) ? 0 : prob; //uniform!! TSV
    return prob;
}

std::vector<NDTCell*> NDTMapHMT::getInitializedCellsForPoint(const pcl::PointXYZ pt) const
{
    std::vector<NDTCell*> cells, tmpcells;
    for(int i=0; i<3; ++i) {
	for(int j=0; j<3; ++j) {
	    if(grid_[i][j]->isInside(pt)) {
		tmpcells = grid_[i][j]->getClosestCells(pt);
		cells.insert(cells.begin(),tmpcells.begin(),tmpcells.end());
	    }
	}
    }
    return cells;
}

std::vector<NDTCell*> NDTMapHMT::getCellsForPoint(const pcl::PointXYZ pt, int n_neigh, bool checkForGaussian) const
{
    std::vector<NDTCell*> cells, tmpcells;
    for(int i=0; i<3; ++i) {
	for(int j=0; j<3; ++j) {
	    if(grid_[i][j]->isInside(pt)) {
		tmpcells = grid_[i][j]->getClosestNDTCells(pt,n_neigh,checkForGaussian);
		cells.insert(cells.begin(),tmpcells.begin(),tmpcells.end());
	    }
	}
    }
    return cells;
}

bool NDTMapHMT::getCellForPoint(const pcl::PointXYZ &pt, NDTCell* &out_cell, bool checkForGaussian) const
{
    out_cell = NULL;
    if(grid_[1][1]->isInside(pt)) {
	out_cell = grid_[1][1]->getClosestNDTCell(pt,checkForGaussian);
	return true;
    } else {
	for(int i=0; i<3; ++i) {
	    for(int j=0; j<3; ++j) {
		if(grid_[i][j]->isInside(pt)) {
		    out_cell = grid_[i][j]->getClosestNDTCell(pt,checkForGaussian);
		    return true;
		}
	    }
    	}
    }
    return false;
}

std::vector<NDTCell*> NDTMapHMT::pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T)
{

    std::vector<NDTCell*> ret;
    for(int i=0; i<3; ++i) {
	for(int j=0; j<3; ++j) {
	    typename SpatialIndex::CellVectorItr it = grid_[i][j]->begin();
	    while (it != grid_[i][j]->end())
	    {
		NDTCell *cell = (*it);
		if(cell!=NULL)
		{
		    if(cell->hasGaussian_)
		    {
			Eigen::Vector3d mean = cell->getMean();
			Eigen::Matrix3d cov = cell->getCov();
			mean = T*mean;
			///NOTE: The rotation of the covariance fixed by Jari 6.11.2012
			cov = T.rotation()*cov*T.rotation().transpose();
			NDTCell* nd = (NDTCell*)cell->clone();
			nd->setMean(mean);
			nd->setCov(cov);
			ret.push_back(nd);
		    }
		}
		else
		{
		    //ERR("problem casting cell to NDT!\n");
		}
		it++;
	    }
	}
    }
    return ret;
}

std::vector<perception_oru::NDTCell*> NDTMapHMT::getAllCells() const
{

    std::vector<NDTCell*> ret;
    for(int i=0; i<3; ++i) {
	for(int j=0; j<3; ++j) {
	    //if(i==1 && j==1) continue;
	    typename SpatialIndex::CellVectorItr it = grid_[i][j]->begin();
	    while (it != grid_[i][j]->end())
	    {
		NDTCell *cell = (*it);
		if(cell!=NULL)
		{
		    if(cell->hasGaussian_)
		    {
			NDTCell* nd = (NDTCell*)cell->copy();
			ret.push_back(nd);
		    }
		}
		else
		{
		}
		it++;
	    }
	}
    }
    return ret;
}

std::vector<perception_oru::NDTCell*> NDTMapHMT::getAllInitializedCells()
{

    std::vector<NDTCell*> ret;
    for(int i=0; i<3; ++i) {
	for(int j=0; j<3; ++j) {
	    typename SpatialIndex::CellVectorItr it = grid_[i][j]->begin();
	    while (it != grid_[i][j]->end())
	    {
		NDTCell *cell = (*it);
		if(cell!=NULL)
		{
		    NDTCell* nd = (NDTCell*)cell->copy();
		    ret.push_back(nd);
		}
		else
		{
		}
		it++;
	    }
	}
    }
    return ret;
}

int NDTMapHMT::numberOfActiveCells()
{
    int ret = 0;
    for(int i=0; i<3; ++i) {
	for(int j=0; j<3; ++j) {
	    typename SpatialIndex::CellVectorItr it = grid_[i][j]->begin();
	    while (it != grid_[i][j]->end())
	    {
		NDTCell *cell = (*it);
		if(cell!=NULL)
		{
		    if(cell->hasGaussian_)
		    {
			ret++;
		    }
		}
		it++;
	    }
	}
    }
    return ret;
}

}