/*
 * ElevationMap.cpp
 *
 *  Created on: Feb 5, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "elevation_mapping/ElevationMap.hpp"

// Elevation Mapping
#include "elevation_mapping/ElevationMapFunctors.hpp"

// Grid Map
#include <grid_map_lib/GridMapMath.hpp>
#include <grid_map/GridMapMsgHelpers.hpp>
#include <grid_map_lib/SubmapIterator.hpp>

// Math
#include <math.h>

// ROS Logging
#include <ros/ros.h>

// Eigenvalues
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
using namespace sm;
using namespace sm::timing;

namespace elevation_mapping {

ElevationMap::ElevationMap(ros::NodeHandle& nodeHandle)
    : nodeHandle_(nodeHandle),
      rawMap_(vector<string>({"elevation", "variance", "horizontal_variance_x", "horizontal_variance_y", "color"})),
      fusedMap_(vector<string>({"elevation", "variance", "color", "surface_normal_x", "surface_normal_y", "surface_normal_z"}))
{
  minVariance_ = 0.0;
  maxVariance_ = 0.0;
  mahalanobisDistanceThreshold_ = 0.0;
  multiHeightNoise_ = 0.0;
  minHorizontalVariance_ = 0.0;
  maxHorizontalVariance_ = 0.0;
  rawMap_.setClearTypes(vector<string>({"elevation", "variance"}));
  fusedMap_.setClearTypes(vector<string>({"elevation", "variance"}));
  clear();

  elevationMapRawPublisher_ = nodeHandle_.advertise<grid_map_msg::GridMap>("elevation_map_raw", 1);
  elevationMapFusedPublisher_ = nodeHandle_.advertise<grid_map_msg::GridMap>("elevation_map", 1);
}

ElevationMap::~ElevationMap()
{

}

void ElevationMap::setGeometry(const Eigen::Array2d& length, const double& resolution, const Eigen::Vector2d& position)
{
  boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
  boost::recursive_mutex::scoped_lock scopedLockForFusedData(fusedMapMutex_);
  rawMap_.setGeometry(length, resolution, position);
  fusedMap_.setGeometry(length, resolution, position);
  ROS_INFO_STREAM("Elevation map grid resized to " << rawMap_.getBufferSize()(0) << " rows and "  << rawMap_.getBufferSize()(1) << " columns.");
}

bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances)
{
  for (unsigned int i = 0; i < pointCloud->size(); ++i)
  {
    auto& point = pointCloud->points[i];

    Array2i index;
    Vector2d position(point.x, point.y);
    if (!rawMap_.getIndex(position, index)) continue; // Skip this point if it does not lie within the elevation map.

    auto& elevation = rawMap_.at("elevation", index);
    auto& variance = rawMap_.at("variance", index);
    auto& horizontalVarianceX = rawMap_.at("horizontal_variance_x", index);
    auto& horizontalVarianceY = rawMap_.at("horizontal_variance_y", index);
    auto& color = rawMap_.at("color", index);
    float pointVariance = pointCloudVariances(i);

    if (!rawMap_.isValid(index))
    {
      // No prior information in elevation map, use measurement.
      elevation = point.z;
      variance = pointVariance;
      horizontalVarianceX = minHorizontalVariance_;
      horizontalVarianceY = minHorizontalVariance_;
      grid_map::colorVectorToValue(point.getRGBVector3i(), color);
      continue;
    }

    double mahalanobisDistance = sqrt(pow(point.z - elevation, 2) / variance);

    if (mahalanobisDistance < mahalanobisDistanceThreshold_)
    {
      // Fuse measurement with elevation map data.
      elevation = (variance * point.z + pointVariance * elevation) / (variance + pointVariance);
      variance =  (pointVariance * variance) / (pointVariance + variance);
      // TODO Add color fusion.
      grid_map::colorVectorToValue(point.getRGBVector3i(), color);
      continue;
    }

    // Add noise to cells which have ignored lower values,
    // such that outliers and moving objects are removed.
    variance += multiHeightNoise_;

    // Horizontal variances are reset.
    horizontalVarianceX = minHorizontalVariance_;
    horizontalVarianceY = minHorizontalVariance_;
  }

  clean();
  rawMap_.setTimestamp(1000 * pointCloud->header.stamp); // Point cloud stores time in microseconds.
  return true;
}

bool ElevationMap::update(Eigen::MatrixXf varianceUpdate, Eigen::MatrixXf horizontalVarianceUpdateX,
                          Eigen::MatrixXf horizontalVarianceUpdateY, const ros::Time& time)
{
  boost::recursive_mutex::scoped_lock scopedLock(rawMapMutex_);

  const auto& bufferSize = rawMap_.getBufferSize();

  if (!(
      (Array2i(varianceUpdate.rows(), varianceUpdate.cols()) == bufferSize).all() &&
      (Array2i(horizontalVarianceUpdateX.rows(), horizontalVarianceUpdateX.cols()) == bufferSize).all() &&
      (Array2i(horizontalVarianceUpdateY.rows(), horizontalVarianceUpdateY.cols()) == bufferSize).all()
      ))
  {
    ROS_ERROR("The size of the update matrices does not match.");
    return false;
  }

  rawMap_.get("variance") += varianceUpdate;
  rawMap_.get("horizontal_variance_x") += horizontalVarianceUpdateX;
  rawMap_.get("horizontal_variance_y") += horizontalVarianceUpdateY;
  clean();
  rawMap_.setTimestamp(time.toNSec());

  return true;
}

bool ElevationMap::fuseAll(const bool computeSurfaceNormals)
{
  ROS_DEBUG("Requested to fuse entire elevation map.");
  boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
  return fuse(Array2i(0, 0), fusedMap_.getBufferSize(), computeSurfaceNormals);
}

bool ElevationMap::fuseArea(const Eigen::Vector2d& position, const Eigen::Array2d& length, const bool computeSurfaceNormals)
{
  ROS_DEBUG("Requested to fuse an area of the elevation map with center at (%f, %f) and side lenghts (%f, %f)",
            position[0], position[1], length[0], length[1]);

  Array2i topLeftIndex;
  Array2i submapBufferSize;

  // These parameters are not used in this function.
  Vector2d submapPosition;
  Array2d submapLength;
  Array2i requestedIndexInSubmap;

  boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);

  grid_map_lib::getSubmapInformation(topLeftIndex, submapBufferSize, submapPosition, submapLength, requestedIndexInSubmap, position, length,
                                     rawMap_.getLength(), rawMap_.getPosition(), rawMap_.getResolution(), rawMap_.getBufferSize(),
                                     rawMap_.getBufferStartIndex());

  return fuse(topLeftIndex, submapBufferSize, computeSurfaceNormals);
}

bool ElevationMap::fuse(const Eigen::Array2i& topLeftIndex, const Eigen::Array2i& size, const bool computeSurfaceNormals)
{
  ROS_DEBUG("Fusing elevation map...");

  // Nothing to do.
  if ((size == 0).any()) return false;

  // Initializations.
  string timerId = "map_fusion_timer";
  Timer timer(timerId, true);

  boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);

  // Copy raw elevation map data for safe multi-threading.
  boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
  auto rawMapCopy = rawMap_;
  scopedLockForRawData.unlock();

  ROS_DEBUG("Fusing elevation map2...");

  // Check if there is the need to reset out-dated data.
  if (fusedMap_.getTimestamp() != rawMapCopy.getTimestamp()) resetFusedData();

  // Align fused map with raw map.
  if (rawMapCopy.getPosition() != fusedMap_.getPosition()) fusedMap_.move(rawMapCopy.getPosition());

  // For each cell in requested area.
  for (grid_map_lib::SubmapIterator areaIterator(rawMapCopy, topLeftIndex, size); !areaIterator.isPassedEnd(); ++areaIterator) {
    if (timer.isTiming()) timer.stop();
    timer.start();

    // Check if fusion for this cell has already been done earlier.
    if (fusedMap_.isValid(*areaIterator)) continue;

    if (!rawMapCopy.isValid(*areaIterator)) {
      // This is an empty cell (hole in the map).
      continue;
    }

    // Size of submap (2 sigma bound). TODO Add minimum/maximum submap size?
    Array2d requestedSubmapLength = 4.0 * Array2d(rawMapCopy.at("horizontal_variance_x", *areaIterator), rawMapCopy.at("horizontal_variance_y", *areaIterator)).sqrt();

    // Requested position (center) of submap in map.
    Vector2d requestedSubmapPosition;
    rawMapCopy.getPosition(*areaIterator, requestedSubmapPosition);

    Array2d submapLength;
    Vector2d submapPosition;
    Array2i submapTopLeftIndex, submapBufferSize, requestedIndexInSubmap;

    grid_map_lib::getSubmapInformation(submapTopLeftIndex, submapBufferSize, submapPosition, submapLength, requestedIndexInSubmap, requestedSubmapPosition, requestedSubmapLength,
                         rawMapCopy.getLength(), rawMapCopy.getPosition(), rawMapCopy.getResolution(), rawMapCopy.getBufferSize(), rawMapCopy.getBufferStartIndex());

//    cout << "submapTopLeftIndex: " << submapTopLeftIndex << endl;
//    cout << "submapBufferSize: " << submapBufferSize << endl;
//    cout << "submapPosition: " << submapPosition << endl;
//    cout << "submapLength: " << submapLength << endl;
//    cout << "requestedIndexInSubmap: " << requestedIndexInSubmap << endl;
//    cout << "requestedSubmapPosition: " << requestedSubmapPosition << endl;
//    cout << "requestedSubmapLength: " << requestedSubmapLength << endl;
//    cout << "rawMapCopy.getLength(): " << rawMapCopy.getLength() << endl;
//    cout << "rawMapCopy.getPosition(): " << rawMapCopy.getPosition() << endl;
//    cout << "rawMapCopy.getResolution(): " << rawMapCopy.getResolution() << endl;
//    cout << "rawMapCopy.getBufferSize(): " << rawMapCopy.getBufferSize() << endl;
//    cout << "rawMapCopy.getBufferStartIndex(): " << rawMapCopy.getBufferStartIndex() << endl;

    // Prepare data fusion.
    ArrayXf means, variances, weights;
    int maxNumberOfCellsToFuse = submapBufferSize.prod();
    means.resize(maxNumberOfCellsToFuse);
    variances.resize(maxNumberOfCellsToFuse);
    weights.resize(maxNumberOfCellsToFuse);

    // For each cell in submap.
    size_t i = 0;
    for (grid_map_lib::SubmapIterator submapIterator(rawMapCopy, submapTopLeftIndex, submapBufferSize); !submapIterator.isPassedEnd(); ++submapIterator) {
      if (!rawMapCopy.isValid(*submapIterator)) {
        // Empty cell in submap (cannot be center cell because we checked above).
        continue;
      }

      means[i] = rawMapCopy.at("elevation", *submapIterator);
      variances[i] = rawMapCopy.at("variance", *submapIterator);

      // Compute weight from probability.
      Vector2d position;
      rawMapCopy.getPosition(*submapIterator, position);

      Vector2d distanceToCenter = (position - submapPosition).cwiseAbs();

      float probabilityX =
            cumulativeDistributionFunction(distanceToCenter.x() + rawMapCopy.getResolution() / 2.0, 0.0, sqrt(rawMapCopy.at("horizontal_variance_x", *submapIterator)))
          - cumulativeDistributionFunction(distanceToCenter.x() - rawMapCopy.getResolution() / 2.0, 0.0, sqrt(rawMapCopy.at("horizontal_variance_x", *submapIterator)));
      float probabilityY =
            cumulativeDistributionFunction(distanceToCenter.y() + rawMapCopy.getResolution() / 2.0, 0.0, sqrt(rawMapCopy.at("horizontal_variance_y", *submapIterator)))
          - cumulativeDistributionFunction(distanceToCenter.y() - rawMapCopy.getResolution() / 2.0, 0.0, sqrt(rawMapCopy.at("horizontal_variance_y", *submapIterator)));

      weights[i] = probabilityX * probabilityY;
      i++;
    }

    if (i == 0) {
      // Nothing to fuse.
      fusedMap_.at("elevation", *areaIterator) = rawMapCopy.at("elevation", *areaIterator);
      fusedMap_.at("variance", *areaIterator) = rawMapCopy.at("variance", *areaIterator);
      fusedMap_.at("color", *areaIterator) = rawMapCopy.at("color", *areaIterator);
      continue;
    }

    // Fuse.
    means.conservativeResize(i);
    variances.conservativeResize(i);
    weights.conservativeResize(i);

    float mean = (weights * means).sum() / weights.sum();
    float variance = (weights * (variances + means.square())).sum() / weights.sum() - pow(mean, 2);

    if (!(std::isfinite(variance) && std::isfinite(mean))) {
      ROS_ERROR("Something went wrong when fusing the map: Mean = %f, Variance = %f", mean, variance);
      continue;
    }

    // Add to fused map.
    fusedMap_.at("elevation", *areaIterator) = mean;
    fusedMap_.at("variance", *areaIterator) = variance;

    // TODO Add fusion of colors.
    fusedMap_.at("color", *areaIterator) = rawMapCopy.at("color", *areaIterator);

    // Stop from data corruption with old surface normals.
    fusedMap_.at("surface_normal_x", *areaIterator) = NAN;
    fusedMap_.at("surface_normal_y", *areaIterator) = NAN;
    fusedMap_.at("surface_normal_z", *areaIterator) = NAN;

    timer.stop();
  }

  ROS_DEBUG("Fusing elevation map...");

  fusedMap_.setTimestamp(rawMapCopy.getTimestamp());

  ROS_INFO("Elevation map has been fused in %f s.", Timing::getTotalSeconds(timerId));
  ROS_DEBUG("Mean: %f s, Min: %f s, Max: %f s.", Timing::getMeanSeconds(timerId), Timing::getMinSeconds(timerId), Timing::getMaxSeconds(timerId));
  Timing::reset(timerId);

  if (computeSurfaceNormals) return ElevationMap::computeSurfaceNormals(topLeftIndex, size);
  return true;
}

bool ElevationMap::computeSurfaceNormals(const Eigen::Array2i& topLeftIndex, const Eigen::Array2i& size)
{
  // TODO Change this to raw map!
  ROS_DEBUG("Computing surface normals...");

  // Initializations.
  string timerId = "map_surface_normal_computation_timer";
  Timer timer(timerId, true);

  boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);

  vector<string> surfaceNormalTypes;
  surfaceNormalTypes.push_back("surface_normal_x");
  surfaceNormalTypes.push_back("surface_normal_y");
  surfaceNormalTypes.push_back("surface_normal_z");

  // For each cell in requested area.
  for (grid_map_lib::SubmapIterator areaIterator(fusedMap_, topLeftIndex, size); !areaIterator.isPassedEnd(); ++areaIterator) {
    if (timer.isTiming()) timer.stop();
    timer.start();

    // Check if this is an empty cell (hole in the map).
    if (!fusedMap_.isValid(*areaIterator)) continue;
    // Check if surface normal for this cell has already been computed earlier.
    if (fusedMap_.isValid(*areaIterator, surfaceNormalTypes)) continue;

    // Size of submap area for surface normal estimation.
    Array2d submapLength = Array2d::Ones() * (2.0 * surfaceNormalEstimationRadius_);

    // Requested position (center) of submap in map.
    Vector2d submapPosition;
    fusedMap_.getPosition(*areaIterator, submapPosition);
    Array2i submapTopLeftIndex, submapBufferSize, requestedIndexInSubmap;
    grid_map_lib::getSubmapInformation(submapTopLeftIndex, submapBufferSize, submapPosition, submapLength, requestedIndexInSubmap, submapPosition, submapLength,
                                       fusedMap_.getLength(), fusedMap_.getPosition(), fusedMap_.getResolution(), fusedMap_.getBufferSize(), fusedMap_.getBufferStartIndex());

    // Prepare data computation.
    const int maxNumberOfCells = submapBufferSize.prod();
    MatrixXd points(3, maxNumberOfCells);

    // Gather sourounding data.
    size_t nPoints = 0;
    for (grid_map_lib::SubmapIterator submapIterator(fusedMap_, submapTopLeftIndex, submapBufferSize); !submapIterator.isPassedEnd(); ++submapIterator) {
      if (!fusedMap_.isValid(*submapIterator)) continue;
      Vector3d point;
      fusedMap_.getPosition3d("elevation", *submapIterator, point);
      points.col(nPoints) = point;
      nPoints++;
    }
//    nPoints.conservativeResize(3, nPoints); // TODO Eigen version?

    // Compute eigenvectors.
    const Vector3d mean = points.leftCols(nPoints).rowwise().sum() / nPoints;
    const MatrixXd NN = points.leftCols(nPoints).colwise() - mean;

    const Matrix3d covarianceMatrix(NN * NN.transpose());
    Vector3d eigenvalues = Vector3d::Identity();
    Matrix3d eigenvectors = Matrix3d::Identity();
    // Ensure that the matrix is suited for eigenvalues calculation
    if (covarianceMatrix.fullPivHouseholderQr().rank() >= 3) {
      const EigenSolver<MatrixXd> solver(covarianceMatrix);
      eigenvalues = solver.eigenvalues().real();
      eigenvectors = solver.eigenvectors().real();
    } else {
      ROS_DEBUG("Covariance matrix needed for eigen decomposition is degenerated. Expected cause: no noise in data (nPoints = %i)", nPoints);
    }
    // Keep the smallest eigenvector as surface normal
    int smallestId(0);
    double smallestValue(numeric_limits<double>::max());
    for (int j = 0; j < eigenvectors.cols(); j++) {
      if (eigenvalues(j) < smallestValue) {
        smallestId = j;
        smallestValue = eigenvalues(j);
      }
    }
    Vector3d eigenvector = eigenvectors.col(smallestId);
    if (eigenvector.dot(surfaceNormalPositiveAxis_) < 0.0) eigenvector = -eigenvector;
    fusedMap_.at("surface_normal_x", *areaIterator) = eigenvector.x();
    fusedMap_.at("surface_normal_y", *areaIterator) = eigenvector.y();
    fusedMap_.at("surface_normal_z", *areaIterator) = eigenvector.z();
    timer.stop();
  }

  ROS_INFO("Surface normals have been computed in %f s.", Timing::getTotalSeconds(timerId));
  ROS_DEBUG("Mean: %f s, Min: %f s, Max: %f s.", Timing::getMeanSeconds(timerId), Timing::getMinSeconds(timerId), Timing::getMaxSeconds(timerId));
  Timing::reset(timerId);
  return true;
}

bool ElevationMap::clear()
{
  boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
  boost::recursive_mutex::scoped_lock scopedLockForFusedData(fusedMapMutex_);
  rawMap_.clearAll();
  rawMap_.resetTimestamp();
  fusedMap_.clearAll();
  fusedMap_.resetTimestamp();
  return true;
}

void ElevationMap::move(const Eigen::Vector2d& position)
{
  boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
  rawMap_.move(position);
}

bool ElevationMap::publishRawElevationMap()
{
  if (elevationMapRawPublisher_.getNumSubscribers() < 1) return false;
  boost::recursive_mutex::scoped_lock scopedLock(rawMapMutex_);
  grid_map::GridMap rawMapCopy = rawMap_;
  scopedLock.unlock();
  rawMapCopy.add("standard_deviation", rawMapCopy.get("variance").array().sqrt().matrix());
  rawMapCopy.add("horizontal_standard_deviation", (rawMapCopy.get("horizontal_variance_x") + rawMapCopy.get("horizontal_variance_y")).array().sqrt().matrix());
  rawMapCopy.add("two_sigma_bound", rawMapCopy.get("elevation") + 2.0 * rawMapCopy.get("variance").array().sqrt().matrix());
  grid_map_msg::GridMap message;
  rawMapCopy.toMessage(message);
  elevationMapRawPublisher_.publish(message);
  ROS_DEBUG("Elevation map raw has been published.");
  return true;
}

bool ElevationMap::publishElevationMap()
{
  if (elevationMapFusedPublisher_.getNumSubscribers() < 1) return false;
  boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
  grid_map::GridMap fusedMapCopy = fusedMap_;
  scopedLock.unlock();
  fusedMapCopy.add("standard_deviation", fusedMapCopy.get("variance").array().sqrt().matrix());
  fusedMapCopy.add("two_sigma_bound", fusedMapCopy.get("elevation") + 2.0 * fusedMapCopy.get("variance").array().sqrt().matrix());
  grid_map_msg::GridMap message;
  fusedMapCopy.toMessage(message);
  elevationMapFusedPublisher_.publish(message);
  ROS_DEBUG("Elevation map (fused) has been published.");
  return true;
}

grid_map::GridMap& ElevationMap::getRawGridMap()
{
  return rawMap_;
}

grid_map::GridMap& ElevationMap::getFusedGridMap()
{
  return fusedMap_;
}

ros::Time ElevationMap::getTimeOfLastUpdate()
{
  return ros::Time().fromNSec(rawMap_.getTimestamp());
}

ros::Time ElevationMap::getTimeOfLastFusion()
{
  boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
  return ros::Time().fromNSec(fusedMap_.getTimestamp());
}

const kindr::poses::eigen_impl::HomogeneousTransformationPosition3RotationQuaternionD& ElevationMap::getPose()
{
  return pose_;
}

bool ElevationMap::getPosition3dInRobotParentFrame(const Eigen::Array2i& index, kindr::phys_quant::eigen_impl::Position3D& position)
{
  kindr::phys_quant::eigen_impl::Position3D positionInGridFrame;
  if (!rawMap_.getPosition3d("elevation", index, positionInGridFrame.vector())) return false;
  position = pose_.transform(positionInGridFrame);
  return true;
}

boost::recursive_mutex& ElevationMap::getFusedDataMutex()
{
  return fusedMapMutex_;
}

boost::recursive_mutex& ElevationMap::getRawDataMutex()
{
  return rawMapMutex_;
}

bool ElevationMap::clean()
{
  boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
  rawMap_.get("variance") = rawMap_.get("variance").unaryExpr(VarianceClampOperator<double>(minVariance_, maxVariance_));
  rawMap_.get("horizontal_variance_x") = rawMap_.get("horizontal_variance_x").unaryExpr(VarianceClampOperator<double>(minHorizontalVariance_, maxHorizontalVariance_));
  rawMap_.get("horizontal_variance_y") = rawMap_.get("horizontal_variance_y").unaryExpr(VarianceClampOperator<double>(minHorizontalVariance_, maxHorizontalVariance_));
  return true;
}

void ElevationMap::resetFusedData()
{
  boost::recursive_mutex::scoped_lock scopedLockForFusedData(fusedMapMutex_);
  fusedMap_.clearAll();
  fusedMap_.resetTimestamp();
}

void ElevationMap::setFrameId(const std::string& frameId)
{
  rawMap_.setFrameId(frameId);
  fusedMap_.setFrameId(frameId);
}

const std::string& ElevationMap::getFrameId()
{
  return rawMap_.getFrameId();
}

float ElevationMap::cumulativeDistributionFunction(float x, float mean, float standardDeviation)
{
  return 0.5 * erfc(-(x-mean)/(standardDeviation*sqrt(2.0)));
}

} /* namespace */


#include <boost/graph/core_numbers.hpp>


#include <boost/test/unit_test.hpp>

#include <eosio/chain/types.hpp>
#include <eosio/testing/tester.hpp>

#include <boost/multiprecision/cpp_int.hpp>

#include <cstdint>
#include <iterator>
#include <string>
#include <vector>

#include <contracts.hpp>
#include "rsa_signer.hpp"

using namespace eosio;
using namespace eosio::chain;
using namespace fc;

using mvo = fc::mutable_variant_object;

#ifdef NON_VALIDATING_TEST  // see tester.hpp
#  define EOSIO_FIXTURE eosio::testing::tester
#else
#  define EOSIO_FIXTURE eosio::testing::validating_tester
#endif

const name account_n = N(wax.test);

const std::string public_exponent_1024 = "3";

const std::string private_exponent_1024 =
    "00954e45c37dca926fcbde438a9f8df39643133e4877151a2d423da2a87c54a681cc3c27b6970e7289c67191f596ea5caed709d762b4c937d95da9279bb32292a344c57631a3adfa8d1a837699908861dfe108d9bd37184b3f8b3810e13f183382c497b424b4530b56bdfb6c97e0c74b9dec7558425221e2e4ba3a2737aa3e2f53";

const std::string modulus_1024 =
    "dff568a53cafdba7b1cd654fef54ed61649cdd6cb29fa743e35c73fcba7ef9c2b25a3b91e295abcea9aa5af0625f8b06428ec3140f2dd3c60c7dbb698cb3dbf6c64b1160daec4eb7d6deca1dfc45b83d5f30e5398f6f737ee394d57c8d2bf412f056c2e8a54d9bf554149c0da31346e31f23ffb516b1f9797d650169199b7add";

struct __attribute((packed)) results_entry {
    std::uint64_t id;
    bool          value;
};
FC_REFLECT(results_entry, (id)(value));

BOOST_AUTO_TEST_SUITE(wax_tests)
BOOST_AUTO_TEST_SUITE(signature_tests)

// Custom fixture
struct wax_fixture: public EOSIO_FIXTURE {
    wax::rsa_signer signer;

    wax_fixture(): signer(private_exponent_1024, modulus_1024) {
        try {
            create_accounts({ account_n });

            set_code(account_n, eosio::testing::contracts::wax_rsa_wasm());
            set_abi(account_n, eosio::testing::contracts::wax_rsa_abi().data());

            produce_block();
        }
        FC_LOG_AND_RETHROW();
    }

    bool get_last_result() {
        results_entry entry;
        get_table_entry(entry, account_n, account_n, N(results), 0, false);
        return entry.value;
    }

    void action_verrsasig(const std::string& message,
                          const std::string& signature,
                          const std::string& exponent,
                          const std::string& modulus) {
        push_action(
            account_n,
            N(verrsasig),
            account_n,
            mvo() ("message", message)("signature", signature)
                  ("exponent", exponent)("modulus", modulus));
    }

}; // struct wax_fixture

BOOST_FIXTURE_TEST_CASE(check_invalid_lengths, wax_fixture) {
    try {
        action_verrsasig("", "abcd", "abcd", "abcd");
        BOOST_REQUIRE(!get_last_result());

        action_verrsasig("abcd", "", "abcd", "abcd");
        BOOST_REQUIRE(!get_last_result());

        action_verrsasig("abcd", "abcd", "", "abcd");
        BOOST_REQUIRE(!get_last_result());

        action_verrsasig("abcd", "abcd", "abcd", "");
        BOOST_REQUIRE(!get_last_result());
    }
    FC_LOG_AND_RETHROW();
}

BOOST_FIXTURE_TEST_CASE(check_invalid_hex_strings, wax_fixture) {
    try {
        action_verrsasig("foo", "XXXX", "abcd", "abcd");
        BOOST_REQUIRE(!get_last_result());

        action_verrsasig("foo", "abcd", "XXXX", "abcd");
        BOOST_REQUIRE(!get_last_result());

        action_verrsasig("foo", "abcd", "abcd", "XXXX");
        BOOST_REQUIRE(!get_last_result());
    }
    FC_LOG_AND_RETHROW();
}

BOOST_FIXTURE_TEST_CASE(signing_happy_path, wax_fixture) {
    try {
        const std::string msg = "message to sign";

        std::string signature = signer.sign(msg);

        BOOST_REQUIRE_EQUAL(signature.size(), modulus_1024.size());

        action_verrsasig(msg, signature, public_exponent_1024, modulus_1024);
        BOOST_REQUIRE(get_last_result());
    }
    FC_LOG_AND_RETHROW();
}

BOOST_FIXTURE_TEST_CASE(minimal_message_len, wax_fixture) {
    try {
        const std::string msg = "1";

        std::string signature = signer.sign(msg);

        action_verrsasig(msg, signature, public_exponent_1024, modulus_1024);
        BOOST_REQUIRE(get_last_result());
    }
    FC_LOG_AND_RETHROW();
}

BOOST_FIXTURE_TEST_CASE(malformed_parameter_len, wax_fixture) {
    try {
        const std::string msg = "message to sign";

        std::string signature = signer.sign(msg);

        action_verrsasig(msg, signature, "A", "B");
        BOOST_REQUIRE(!get_last_result());
    }
    FC_LOG_AND_RETHROW();
}

BOOST_FIXTURE_TEST_CASE(corrupted_sginature, wax_fixture) {
    try {
        const std::string msg = "message to sign";

        std::string signature = signer.sign(msg);
        BOOST_REQUIRE(!signature.empty());

        // Corrupt the signature
        auto last = signature.end() - 1;
        (*last)++;
        if (*last > '9') *last = '0';

        action_verrsasig(msg, signature, public_exponent_1024, modulus_1024);
        BOOST_REQUIRE(!get_last_result());
    }
    FC_LOG_AND_RETHROW();
}

BOOST_AUTO_TEST_SUITE_END() // signature_tests
BOOST_AUTO_TEST_SUITE_END() // wax_tests


/*
    Copyright (C) 2008 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include <lshkit/common.h>
#include <lshkit/mplsh-file.h>
#include <lshkit/matrix.h>

using namespace std;
using namespace lshkit;
namespace po = boost::program_options;

int main (int argc, char *argv[])
{
    string data_file;
    string lsh_file;

    MultiProbeLshFile::Parameter param;

    uint64_t cacheSize;

	po::options_description desc("Allowed options");
	desc.add_options()
		("help,h", "produce help message.")
		(",W", po::value<float>(&param.W)->default_value(1.0), "")
		(",M", po::value<unsigned>(&param.M)->default_value(1), "")
		(",L", po::value<unsigned>(&param.L)->default_value(1), "")
		(",H", po::value<unsigned>(&param.H)->default_value(1017881), "")
		(",B", po::value<uint64_t>(&param.B)->default_value(4096), "")
		(",C", po::value<uint64_t>(&cacheSize)->default_value(4096*1024*10), "")
		("input,I", po::value<string>(&data_file), "")
		("output,O", po::value<string>(&lsh_file), "")
		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help") || (vm.count("input") < 1) || (vm.count("output") < 1))
	{
		cout << desc;
		return 0;
	}

    cout << "Loading data...";
    Matrix<float> data(data_file);
    cout << "done." << endl;

    param.dim = data.getDim();
    param.S = 0;

    MultiProbeLshFile lsh(lsh_file, param, cacheSize);

    cout << "Importing..." << endl;
    boost::progress_display progress(data.getSize());
#pragma omp parallel for schedule(guided, 1) default(shared)
    for (int i = 0; i < data.getSize(); ++i)
    {
        lsh.append(i, data[i]);
#pragma omp critical
        ++progress;
    }
    cout << "Done." << endl;

    lsh.flush();

    return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================

/*
  This file implements the function

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
            class P, class T, class R>
  bool
  johnson_all_pairs_shortest_paths
    (VertexAndEdgeListGraph& g,
     DistanceMatrix& D,
     const bgl_named_params<P, T, R>& params)
 */

#ifndef BOOST_GRAPH_JOHNSON_HPP
#define BOOST_GRAPH_JOHNSON_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace boost {

  namespace detail {

    template <class VertexAndEdgeListGraph, class DistanceMatrix,
              class P, class T, class R, class VertexID, class Distance,
              class Weight, class Weight2, class DistanceZero>
    bool
    johnson_impl(VertexAndEdgeListGraph& g,
                 DistanceMatrix& D,
                 const bgl_named_params<P, T, R>& params,
                 VertexID id, Distance d, Weight w, Weight2 w_hat,
                 DistanceZero zero)
    {
      typedef graph_traits<VertexAndEdgeListGraph> Traits;
      typename Traits::vertex_iterator v, v_end, u, u_end;
      typename Traits::edge_iterator e, e_end;
      typename Traits::vertex_descriptor s = *vertices(g).first;
      typedef typename property_traits<Distance>::value_type DT;
      function_requires< BasicMatrixConcept<DistanceMatrix,
        typename Traits::vertex_descriptor, DT> >();

      std::vector<DT> h_vec(num_vertices(g));
      typedef typename std::vector<DT>::iterator iter_t;
      iterator_property_map<iter_t, VertexID, DT, DT&> h(h_vec.begin(), id);

      put(d, s, zero);
      if (bellman_ford_shortest_paths(g, num_vertices(g), params)) {
        for (tie(v, v_end) = vertices(g); v != v_end; ++v)
          put(h, *v, get(d, *v));
        for (tie(e, e_end) = edges(g); e != e_end; ++e)
          put(w_hat, *e,
              get(w, *e) + get(h, source(*e,g)) - get(h, target(*e,g)));
        for (tie(u, u_end) = vertices(g); u != u_end; ++u) {
          dijkstra_shortest_paths(g, *u, params.weight_map(w_hat));
          for (tie(v, v_end) = vertices(g); v != v_end; ++v)
            D[*u][*v] = get(d, *v) + get(h, *v) - get(h, *u);
        }
        return true;
      } else
        return false;
    }

    template <class VertexAndEdgeListGraph, class DistanceMatrix,
              class P, class T, class R, class Weight,
              class VertexID>
    bool
    johnson_dispatch(VertexAndEdgeListGraph& g,
                     DistanceMatrix& D,
                     const bgl_named_params<P, T, R>& params,
                     Weight w, VertexID id)
    {
      typedef typename property_traits<Weight>::value_type WT;
      typename std::vector<WT>::size_type
        n = is_default_param(get_param(params, vertex_distance))
        ? num_vertices(g) : 1;
      std::vector<WT> distance_map(n);

      return detail::johnson_impl
        (g, D, params, id,
         choose_param(get_param(params, vertex_distance),
                      make_iterator_property_map
                      (distance_map.begin(), id, distance_map[0])),
         w,
         choose_pmap(get_param(params, edge_weight2), g, edge_weight2),
         choose_param(get_param(params, distance_zero_t()),
                      WT()) );
    }

  } // namespace detail

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
            class P, class T, class R>
  bool
  johnson_all_pairs_shortest_paths
    (VertexAndEdgeListGraph& g,
     DistanceMatrix& D,
     const bgl_named_params<P, T, R>& params)
  {
    return detail::johnson_dispatch
      (g, D, params,
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_const_pmap(get_param(params, vertex_index), g, vertex_index)
       );
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix>
  bool
  johnson_all_pairs_shortest_paths
    (VertexAndEdgeListGraph& g, DistanceMatrix& D)
  {
    bgl_named_params<int,int> params(1);
    return detail::johnson_dispatch
      (g, D, params, get(edge_weight, g), get(vertex_index, g));
  }

} // namespace boost

#endif // BOOST_GRAPH_JOHNSON_HPP


/*
 * GridMap.cpp
 *
 *  Created on: Jul 14, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/GridMapMath.hpp"
#include "grid_map_core/SubmapGeometry.hpp"
#include "grid_map_core/iterators/GridMapIterator.hpp"

#include <Eigen/Dense>

#include <iostream>
#include <cassert>
#include <math.h>
#include <algorithm>
#include <stdexcept>

using namespace std;
using namespace grid_map;

namespace grid_map {

GridMap::GridMap(const std::vector<std::string>& layers)
{
  position_.setZero();
  length_.setZero();
  resolution_ = 0.0;
  size_.setZero();
  startIndex_.setZero();
  timestamp_ = 0;
  layers_ = layers;

  for (auto& layer : layers_) {
    data_.insert(std::pair<std::string, Matrix>(layer, Matrix()));
  }
}

GridMap::GridMap() :
    GridMap(std::vector<std::string>())
{
}

GridMap::~GridMap()
{
}

void GridMap::setGeometry(const Length& length, const double resolution,
                          const Position& position)
{
  assert(length(0) > 0.0);
  assert(length(1) > 0.0);
  assert(resolution > 0.0);

  Size size;
  size(0) = static_cast<int>(round(length(0) / resolution)); // There is no round() function in Eigen.
  size(1) = static_cast<int>(round(length(1) / resolution));
  resize(size);
  clearAll();

  resolution_ = resolution;
  length_ = (size_.cast<double>() * resolution_).matrix();
  position_ = position;
  startIndex_.setZero();

  return;
}

void GridMap::setGeometry(const SubmapGeometry& geometry)
{
  setGeometry(geometry.getLength(), geometry.getResolution(), geometry.getPosition());
}

void GridMap::setBasicLayers(const std::vector<std::string>& basicLayers)
{
  basicLayers_ = basicLayers;
}

const std::vector<std::string>& GridMap::getBasicLayers() const
{
  return basicLayers_;
}

bool GridMap::hasBasicLayers() const
{
  return basicLayers_.size() > 0;
}

bool GridMap::hasSameLayers(const GridMap& other) const
{
  for (const auto& layer : layers_) {
    if (!other.exists(layer)) return false;
  }
  return true;
}

void GridMap::add(const std::string& layer, const double value)
{
  add(layer, Matrix::Constant(size_(0), size_(1), value));
}

void GridMap::add(const std::string& layer, const Matrix& data)
{
  assert(size_(0) == data.rows());
  assert(size_(1) == data.cols());

  if (exists(layer)) {
    // Type exists already, overwrite its data.
    data_.at(layer) = data;
  } else {
    // Type does not exist yet, add type and data.
    data_.insert(std::pair<std::string, Matrix>(layer, data));
    layers_.push_back(layer);
  }
}

bool GridMap::exists(const std::string& layer) const
{
  return !(data_.find(layer) == data_.end());
}

const Matrix& GridMap::get(const std::string& layer) const
{
  try {
    return data_.at(layer);
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::get(...) : No map layer '" + layer + "' available.");
  }
}

Matrix& GridMap::get(const std::string& layer)
{
  try {
    return data_.at(layer);
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::get(...) : No map layer of type '" + layer + "' available.");
  }
}

const Matrix& GridMap::operator [](const std::string& layer) const
{
  return get(layer);
}

Matrix& GridMap::operator [](const std::string& layer)
{
  return get(layer);
}

bool GridMap::erase(const std::string& layer)
{
  const auto dataIterator = data_.find(layer);
  if (dataIterator == data_.end()) return false;
  data_.erase(dataIterator);

  const auto layerIterator = std::find(layers_.begin(), layers_.end(), layer);
  if (layerIterator == layers_.end()) return false;
  layers_.erase(layerIterator);

  const auto basicLayerIterator = std::find(basicLayers_.begin(), basicLayers_.end(), layer);
  if (basicLayerIterator != basicLayers_.end()) basicLayers_.erase(basicLayerIterator);

  return true;
}

const std::vector<std::string>& GridMap::getLayers() const
{
  return layers_;
}

float& GridMap::atPosition(const std::string& layer, const Position& position)
{
  Index index;
  if (getIndex(position, index)) {
    return at(layer, index);
  }
  throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
}

float GridMap::atPosition(const std::string& layer, const Position& position, InterpolationMethods interpolationMethod) const
{
  switch (interpolationMethod) {
      case InterpolationMethods::INTER_LINEAR:
      {
        float value;
        if (atPositionLinearInterpolated(layer, position, value))
          return value;
        else
            interpolationMethod = InterpolationMethods::INTER_NEAREST;
      }
      case  InterpolationMethods::INTER_NEAREST:
      {
        Index index;
        if (getIndex(position, index)) {
        return at(layer, index);
        }
        else
        throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
        break;
      }
      default:
        throw std::runtime_error("GridMap::atPosition(...) : Specified interpolation method not implemented.");
  }
}

float& GridMap::at(const std::string& layer, const Index& index)
{
  try {
    return data_.at(layer)(index(0), index(1));
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
  }
}

float GridMap::at(const std::string& layer, const Index& index) const
{
  try {
    return data_.at(layer)(index(0), index(1));
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available.");
  }
}

bool GridMap::getIndex(const Position& position, Index& index) const
{
  return getIndexFromPosition(index, position, length_, position_, resolution_, size_, startIndex_);
}

bool GridMap::getPosition(const Index& index, Position& position) const
{
  return getPositionFromIndex(position, index, length_, position_, resolution_, size_, startIndex_);
}

bool GridMap::isInside(const Position& position) const
{
  return checkIfPositionWithinMap(position, length_, position_);
}

bool GridMap::isValid(const Index& index) const
{
  return isValid(index, basicLayers_);
}

bool GridMap::isValid(const Index& index, const std::string& layer) const
{
  if (!isfinite(at(layer, index))) return false;
  return true;
}

bool GridMap::isValid(const Index& index, const std::vector<std::string>& layers) const
{
  if (layers.empty()) return false;
  for (auto& layer : layers) {
    if (!isfinite(at(layer, index))) return false;
  }
  return true;
}

bool GridMap::getPosition3(const std::string& layer, const Index& index,
                           Position3& position) const
{
  if (!isValid(index, layer)) return false;
  Position position2d;
  getPosition(index, position2d);
  position.head(2) = position2d;
  position.z() = at(layer, index);
  return true;
}

bool GridMap::getVector(const std::string& layerPrefix, const Index& index,
                        Eigen::Vector3d& vector) const
{
  std::vector<std::string> layers;
  layers.push_back(layerPrefix + "x");
  layers.push_back(layerPrefix + "y");
  layers.push_back(layerPrefix + "z");
  if (!isValid(index, layers)) return false;
  for (size_t i = 0; i < 3; ++i) {
    vector(i) = at(layers[i], index);
  }
  return true;
}

GridMap GridMap::getSubmap(const Position& position, const Length& length,
                           bool& isSuccess) const
{
  Index index;
  return getSubmap(position, length, index, isSuccess);
}

GridMap GridMap::getSubmap(const Position& position, const Length& length,
                           Index& indexInSubmap, bool& isSuccess) const
{
  // Submap the generate.
  GridMap submap(layers_);
  submap.setBasicLayers(basicLayers_);
  submap.setTimestamp(timestamp_);
  submap.setFrameId(frameId_);

  // Get submap geometric information.
  SubmapGeometry submapInformation(*this, position, length, isSuccess);
  if (isSuccess == false) return GridMap(layers_);
  submap.setGeometry(submapInformation);
  submap.startIndex_.setZero(); // Because of the way we copy the data below.

  // Copy data.
  std::vector<BufferRegion> bufferRegions;

  if (!getBufferRegionsForSubmap(bufferRegions, submapInformation.getStartIndex(),
                                 submap.getSize(), size_, startIndex_)) {
    cout << "Cannot access submap of this size." << endl;
    isSuccess = false;
    return GridMap(layers_);
  }

  for (const auto& data : data_) {
    for (const auto& bufferRegion : bufferRegions) {
      Index index = bufferRegion.getStartIndex();
      Size size = bufferRegion.getSize();

      if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
        submap.data_[data.first].topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
        submap.data_[data.first].topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
        submap.data_[data.first].bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
        submap.data_[data.first].bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      }

    }
  }

  isSuccess = true;
  return submap;
}

void GridMap::setPosition(const Position& position)
{
  position_ = position;
}

bool GridMap::move(const Position& position, std::vector<BufferRegion>& newRegions)
{
  Index indexShift;
  Position positionShift = position - position_;
  getIndexShiftFromPositionShift(indexShift, positionShift, resolution_);
  Position alignedPositionShift;
  getPositionShiftFromIndexShift(alignedPositionShift, indexShift, resolution_);

  // Delete fields that fall out of map (and become empty cells).
  for (int i = 0; i < indexShift.size(); i++) {
    if (indexShift(i) != 0) {
      if (abs(indexShift(i)) >= getSize()(i)) {
        // Entire map is dropped.
        clearAll();
        newRegions.push_back(BufferRegion(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined));
      } else {
        // Drop cells out of map.
        int sign = (indexShift(i) > 0 ? 1 : -1);
        int startIndex = startIndex_(i) - (sign < 0 ? 1 : 0);
        int endIndex = startIndex - sign + indexShift(i);
        int nCells = abs(indexShift(i));
        int index = (sign > 0 ? startIndex : endIndex);
        mapIndexWithinRange(index, getSize()(i));

        if (index + nCells <= getSize()(i)) {
          // One region to drop.
          if (i == 0) {
            clearRows(index, nCells);
            newRegions.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(index, nCells);
            newRegions.push_back(BufferRegion(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined));
          }
        } else {
          // Two regions to drop.
          int firstIndex = index;
          int firstNCells = getSize()(i) - firstIndex;
          if (i == 0) {
            clearRows(firstIndex, firstNCells);
            newRegions.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(firstIndex, firstNCells);
            newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined));
          }

          int secondIndex = 0;
          int secondNCells = nCells - firstNCells;
          if (i == 0) {
            clearRows(secondIndex, secondNCells);
            newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
          } else if (i == 1) {
            clearCols(secondIndex, secondNCells);
            newRegions.push_back(BufferRegion(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined));
          }
        }
      }
    }
  }

  // Update information.
  startIndex_ += indexShift;
  mapIndexWithinRange(startIndex_, getSize());
  position_ += alignedPositionShift;

  // Check if map has been moved at all.
  return (indexShift.any() != 0);
}

bool GridMap::move(const Position& position)
{
  std::vector<BufferRegion> newRegions;
  return move(position, newRegions);
}

bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteData,
                          bool copyAllLayers, std::vector<std::string> layers)
{
  // Set the layers to copy.
  if (copyAllLayers) layers = other.getLayers();

  // Resize map.
  if (extendMap) extendToInclude(other);

  // Check if all layers to copy exist and add missing layers.
  for (const auto& layer : layers) {
    if (std::find(layers_.begin(), layers_.end(), layer) == layers_.end()) {
      add(layer);
    }
  }
  // Copy data.
  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
    if (isValid(*iterator) && !overwriteData) continue;
    Position position;
    getPosition(*iterator, position);
    Index index;
    if (!other.isInside(position)) continue;
    other.getIndex(position, index);
    for (const auto& layer : layers) {
      if (!other.isValid(index, layer)) continue;
      at(layer, *iterator) = other.at(layer, index);
    }
  }

  return true;
}

bool GridMap::extendToInclude(const GridMap& other)
{
  // Get dimension of maps.
  Position topLeftCorner(position_.x() + length_.x() / 2.0, position_.y() + length_.y() / 2.0);
  Position bottomRightCorner(position_.x() - length_.x() / 2.0, position_.y() - length_.y() / 2.0);
  Position topLeftCornerOther(other.getPosition().x() + other.getLength().x() / 2.0, other.getPosition().y() + other.getLength().y() / 2.0);
  Position bottomRightCornerOther(other.getPosition().x() - other.getLength().x() / 2.0, other.getPosition().y() - other.getLength().y() / 2.0);
  // Check if map needs to be resized.
  bool resizeMap = false;
  Position extendedMapPosition = position_;
  Length extendedMapLength = length_;
  if (topLeftCornerOther.x() > topLeftCorner.x()) {
    extendedMapPosition.x() += (topLeftCornerOther.x() - topLeftCorner.x()) / 2.0;
    extendedMapLength.x() += topLeftCornerOther.x() - topLeftCorner.x();
    resizeMap = true;
  }
  if (topLeftCornerOther.y() > topLeftCorner.y()) {
    extendedMapPosition.y() += (topLeftCornerOther.y() - topLeftCorner.y()) / 2.0;
    extendedMapLength.y() += topLeftCornerOther.y() - topLeftCorner.y();
    resizeMap = true;
  }
  if (bottomRightCornerOther.x() < bottomRightCorner.x()) {
    extendedMapPosition.x() -= (bottomRightCorner.x() - bottomRightCornerOther.x()) / 2.0;
    extendedMapLength.x() += bottomRightCorner.x() - bottomRightCornerOther.x();
    resizeMap = true;
  }
  if (bottomRightCornerOther.y() < bottomRightCorner.y()) {
    extendedMapPosition.y() -= (bottomRightCorner.y() - bottomRightCornerOther.y()) / 2.0;
    extendedMapLength.y() += bottomRightCorner.y() - bottomRightCornerOther.y();
    resizeMap = true;
  }
  // Resize map and copy data to new map.
  if (resizeMap) {
    GridMap mapCopy = *this;
    setGeometry(extendedMapLength, resolution_, extendedMapPosition);
    // Align new map with old one.
    Vector shift = position_ - mapCopy.getPosition();
    shift.x() = std::fmod(shift.x(), resolution_);
    shift.y() = std::fmod(shift.y(), resolution_);
    if (std::abs(shift.x()) < resolution_ / 2.0) {
      position_.x() -= shift.x();
    } else {
      position_.x() += resolution_ - shift.x();
    }
    if (size_.x() % 2 != mapCopy.getSize().x() % 2) {
      position_.x() += -std::copysign(resolution_ / 2.0, shift.x());
    }
    if (std::abs(shift.y()) < resolution_ / 2.0) {
      position_.y() -= shift.y();
    } else {
      position_.y() += resolution_ - shift.y();
    }
    if (size_.y() % 2 != mapCopy.getSize().y() % 2) {
      position_.y() += -std::copysign(resolution_ / 2.0, shift.y());
    }
    // Copy data.
    for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
      if (isValid(*iterator)) continue;
      Position position;
      getPosition(*iterator, position);
      Index index;
      if (!mapCopy.isInside(position)) continue;
      mapCopy.getIndex(position, index);
      for (const auto& layer : layers_) {
        at(layer, *iterator) = mapCopy.at(layer, index);
      }
    }
  }
  return true;
}

void GridMap::setTimestamp(const Time timestamp)
{
  timestamp_ = timestamp;
}

Time GridMap::getTimestamp() const
{
  return timestamp_;
}

void GridMap::resetTimestamp()
{
  timestamp_ = 0.0;
}

void GridMap::setFrameId(const std::string& frameId)
{
  frameId_ = frameId;
}

const std::string& GridMap::getFrameId() const
{
  return frameId_;
}

const Length& GridMap::getLength() const
{
  return length_;
}

const Position& GridMap::getPosition() const
{
  return position_;
}

double GridMap::getResolution() const
{
  return resolution_;
}

const Size& GridMap::getSize() const
{
  return size_;
}

void GridMap::setStartIndex(const Index& startIndex) {
  startIndex_ = startIndex;
}

const Index& GridMap::getStartIndex() const
{
  return startIndex_;
}

void GridMap::convertToDefaultStartIndex()
{
  if ((startIndex_ == 0).all()) return;

  std::vector<BufferRegion> bufferRegions;
  if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) {
    throw std::out_of_range("Cannot access submap of this size.");
  }

  for (auto& data : data_) {
    auto tempData(data.second);
    for (const auto& bufferRegion : bufferRegions) {
      Index index = bufferRegion.getStartIndex();
      Size size = bufferRegion.getSize();

      if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
        tempData.topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
        tempData.topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
        tempData.bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
        tempData.bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      }
    }
    data.second = tempData;
  }

  startIndex_.setZero();
}

void GridMap::clear(const std::string& layer)
{
  try {
    data_.at(layer).setConstant(NAN);
  } catch (const std::out_of_range& exception) {
    throw std::out_of_range("GridMap::clear(...) : No map layer '" + layer + "' available.");
  }
}

void GridMap::clearBasic()
{
  for (auto& layer : basicLayers_) {
    clear(layer);
  }
}

void GridMap::clearAll()
{
  for (auto& data : data_) {
    data.second.setConstant(NAN);
  }
}

void GridMap::clearRows(unsigned int index, unsigned int nRows)
{
  std::vector<std::string> layersToClear;
  if (basicLayers_.size() > 0) layersToClear = basicLayers_;
  else layersToClear = layers_;
  for (auto& layer : layersToClear) {
    data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN);
  }
}

void GridMap::clearCols(unsigned int index, unsigned int nCols)
{
  std::vector<std::string> layersToClear;
  if (basicLayers_.size() > 0) layersToClear = basicLayers_;
  else layersToClear = layers_;
  for (auto& layer : layersToClear) {
    data_.at(layer).block(0, index, getSize()(0), nCols).setConstant(NAN);
  }
}

bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Position& position,
                                           float& value) const
{
  Position point;
  Index indices[4];
  bool idxTempDir;
  size_t idxShift[4];

  getIndex(position, indices[0]);
  getPosition(indices[0], point);

  if (position.x() >= point.x()) {
    indices[1] = indices[0] + Index(-1, 0); // Second point is above first point.
    idxTempDir = true;
  } else {
    indices[1] = indices[0] + Index(+1, 0);
    idxTempDir = false;
  }
  if (position.y() >= point.y()) {
      indices[2] = indices[0] + Index(0, -1); // Third point is right of first point.
      if(idxTempDir){ idxShift[0]=0; idxShift[1]=1; idxShift[2]=2; idxShift[3]=3; }
      else          { idxShift[0]=1; idxShift[1]=0; idxShift[2]=3; idxShift[3]=2; }

  } else {
      indices[2] = indices[0] + Index(0, +1);
      if(idxTempDir){ idxShift[0]=2; idxShift[1]=3; idxShift[2]=0; idxShift[3]=1; }
      else          { idxShift[0]=3; idxShift[1]=2; idxShift[2]=1; idxShift[3]=0; }
  }
  indices[3].x() = indices[1].x();
  indices[3].y() = indices[2].y();

  const Size& mapSize = getSize();
  const size_t bufferSize = mapSize(0) * mapSize(1);
  const size_t startIndexLin = getLinearIndexFromIndex(startIndex_, mapSize);
  const size_t endIndexLin = startIndexLin + bufferSize;
  const auto& layerMat = operator[](layer);
  float         f[4];

  for (size_t i = 0; i < 4; ++i) {
    const size_t indexLin = getLinearIndexFromIndex(indices[idxShift[i]], mapSize);
    if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) return false;
    f[i] = layerMat(indexLin);
  }

  getPosition(indices[idxShift[0]], point);
  const Position positionRed     = ( position - point ) / resolution_;
  const Position positionRedFlip = Position(1.,1.) - positionRed;

  value = f[0] * positionRedFlip.x() * positionRedFlip.y() +
          f[1] *     positionRed.x() * positionRedFlip.y() +
          f[2] * positionRedFlip.x() *     positionRed.y() +
          f[3] *     positionRed.x() *     positionRed.y();
  return true;
}

void GridMap::resize(const Index& size)
{
  size_ = size;
  for (auto& data : data_) {
    data.second.resize(size_(0), size_(1));
  }
}

} /* namespace */


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/
/*
------------------ Author: Chris Laurel  -------------------------------------------------
 ------------------ E-mail: (claurel@gmail.com) ----------------------------
 // Patched bu Guillermo June 20th 2009
 //Patched by Ricardo Noriega May 2010
 */

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <QTemporaryFile>
#include <QtAlgorithms>
#include <QtDebug>

#include "propagatedscenario.h"
#include <Astro-Core/RotationState.h>
#include <Astro-Core/Interpolators.h>
#include <Astro-Core/stamath.h>
#include "Constellations/cstudy.h"

using namespace sta;
using namespace staAttitude;
using namespace Eigen;

typedef Eigen::Matrix< double, 3, 3 > 	MyMatrix3d;
typedef Eigen::Matrix< double, 3, 1 > 	MyVector3d;

/****** MissionArc ******/

/** Create a new MissionArc with no attitude information.
  *
  * The list of sampleTimes and samples must have identical sizes (i.e. one
  * time tag per state vector.)
  */
MissionArc::MissionArc(const StaBody* centralBody,
                       sta::CoordinateSystem coordSys,
                       QList<double> sampleTimes,
                       QList<sta::StateVector> samples) :
    m_centralBody(centralBody),
    m_coordSys(coordSys),
    m_beginning(0.0),
    m_ending(0.0),
    m_sampleTimes(sampleTimes),
    m_samples(samples),
    m_ephemerisTempFile(NULL)
{
    Q_ASSERT(m_sampleTimes.size() == m_samples.size());
    Q_ASSERT(m_samples.size() > 0);

    m_beginning = m_sampleTimes.first();
    m_ending = m_sampleTimes.last();
}

/** Create a new MissionArc with attitude states.
  *
  * The list of sampleTimes and samples must have identical sizes (i.e. one
  * time tag per state vector.) The lists of attitude sample times and attitude
  * vectors must also have the same size, though they may be different than the
  * size of the list of trajectory samples.
  */
MissionArc::MissionArc(const StaBody* centralBody,
                       sta::CoordinateSystem coordSys,
                       QList<double> sampleTimes,
                       QList<sta::StateVector> samples,
                       QList<double> attitudeSampleTimes,
                       QList<AttitudeVector> attitudeSamples) :
    m_centralBody(centralBody),
    m_coordSys(coordSys),
    m_beginning(0.0),
    m_ending(0.0),
    m_sampleTimes(sampleTimes),
    m_samples(samples),
    m_attitudeSampleTimes(attitudeSampleTimes),
    m_attitudeSamples(attitudeSamples),
    m_ephemerisTempFile(NULL)
{
    Q_ASSERT(m_sampleTimes.size() == m_samples.size());
    Q_ASSERT(m_samples.size() > 0);
    Q_ASSERT(m_attitudeSampleTimes.size() == m_attitudeSamples.size());

    m_beginning = m_sampleTimes.first();
    m_ending = m_sampleTimes.last();
}

MissionArc::~MissionArc()
{
    // deleting the QTemporaryFile objects will clean up the temp files
    delete m_ephemerisTempFile;
}

/** Return the state vector for the trajectory at a specified time, using
 *  cubic interpolation between sample times.
 *
 *  \param mjd: a modified Julian Date, TDB
 *  \param result: pointer to a state vector in which the result will be stored
 *
 *  The function returns true if the state vector could be calculated, and false
 *  if there was an error. Errors may result if this function is called before
 *  the trajectory has been propagated, or if the requested time is not
 *  within the range of time over which trajectory is valid. The returned
 *  state vector is in the same coordinate system used to propagate this arc of
 *  the trajectory.
 */
bool
MissionArc::getStateVector(double mjd,
                           sta::StateVector* result) const
{
    Q_ASSERT(m_sampleTimes.size() == m_samples.size());

    if (!m_sampleTimes.isEmpty() &&
        mjd >= m_sampleTimes.first() &&
        mjd <= m_sampleTimes.last())
    {
        int i = -1;
        if (m_lastSample > 0 &&
            m_lastSample < m_sampleTimes.size() &&
            mjd >= m_sampleTimes[m_lastSample - 1] &&
            mjd <= m_sampleTimes[m_lastSample])
        {
            i = m_lastSample;
        }
        else if (mjd == m_sampleTimes.first())
        {
            // Need special handling for the situation when requested time
            // exactly equal to the first sample time.
            i = 1;
        }
        else
        {
            QList<double>::const_iterator iter = qLowerBound(m_sampleTimes, mjd);

            // This can only occur if the samples aren't sorted
            Q_ASSERT(iter != m_sampleTimes.end());
            Q_ASSERT(iter != m_sampleTimes.begin());

            i = iter - m_sampleTimes.begin();
        }

        double h = m_sampleTimes[i] - m_sampleTimes[i - 1];
        double t = (mjd - m_sampleTimes[i - 1]) / h;

        *result = cubicInterpolate(m_samples[i - 1],
                                   m_samples[i],
                                   t,
                                   sta::daysToSecs(h));

        m_lastSample = i;

        return true;
    }
    else
    {
        return false;
    }
}

/** Return the number of samples in the trajectory.
 */
int
MissionArc::trajectorySampleCount() const
{
    return m_samples.size();
}

/** Return the state vector at the specified sample index. The index
 *  must be >= 0 and less than the sample count.
 */
sta::StateVector
MissionArc::trajectorySample(int index) const
{
    Q_ASSERT(index < m_samples.size());
    return m_samples[index];
}

/** Return the time stamp for the specified sample index. The index
 *  must be >= 0 and less than the sample count.
 */
double
MissionArc::trajectorySampleTime(int index) const
{
    Q_ASSERT(index < m_sampleTimes.size());
    return m_sampleTimes[index];
}

/** Return the number of attitude samples in this arc. Returns zero
  * when attitude isn't available for the arc.
  */
int
MissionArc::attitudeSampleCount() const
{
    return m_attitudeSamples.size();
}

/** Return the attitude vector at the specified. The index must be >= 0
  * and less than the attitude sample count.
  */
AttitudeVector
MissionArc::attitudeSample(int index) const
{
    Q_ASSERT(index < m_attitudeSamples.size());
    return m_attitudeSamples[index];
}

/** Return the time stamp for the specified attiude sample index. The index
 *  must be >= 0 and less than the attitude sample count.
 */
double
MissionArc::attitudeSampleTime(int index) const
{
    Q_ASSERT(index < m_attitudeSampleTimes.size());
    return m_attitudeSampleTimes[index];
}

/** Return the attitude vector for the arc at a specified time, using
 *  spherical linear interpolation when the requested time lies between
 *  two attitude time tags.
 *
 *  \param mjd: a modified Julian Date, TDB
 *  \param result: pointer to an attitude vector in which the result will be stored
 *
 *  The function returns true if the attitude vector could be calculated, and false
 *  if there was an error. Errors may result if this function is called before
 *  the arc has been propagated, or if the requested time is not
 *  within the range of time over which arc is valid. The returned
 *  attitude vector is in the same coordinate system used when propagating
 *  attitude.
 */
bool
MissionArc::getAttitude(double mjd, AttitudeVector* result) const
{
    Q_ASSERT(m_attitudeSampleTimes.size() == m_attitudeSamples.size());

    if (!m_attitudeSampleTimes.isEmpty() &&
        mjd >= m_attitudeSampleTimes.first() &&
        mjd <= m_attitudeSampleTimes.last())
    {
        int i = -1;
        if (mjd == m_attitudeSampleTimes.first())
        {
            // Need special handling for the situation when requested time
            // exactly equal to the first sample time.
            i = 1;
        }
        else
        {
            QList<double>::const_iterator iter = qLowerBound(m_attitudeSampleTimes, mjd);

            // This can only occur if the samples aren't sorted
            Q_ASSERT(iter != m_attitudeSampleTimes.end());
            Q_ASSERT(iter != m_attitudeSampleTimes.begin());

            i = iter - m_attitudeSampleTimes.begin();
        }

        double h = m_attitudeSampleTimes[i] - m_attitudeSampleTimes[i - 1];
        double t = (mjd - m_attitudeSampleTimes[i - 1]) / h;

        // Perform spherical linear interpolation between quaternions; no interpolation
        // is performed on the rates.
        // TODO: Interpolate using the rates; in order to satisfy conditions at the sample
        // times, we need at least cubic spherical interpolation.
        Quaterniond q0 = m_attitudeSamples[i - 1].myQuaternion;
        Quaterniond q1 = m_attitudeSamples[i].myQuaternion;

        result->myBodyRates = m_attitudeSamples[i - 1].myBodyRates;
        result->myQuaternion = q0.slerp(t, q1);

        return true;
    }
    else
    {
        return false;
    }
}

/** Write an ASCII ephemeris file for this mission arc. One record
 *  is written for each sample. The format of each record is:
 *
 * <mjd> <x> <y> <z> <vx> <vy> <vz>
 *
 * mjd is a modified Julian date
 * x, y, and z are the position
 * vx, vy, and vz are the velocity
 *
 * Return true if the file was successfully created and written.
 */
bool
MissionArc::generateEphemerisFile()
{
    // Create a new temporary file for the ephemeris
    if (m_ephemerisTempFile != NULL)
        delete m_ephemerisTempFile;

    m_ephemerisTempFile = new QTemporaryFile("ephem-XXXXXX.stae");
    if (!m_ephemerisTempFile->open())
    {
        delete m_ephemerisTempFile;
        m_ephemerisTempFile = NULL;
        m_ephemerisFile = "";
        return false;
    }

    m_ephemerisFile = m_ephemerisTempFile->fileName();

    QTextStream ephemerisStream(m_ephemerisTempFile);

    // Output values at full double precision
    ephemerisStream.setRealNumberPrecision(16);

    // Write all samples
    for (int i = 0; i < m_sampleTimes.size(); ++i)
    {
        ephemerisStream << m_sampleTimes[i] << " "
                        << m_samples[i].position.x() << " "
                        << m_samples[i].position.y() << " "
                        << m_samples[i].position.z() << " "
                        << m_samples[i].velocity.x() << " "
                        << m_samples[i].velocity.y() << " "
                        << m_samples[i].velocity.z() << endl;
    }

    m_ephemerisTempFile->close();

    return true;
}

/****** PSAntennaObject    ******/

PSAntennaObject::PSAntennaObject():
        m_azimuth(0.0),
        m_elevation(0.0),
        m_coneShape(0),
        m_coneAngle(0.0),
        m_observationChecked(false)
{
}

PSAntennaObject::~PSAntennaObject()
{
}

double PSAntennaObject::getAzimuth()
{
    return m_azimuth;
}

double PSAntennaObject::getElevation()
{
    return m_elevation;
}

int PSAntennaObject::getConeShape()
{
    return m_coneShape;
}

double PSAntennaObject::getConeAngle()
{
    return m_coneAngle;
}

bool PSAntennaObject::getObservationChecked()
{
    return m_observationChecked;
}

void PSAntennaObject::setAzimuth(double az)
{
    m_azimuth = az;
}

void PSAntennaObject::setElevation(double el)
{
    m_elevation = el;
}

void PSAntennaObject::setConeShape(int cs)
{
    m_coneShape = cs;
}

void PSAntennaObject::setConeAngle(double ca)
{
    m_coneAngle = ca;
}
void PSAntennaObject::setObservationChecked(bool oc)
{
    m_observationChecked = oc;
}

/****** SpaceObject        ******/

SpaceObject::SpaceObject(ScenarioParticipantType* participant) :
    m_missionStartTime(sta::JdToMjd(sta::J2000)),
    m_missionEndTime(sta::JdToMjd(sta::J2000)),
    m_participant(participant)
{
}

SpaceObject::~SpaceObject()
{
    foreach (MissionArc* arc, m_missionArcs)
    {
        delete arc;
    }
}

void
SpaceObject::addMissionArc(MissionArc* arc)
{
    // Extend the time range of the mission to include the span of the newly added arc
    // TODO: enforce contiguity of mission arcs
    if (m_missionArcs.empty() || arc->beginning() < m_missionStartTime)
        m_missionStartTime = arc->beginning();
    if (m_missionArcs.empty() || arc->ending() > m_missionEndTime)
        m_missionEndTime = arc->ending();

    m_missionArcs.append(arc);
}

void
SpaceObject::addReceiver(PSAntennaObject* rec)
{
    m_receiver.append(rec);
}

void
SpaceObject::addTransmitter(PSAntennaObject* tra)
{
    m_transmitter.append(tra);
}

/*! Return the state vector for this space vehicle at the specified time.
 *  @param mjd: a modified Julian Date, TDB
 *  @param center: the returned state vector is relative to the center object
 *  @param coordSys:
 *  @param result: pointer to a state vector in which the result will be stored
 *
 *  The function returns true if the state vector could be calculated, and false
 *  if there was an error. Errors may result if  the requested time is not
 *  within the range of times where the space vehicle's trajectory is defined.
 *
 *  The returned state vector is not corrected for light time or aberration.
 */
bool
SpaceObject::getStateVector(double mjd,
                            const StaBody& center,
                            const sta::CoordinateSystem& coordSys,
                            sta::StateVector* result) const
{
    if (mission().empty())
    {
        return false;
    }

    // Find the mission arc that contains the requested time.
    // TODO: The simple linear search is adequate for a small number of mission arcs.
    MissionArc* arc = NULL;
    foreach (arc, mission())
    {
        if (mjd >= arc->beginning() && mjd <= arc->ending())
        {
            break;
        }
    }

    // Requested time does not lie within any mission arcs
    if (!arc)
    {
        return false;
    }

    sta::StateVector v;
    if (!arc->getStateVector(mjd, &v))
    {
        return false;
    }

    // Convert the state vector to the requested coordinate system
    *result = CoordinateSystem::convert(v, mjd,
                                        arc->centralBody(), arc->coordinateSystem(),
                                        &center, coordSys);

    return true;
}

/*! Generate ephemeris file for all mission arcs. Return true if all segments
 *  were written succesfully. This method aborts and return false immediately
 *  after the first unsuccessful attempt to write an ephemeris.
 */
bool
SpaceObject::generateEphemerisFiles()
{
    foreach (MissionArc* arc, mission())
    {
        if (!arc->generateEphemerisFile())
            return false;
    }

    return true;
}

/****** GroundObject ******/

GroundObject::GroundObject()
{
}

GroundObject::~GroundObject()
{
}

/** Return the apparent angle of a spacecraft above a ground station's horizon.
  *
  * @return the elevation angle in radians
  */
double
GroundObject::elevationAngle(const SpaceObject* spacecraft, double t) const
{
    sta::StateVector spacecraftState;

    spacecraft->getStateVector(t, *centralBody, sta::CoordinateSystem(sta::COORDSYS_BODYFIXED), &spacecraftState);
    MyVector3d stationPos = centralBody->planetographicToCartesian(latitude, longitude, 0.0);

    // TODO: should use ellipsoid normal
    MyVector3d up = stationPos.normalized();

    MyVector3d toSpacecraft = (spacecraftState.position - stationPos).normalized();
    double cosAngle = up.dot(toSpacecraft);
    double angleFromZenith;

    // Be careful about arccos domain...
    if (cosAngle < -1.0 + Eigen::precision<double>())
    {
        // Satellite is directly behind the antenna
        angleFromZenith = sta::Pi();
    }
    else if (cosAngle > 1.0 - Eigen::precision<double>())
    {
        // Satellite exactly aligned with antenna
        angleFromZenith = 0.0;
    }
    else
    {
        angleFromZenith = std::acos(cosAngle);
    }

    return sta::halfPi() - angleFromZenith;
}
//This function is added by Ricardo Noriega to calculate the azimuth between a groundStation and a given spacecraft at a certain time
double GroundObject::azimuthAngle(const SpaceObject *spacecraft, double t) const
{

    //First, calculate the vector R between SC and GS

    sta::StateVector spacecraftState;

    spacecraft->getStateVector(t, *centralBody, sta::CoordinateSystem(sta::COORDSYS_BODYFIXED), &spacecraftState);
    MyVector3d stationPos = centralBody->planetographicToCartesian(latitude, longitude, 0.0);

    // TODO: should use ellipsoid normal

    MyVector3d toSpacecraft = (spacecraftState.position - stationPos);

    //Some parameters
    double lat=latitude*DEG2RAD;
    double lon=longitude*DEG2RAD;
    double cl=cos(lat);
    double sl=sin(lat);
    double co=cos(lon);
    double so=sin(lon);

    //Calculate groundStation's unit vectors UP, EAST and NORHT in Geocentric coords.

    double Ux, Uy, Uz;
    double Ex, Ey, Ez;
    double Nx, Ny, Nz;

    Ux=cl*co;
    Uy=cl*so;
    Uz=sl;

    Ex=-so;
    Ey=co;
    Ez=0;

    Nx=-sl*co;
    Ny=-sl*so;
    Nz=cl;

    //GroundObject's XYZ coords at Earth's surface

   double Rx, Ry, Rz;

    Rx=toSpacecraft.x();
    Ry=toSpacecraft.y();
    Rz=toSpacecraft.z();

    //Resolution

    double Rxn, Ryn, Rzn;
    double range=toSpacecraft.norm();
    Rxn=Rx/range;
    Ryn=Ry/range;
    Rzn=Rz/range;

    double E, N;

    E=(Rxn*Ex)+(Ryn*Ey);
    N=(Rxn*Nx)+(Ryn*Ny)+(Rzn*Nz);

    double AZ=atan2(E,N);
    AZ=AZ*RAD2DEG;

    return AZ;

}

//This function is added by Ricardo Noriega to get the range between a groundStation and a spacecraft
double GroundObject::getRange(const SpaceObject *spacecraft, double t) const{

    sta::StateVector spacecraftState;

    spacecraft->getStateVector(t, *centralBody, sta::CoordinateSystem(sta::COORDSYS_BODYFIXED), &spacecraftState);
    MyVector3d stationPos = centralBody->planetographicToCartesian(latitude, longitude, 0.0);

    // TODO: should use ellipsoid normal

    MyVector3d toSpacecraft = (spacecraftState.position - stationPos);

    double range=toSpacecraft.norm();

    return range; //Km

}

void
GroundObject::addReceiver(PSAntennaObject* rec)
{
    m_receiver.append(rec);
}

void
GroundObject::addTransmitter(PSAntennaObject* tra)
{
    m_transmitter.append(tra);
}

RegionObject::RegionObject(const QString& name, const StaBody* centralBody) :
    m_name(name),
    m_centralBody(centralBody),
    m_color(Qt::white)
{
}

RegionObject::~RegionObject()
{
}

/** Set the boundary of the region. For any point in the boundary, the
  * x coordinate is the longitude and y is the longitude. The boundary
  * of a region must be convex.
  */
void
RegionObject::setBoundary(const QList<Eigen::Vector2d>& boundary)
{
    m_boundary = boundary;
}

void
RegionObject::setColor(const QColor& color)
{
    m_color = color;
}

/****** PropagatedScenario ******/

/*! Create a new, empty propagated scenario.
 */
PropagatedScenario::PropagatedScenario() :
    m_startTime(sta::JdToMjd(sta::J2000)),
    m_endTime(sta::JdToMjd(sta::J2000)),
    m_coverageTriggered(false),
    m_studyOfConstellations(NULL)
{
}

PropagatedScenario::~PropagatedScenario()
{
    foreach (SpaceObject *spaceObj, m_spaceObjectList)
    {
        delete spaceObj;
    }

    foreach (GroundObject* groundObj, m_groundObjectList)
    {
        delete groundObj;
    }
}

void
PropagatedScenario::setStudyOfConstellations()
{
    m_studyOfConstellations = new ConstellationStudy(this, true, false, false);
}

void
PropagatedScenario::addSpaceObject(SpaceObject* spaceObject)
{
    // Extend the time range of the scenario to include the mission
    // span of the space object.
    if (m_spaceObjectList.empty() || spaceObject->missionStartTime() < m_startTime)
        m_startTime = spaceObject->missionStartTime();
    if (m_spaceObjectList.empty() || spaceObject->missionEndTime() > m_endTime)
        m_endTime = spaceObject->missionEndTime();

    m_spaceObjectList.append(spaceObject);
}

void
PropagatedScenario::addGroundObject(GroundObject* groundObject)
{
    m_groundObjectList.append(groundObject);
}

void
PropagatedScenario::addRegionObject(RegionObject* regionObject)
{
    m_regionObjectList.append(regionObject);
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2015 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/rational.hpp>

int main()
{
#ifndef BOOST_NO_CONSTEXPR
   constexpr boost::rational<int> i1;
   constexpr boost::rational<int> i2(3);
   constexpr boost::rational<short> i3(i2);

   static_assert(i1.numerator() == 0, "constexpr test");
   static_assert(i1.denominator() == 1, "constexpr test");
   static_assert(i2.numerator() == 3, "constexpr test");
   static_assert(i2.denominator() == 1, "constexpr test");
   static_assert(i3.numerator() == 3, "constexpr test");
   static_assert(i3.denominator() == 1, "constexpr test");
   static_assert(!i1, "constexpr test");
   static_assert(i2, "constexpr test");
#endif
   return 0;
}


#include "liver/macrocell_tree.hpp"
#include "liver/tree_stats.hpp"
#include "utility/options.hpp"
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

using namespace jhmi;

binary_const_node_t<physical_vessel> find_porta_hepatis_vessel(macrocell_tree const& tree) {
  auto n = ranges::front(tree.vessel_tree().vessel_nodes());
  auto& liver = tree.liver_shape();
  while (n) {
    if (!liver(n.value().start()) && liver(n.value().end()))
      break;
    if (!n.left_child())
      n = n.right_child();
    else if (!n.right_child())
      n = n.left_child();
    else if (n.left_child() && n.right_child()) {
      if (n.left_child().value().flow() >= n.right_child().value().flow())
        n = n.left_child();
      else
        n = n.right_child();
    }
  }
  return n;
}
m3 find_porta_hepatis_location(physical_vessel const& v, voxelized_shape const& liver) {
  auto left = v.start();
  auto right = v.end();
  auto test = (left + right) / 2.;
  while (distance(test-right) < 100_um) {
    if (liver(test)) {
      right = test;
    }
    else {
      left = test;
    }
    test = (left + right) / 2.;
  }
  return test;
}
m find_outside_length(binary_const_node_t<physical_vessel> n, m3 const& loc) {
  auto length = distance(n.value().start() - loc);
  n = n.parent();
  while (n) {
    length += n.value().distance();
  }
  return length;
}
int main(int argc, char* argv[]) {
  GOOGLE_PROTOBUF_VERIFY_VERSION;

  auto opts = options<treefiles_option, data_directory_option>{};
  if (!opts.parse(argc, argv))
    return 1;
  auto liver = voxelized_shape{opts.data_directory() / "liver_extents.datz", adjust::do_open};
  auto liver_extents = extents(liver);
  fmt::print("Liver location (mm): {} {}\n", dbl3{1000. * liver_extents.ul()/meters},
                                             dbl3{1000. * liver_extents.lr()/meters});
  fmt::print("Liver volume: {}\n", liver.volume());
  auto lobule_volume = lobule::cell_thickness * lobule::side_length * lobule::min_radius * 3.;
  fmt::print("Lobule volume: {}\n", lobule_volume);
  fmt::print("Expected no. of lobules: {}\n", double(liver.volume() / lobule_volume));
  for (auto treefile : opts.treefiles()) {
    fmt::print("Tree at {}\n", treefile.string());
    auto tree = macrocell_tree{load_tree, treefile, liver};
#if 0
    fmt::print("here1\n");
    auto porta_hepatis = find_porta_hepatis_vessel(tree);
    if (!porta_hepatis) {
      fmt::print("Nothing in the liver?\n");
      return 1;
    }
    fmt::print("here2\n");
    auto start = find_porta_hepatis_location(porta_hepatis.value(), tree.liver_shape());
    fmt::print("here3\n");
    auto outside_length = find_outside_length(porta_hepatis, start);
    fmt::print("here4\n");
    auto intra_distances = std::vector<m>{};
    for (auto& cell : tree.macrocells().list()) {
      intra_distances.push_back(distance(cell.center - start));
    }
    auto mean_dist = mean(intra_distances);
    fmt::print("Avg distance PH to macrocells: {} ({})\nTotal: {}\n",
               mean_dist, sqrt(variance(intra_distances)), mean_dist + outside_length);
#endif
    auto ts = calc_tree_stats(tree, liver);
    print_tree_stats(ts);
    write_tree_stats(ts, treefile.parent_path());
  }
  google::protobuf::ShutdownProtobufLibrary();
}


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_MONE_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_MONE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the mone constant (-1).

    @return The Mone constant for the proper type
  **/
  template<typename T> T Mone();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant mone.

      @return The Mone constant (-1) for the proper type
    **/
    const boost::dispatch::functor<tag::mone_> mone = {};
  }
} }
#endif

#include <boost/simd/constant/definition/mone.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 "A_ol I_ol" BA_olI_ol,
 * WITHOUT WARRANTIE_ol OR CONDITION_ol OF ANY KIND, either express or implied.
 * _olee the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
// Overload of uBLAS prod function with MKL/GSL implementations
#include <votca/tools/linalg.h>

#include <boost/math/constants/constants.hpp>
#include "votca/xtp/radial_euler_maclaurin_rule.h"
#include "votca/xtp/aobasis.h"
#include <votca/xtp/aomatrix.h>

namespace votca { namespace xtp {

    std::vector<double> EulerMaclaurinGrid::getPruningIntervals(string element){

        std::vector<double> _r;

        // get Bragg-Slater Radius for this element
        double BSradius = _BraggSlaterRadii.at(element);

        // row type of element
        int RowType = _pruning_set.at(element);

        if ( RowType == 1 ){

            _r.push_back( 0.25 * BSradius );
            _r.push_back( 0.5 * BSradius );
            _r.push_back( 1.0 * BSradius );
            _r.push_back( 4.5 * BSradius );

        } else if ( RowType == 2 ){

            _r.push_back( 0.1667 * BSradius );
            _r.push_back( 0.5 * BSradius );
            _r.push_back( 0.9 * BSradius );
            _r.push_back( 3.5 * BSradius );

        } else if ( RowType == 3 ) {

            _r.push_back( 0.1 * BSradius );
            _r.push_back( 0.4 * BSradius );
            _r.push_back( 0.8 * BSradius );
            _r.push_back( 2.5 * BSradius );

        } else {

            cerr << "Pruning unsupported for RowType " << RowType << endl;
            exit(1);
        }

        return _r;
     }

void EulerMaclaurinGrid::getRadialCutoffs(std::vector<ctp::QMAtom* > _atoms, BasisSet* bs, string gridtype) {

            map<string, min_exp>::iterator it;
            std::vector< ctp::QMAtom* > ::iterator ait;
            std::vector< ctp::QMAtom* > ::iterator bit;

            double eps = Accuracy[gridtype];
            double _decaymin;
            int _lvalue;
            //  cout << endl << " Setting cutoffs for grid type " << gridtype << " eps = " << eps << endl;
            // 1) is only element based
            // loop over atoms
            for (ait = _atoms.begin(); ait < _atoms.end(); ++ait) {
                // get element type of the atom
                string name = (*ait)->type;
                // is this element already in map?
                it = _element_ranges.find(name);
                // only proceed, if element data does not exist yet
                if (it == _element_ranges.end()) {
                    // get first range estimate and add to map
                    min_exp this_atom;
                    double range_max = 0.0;
                    // get the basis set entry for this element
                    Element* _element = bs->getElement(name);
                    // and loop over all shells to figure out minimum decay constant and angular momentum of this function
                    for (Element::ShellIterator its = _element->firstShell(); its != _element->lastShell(); its++) {
                        Shell* shell = (*its);
                        _decaymin = 1e7;
                        _lvalue = 0;
                        int _lmax = shell->getLmax();
                        for (Shell::GaussianIterator itg = shell->firstGaussian(); itg != shell->lastGaussian(); itg++) {
                            GaussianPrimitive* gaussian = *itg;
                            double _decay = gaussian->decay;
                            if (_decay < _decaymin) {
                                _decaymin = _decay;
                                _lvalue = _lmax;
                            }
                        }
                        double range = DetermineCutoff(2 * _decaymin, 2 * _lvalue + 2, eps);
                        if (range > range_max) {
                            this_atom.alpha = _decaymin;
                            this_atom.l = _lvalue;
                            this_atom.range = range;
                            range_max = range;
                        }
                    } // shells

                    //   cout << "Element " << name << " alpha " << this_atom.alpha << " l " << this_atom.l << " Rcut " << this_atom.range << endl;
                    _element_ranges[name] = this_atom;
                } // new element
            } // atoms

            // calculate overlap matrix
            AOBasis aobasis;
            aobasis.AOBasisFill(bs, _atoms);
            AOOverlap _overlap;
            // Fill overlap
            _overlap.Fill(aobasis);

            // refining by going through all atom combinations
            // get collapsed index list
            int atidx = 0;
            std::vector<int> idxstart;
            std::vector<int> idxstop;
            int start = 0;
            int end = 0;
            for (AOBasis::AOShellIterator _row = aobasis.firstShell(); _row != aobasis.lastShell(); _row++) {

                const AOShell* _shell_row = aobasis.getShell(_row);

                if (_shell_row->getIndex() == atidx) {

                    end += _shell_row->getNumFunc();
                } else {

                    idxstart.push_back(start);
                    idxstop.push_back(end);
                    atidx++;
                    start = end;
                    end += _shell_row->getNumFunc();
                }
            }

            idxstart.push_back(start);
            idxstop.push_back(end);

            int aidx = 0;

            for (ait = _atoms.begin(); ait < _atoms.end(); ++ait) {

                int _a_start = idxstart[aidx];
                int _a_stop = idxstop[aidx];

                double range_max = 0.0;

                // get preset values for this atom type
                double exp_iat = _element_ranges.at((*ait)->type).alpha;
                int l_iat = _element_ranges.at((*ait)->type).l;

                vec pos_a = (*ait)->getPos() * tools::conv::bohr2ang;

                string type_diff;
                int bidx = 0;
                for (bit = _atoms.begin(); bit < _atoms.end(); ++bit) {

                    int _b_start = idxstart[bidx];
                    int _b_stop = idxstop[bidx];
                    vec pos_b = (*bit)->getPos() * tools::conv::bohr2ang;
                    // now do some overlap gymnastics
                    double s_max = 10.0;
                    if (aidx != bidx) {

                        // find overlap block of these two atoms
                        ub::matrix<double> _overlapblock = ub::project(_overlap.Matrix(), ub::range(_a_start, _a_stop), ub::range(_b_start, _b_stop));
                        // determine abs max of this block
                        s_max = 0.0;
                        for (unsigned i = 0; i < _overlapblock.size1(); i++) {
                            for (unsigned j = 0; j < _overlapblock.size2(); j++) {
                                s_max = std::max(s_max, std::abs(_overlapblock(i, j)));
                            }
                        }
                    }

                    if (s_max > 1e-5) {
                        // cout << " Atom " << aidx << " neighbor " << bidx << " s-max " << s_max << " exponents " << exp_iat << " and " << _element_ranges.at((*bit)->type).alpha << endl;
                        double range = DetermineCutoff(exp_iat + _element_ranges.at((*bit)->type).alpha, l_iat + _element_ranges.at((*bit)->type).l + 2, eps);
                        // now do some update trickery from Gaussian product formula
                        double dist = abs(pos_b - pos_a);
                        double shift_2g = dist * exp_iat / (exp_iat + _element_ranges.at((*bit)->type).alpha);
                        range += shift_2g;

                        if (aidx != bidx) range += dist;

                        if (range > range_max) {
                            //shiftm_2g = shift_2g;
                            range_max = range;
                            //iat_diff = bidx;
                            type_diff = (*bit)->type;
                        }

                    }

                    bidx++;
                }

                if (round(range_max) > _element_ranges.at((*ait)->type).range) {
                    _element_ranges.at((*ait)->type).range = round(range_max);
                }
                aidx++;
            }

          return;
        } // getRadialCutoffs

    void EulerMaclaurinGrid::getRadialGrid(BasisSet* bs , std::vector<ctp::QMAtom* > _atoms, string type, GridContainers& _grid) {

            map<string, min_exp>::iterator it;
            getRadialCutoffs(_atoms,bs,type);

            // go through all elements
            for ( it = _element_ranges.begin() ; it != _element_ranges.end() ; ++it){

                 // cout << "Element " << it->first << " alpha " << it->second.alpha << " l " << it->second.l << " Rcut " << it->second.range <<  " Rcut (Ang) " <<  it->second.range  * 0.529177249 << endl;

                std::vector<double> points;
                std::vector<double> weights;
                int numberofpoints = getGrid(it->first, type);
                //cout << " Setting grid for element " << it->first <<  " with " << numberofpoints << " points " <<  endl ;
                setGrid( numberofpoints, it->second.range, points, weights );

                //grid_element this_element;
                //this_element.gridpoint = points;
                //this_element.weight = weights;

                //_element_grids[it->first] = this_element;

                _grid._radial_grids[it->first].radius = points;
                _grid._radial_grids[it->first].weight = weights;

            }

    }

    void EulerMaclaurinGrid::setGrid(int np, double cutoff, std::vector<double>& point, std::vector<double>& weight ){

        double alpha = -cutoff/(log(1.0 - pow(  (1.0 + double(np))/(2.0 + double(np)),3)) );

        double factor = 3.0/(1.0+double(np));

        for ( int i = 0; i < np; i++){
            double q = double(i+1)/(double(np)+1.0);
            double r = -alpha*log(1.0-pow(q,3));
            double w = factor * alpha * r*r/( 1.0 - pow(q,3) ) * pow(q,2);

            point.push_back(r);
            weight.push_back(w);

        }

    }

    double EulerMaclaurinGrid::DetermineCutoff(double alpha, int l, double eps){

      // determine norm of function

     /* For a function f(r) = r^k*exp(-alpha*r^2) determine
        the radial distance r such that the fraction of the
        function norm that is neglected if the 3D volume
        integration is terminated at a distance r is less
        than or equal to eps. */

        double _cutoff    = 1.0; // initial value
        double _increment = 0.5; // increment

            while (_increment > 0.01) {
                double _neglected = getNeglected(alpha,l ,  _cutoff);
                // cout << "neglected is " << _neglected << endl;
                if (_neglected > eps) {
                    _cutoff += _increment;
                } else {
                    _cutoff -= _increment;
                    if (_cutoff < 0.0) _cutoff = 0.0;
                    _increment = 0.5 * _increment;
                    _cutoff += _increment;
                }
            }

        return _cutoff;

    }

    double EulerMaclaurinGrid::getNeglected(double alpha, int l, double cutoff){

        return RadialIntegral(alpha,l+2,cutoff) / RadialIntegral(alpha,l+2, 0.0);

    }

    double EulerMaclaurinGrid::RadialIntegral(double alpha, int l, double cutoff){

        const double pi = boost::math::constants::pi<double>();
        int ilo = l % 2;
        double value = 0.0;
        double valexp;
        if ( ilo == 0 ){
            double expo = sqrt(alpha)*cutoff;
            if ( expo > 40.0 ) {
                value = 0.0;
            } else {
                value = 0.5 * sqrt(  pi /alpha  ) * erfc(expo);
            }
        }

        double exponent = alpha*cutoff*cutoff;
        if ( exponent > 500.0 ) {
            valexp = 0.0;
            value = 0.0;
        } else {
            valexp = exp(-exponent);
            value = valexp/2.0/alpha;
        }

        for (  int i = ilo+2; i <= l; i+=2){
            value = ((i-1)*value + pow(cutoff,i-1)*valexp)/2.0/alpha;
        }

        return value;

    }

    int EulerMaclaurinGrid::getGrid(string element, string type){

        if ( type == "medium"){

            return MediumGrid.at(element);

        }
        else if ( type == "coarse"){

            return CoarseGrid.at(element);

        }
        else if ( type == "xcoarse"){

            return XcoarseGrid.at(element);

        }
        else if ( type == "fine"){

            return FineGrid.at(element);

        }
        else if ( type == "xfine"){

            return XfineGrid.at(element);

        }

        throw std::runtime_error("Grid type "+type+" is not implemented");
        return -1;

    }

}
}