// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/perspective_three_point.h"

#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <glog/logging.h>
#include <math.h>

#include "theia/math/polynomial.h"
#include "theia/sfm/pose/util.h"

namespace theia {
using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

namespace {

// Solves for cos(theta) that will describe the rotation of the plane from
// intermediate world frame to intermediate camera frame. The method returns the
// roots of a quartic (i.e. solutions to cos(alpha) ) and several factors that
// are needed for back-substitution.
int SolvePlaneRotation(const Vector3d normalized_image_points[3],
                       const Vector3d& intermediate_image_point,
                       const Vector3d& intermediate_world_point,
                       const double d_12,
                       double cos_theta[4],
                       double cot_alphas[4],
                       double* b) {
  // Calculate these parameters ahead of time for reuse and
  // readability. Notation for these variables is consistent with the notation
  // from the paper.
  const double f_1 = intermediate_image_point[0] / intermediate_image_point[2];
  const double f_2 = intermediate_image_point[1] / intermediate_image_point[2];
  const double p_1 = intermediate_world_point[0];
  const double p_2 = intermediate_world_point[1];
  const double cos_beta =
      normalized_image_points[0].dot(normalized_image_points[1]);
  *b = 1.0 / (1.0 - cos_beta * cos_beta) - 1.0;

  if (cos_beta < 0) {
    *b = -sqrt(*b);
  } else {
    *b = sqrt(*b);
  }

  // Definition of temporary variables for readability in the coefficients
  // calculation.
  const double f_1_pw2 = f_1 * f_1;
  const double f_2_pw2 = f_2 * f_2;
  const double p_1_pw2 = p_1 * p_1;
  const double p_1_pw3 = p_1_pw2 * p_1;
  const double p_1_pw4 = p_1_pw3 * p_1;
  const double p_2_pw2 = p_2 * p_2;
  const double p_2_pw3 = p_2_pw2 * p_2;
  const double p_2_pw4 = p_2_pw3 * p_2;
  const double d_12_pw2 = d_12 * d_12;
  const double b_pw2 = (*b) * (*b);

  // Computation of coefficients of 4th degree polynomial.
  Eigen::VectorXd coefficients(5);
  coefficients(0) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;
  coefficients(1) = 2.0 * p_2_pw3 * d_12 * (*b) +
                    2.0 * f_2_pw2 * p_2_pw3 * d_12 * (*b) -
                    2.0 * f_2 * p_2_pw3 * f_1 * d_12;
  coefficients(2) =
      -f_2_pw2 * p_2_pw2 * p_1_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 -
      f_2_pw2 * p_2_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 +
      2.0 * p_1 * p_2_pw2 * d_12 +
      2.0 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * (*b) -
      p_2_pw2 * p_1_pw2 * f_1_pw2 + 2.0 * p_1 * p_2_pw2 * f_2_pw2 * d_12 -
      p_2_pw2 * d_12_pw2 * b_pw2 - 2.0 * p_1_pw2 * p_2_pw2;
  coefficients(3) =
      2.0 * p_1_pw2 * p_2 * d_12 * (*b) + 2.0 * f_2 * p_2_pw3 * f_1 * d_12 -
      2.0 * f_2_pw2 * p_2_pw3 * d_12 * (*b) - 2.0 * p_1 * p_2 * d_12_pw2 * (*b);
  coefficients(4) = -2 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * (*b) +
                    f_2_pw2 * p_2_pw2 * d_12_pw2 + 2.0 * p_1_pw3 * d_12 -
                    p_1_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4 -
                    2.0 * f_2_pw2 * p_2_pw2 * p_1 * d_12 +
                    p_2_pw2 * f_1_pw2 * p_1_pw2 +
                    f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2;

  // Computation of roots.
  Eigen::VectorXd roots;
  FindPolynomialRoots(coefficients, &roots, NULL);

  // Calculate cot(alpha) needed for back-substitution.
  for (int i = 0; i < roots.size(); i++) {
    cos_theta[i] = roots(i);
    cot_alphas[i] = (-f_1 * p_1 / f_2 - cos_theta[i] * p_2 + d_12 * (*b)) /
                    (-f_1 * cos_theta[i] * p_2 / f_2 + p_1 - d_12);
  }

  return static_cast<int>(roots.size());
}

// Given the complete transformation between intermediate world and camera
// frames (parameterized by cos_theta and cot_alpha), back-substitute the
// solution and get an absolute camera pose.
void Backsubstitute(const Matrix3d& intermediate_world_frame,
                    const Matrix3d& intermediate_camera_frame,
                    const Vector3d& world_point_0,
                    const double cos_theta,
                    const double cot_alpha,
                    const double d_12,
                    const double b,
                    Vector3d* translation,
                    Matrix3d* rotation) {
  const double sin_theta = sqrt(1.0 - cos_theta * cos_theta);
  const double sin_alpha = sqrt(1.0 / (cot_alpha * cot_alpha + 1.0));
  double cos_alpha = sqrt(1.0 - sin_alpha * sin_alpha);

  if (cot_alpha < 0) {
    cos_alpha = -cos_alpha;
  }

  // Get the camera position in the intermediate world frame
  // coordinates. (Eq. 5 from the paper).
  const Vector3d c_nu(
      d_12 * cos_alpha * (sin_alpha * b + cos_alpha),
      cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha),
      sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha));

  // Transform c_nu into world coordinates. Use a Map to put the solution
  // directly into the output.
  *translation = world_point_0 + intermediate_world_frame.transpose() * c_nu;

  // Construct the transformation from the intermediate world frame to the
  // intermediate camera frame.
  Matrix3d intermediate_world_to_camera_rotation;
  intermediate_world_to_camera_rotation << -cos_alpha, -sin_alpha * cos_theta,
      -sin_alpha * sin_theta, sin_alpha, -cos_alpha * cos_theta,
      -cos_alpha * sin_theta, 0, -sin_theta, cos_theta;

  // Construct the rotation matrix.
  *rotation = (intermediate_world_frame.transpose() *
               intermediate_world_to_camera_rotation.transpose() *
               intermediate_camera_frame)
                  .transpose();

  // Adjust translation to account for rotation.
  *translation = -(*rotation) * (*translation);
}

}  // namespace

bool PoseFromThreePoints(const std::vector<Vector2d>& feature_point,
                         const std::vector<Vector3d>& points_3d,
                         std::vector<Matrix3d>* solution_rotations,
                         std::vector<Vector3d>* solution_translations) {
  Vector3d normalized_image_points[3];
  // Store points_3d in world_points for ease of use. NOTE: we cannot use a
  // const ref or a Map because the world_points entries may be swapped later.
  Vector3d world_points[3];
  for (int i = 0; i < 3; ++i) {
    normalized_image_points[i] = feature_point[i].homogeneous().normalized();
    world_points[i] = points_3d[i];
  }

  // If the points are collinear, there are no possible solutions.
  double kTolerance = 1e-6;
  Vector3d world_1_0 = world_points[1] - world_points[0];
  Vector3d world_2_0 = world_points[2] - world_points[0];
  if (world_1_0.cross(world_2_0).squaredNorm() < kTolerance) {
    VLOG(2) << "The 3 world points are collinear! No solution for absolute "
               "pose exits.";
    return false;
  }

  // Create intermediate camera frame such that the x axis is in the direction
  // of one of the normalized image points, and the origin is the same as the
  // absolute camera frame. This is a rotation defined as the transformation:
  // T = [tx, ty, tz] where tx = f0, tz = (f0 x f1) / ||f0 x f1||, and
  // ty = tx x tz and f0, f1, f2 are the normalized image points.
  Matrix3d intermediate_camera_frame;
  intermediate_camera_frame.row(0) = normalized_image_points[0];
  intermediate_camera_frame.row(2) =
      normalized_image_points[0].cross(normalized_image_points[1]).normalized();
  intermediate_camera_frame.row(1) =
      intermediate_camera_frame.row(2).cross(intermediate_camera_frame.row(0));

  // Project the third world point into the intermediate camera frame.
  Vector3d intermediate_image_point =
      intermediate_camera_frame * normalized_image_points[2];

  // Enforce that the intermediate_image_point is in front of the intermediate
  // camera frame. If the point is behind the camera frame, recalculate the
  // intermediate camera frame by swapping which feature we align the x axis to.
  if (intermediate_image_point[2] > 0) {
    std::swap(normalized_image_points[0], normalized_image_points[1]);

    intermediate_camera_frame.row(0) = normalized_image_points[0];
    intermediate_camera_frame.row(2) = normalized_image_points[0]
                                           .cross(normalized_image_points[1])
                                           .normalized();
    intermediate_camera_frame.row(1) = intermediate_camera_frame.row(2).cross(
        intermediate_camera_frame.row(0));

    intermediate_image_point =
        intermediate_camera_frame * normalized_image_points[2];

    std::swap(world_points[0], world_points[1]);
    world_1_0 = world_points[1] - world_points[0];
    world_2_0 = world_points[2] - world_points[0];
  }

  // Create the intermediate world frame transformation that has the
  // origin at world_points[0] and the x-axis in the direction of
  // world_points[1]. This is defined by the transformation: N = [nx, ny, nz]
  // where nx = (p1 - p0) / ||p1 - p0||
  // nz = nx x (p2 - p0) / || nx x (p2 -p0) || and ny = nz x nx
  // Where p0, p1, p2 are the world points.
  Matrix3d intermediate_world_frame;
  intermediate_world_frame.row(0) = world_1_0.normalized();
  intermediate_world_frame.row(2) =
      intermediate_world_frame.row(0).cross(world_2_0).normalized();
  intermediate_world_frame.row(1) =
      intermediate_world_frame.row(2).cross(intermediate_world_frame.row(0));

  // Transform world_point[2] to the intermediate world frame coordinates.
  Vector3d intermediate_world_point = intermediate_world_frame * world_2_0;

  // Distance from world_points[1] to the intermediate world frame origin.
  double d_12 = world_1_0.norm();

  // Solve for the cos(theta) that will give us the transformation from
  // intermediate world frame to intermediate camera frame. We also get the
  // cot(alpha) for each solution necessary for back-substitution.
  double cos_theta[4];
  double cot_alphas[4];
  double b;
  const int num_solutions = SolvePlaneRotation(normalized_image_points,
                                               intermediate_image_point,
                                               intermediate_world_point,
                                               d_12,
                                               cos_theta,
                                               cot_alphas,
                                               &b);

  // Backsubstitution of each solution
  solution_translations->resize(num_solutions);
  solution_rotations->resize(num_solutions);
  for (int i = 0; i < num_solutions; i++) {
    Backsubstitute(intermediate_world_frame,
                   intermediate_camera_frame,
                   world_points[0],
                   cos_theta[i],
                   cot_alphas[i],
                   d_12,
                   b,
                   &solution_translations->at(i),
                   &solution_rotations->at(i));
  }

  return num_solutions > 0;
}

}  // namespace theia


#include <algorithm>
#include <fstream>
#include <iterator>
#include <map>
#include <set>
#include <sstream>
#include <string>
#include <utility>

#include "utils/profiling.h"

#include <armadillo>

using namespace util;
using namespace arma;

bool checkMat(int row, int col, mat const &mat) {
    if(mat.n_rows == row && mat.n_cols == col) return true;
    return false;
}
bool checkVec(int dim, vec const &vec) {
    if(vec.size() == dim) return true;
    return false;
}

int main() {

    auto timer = Timer{};
    timer.here_then_reset("");

    int r_dim = 3;

    sp_mat M = sprandu<sp_mat>(1000,1000, 0.1);

    M(0,0) = 10;
    M(10,10)= 10;
    M(100,100) = 10;
    M(102,102) = 100;
    //M(105,105) = 100;
    mat U;
    vec s;
    mat V;

    svds(U,s,V,M,r_dim);

    s.print("s = ");
    return 0;
}


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

/// \copyright Copyright 2021 Apex.AI, Inc.
/// All rights reserved.

#ifndef STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_
#define STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_

#include <common/types.hpp>
#include <kalman_filter/esrcf.hpp>
#include <motion_model/constant_acceleration.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <state_estimation_nodes/history.hpp>
#include <state_estimation_nodes/measurement.hpp>
#include <state_estimation_nodes/measurement_typedefs.hpp>
#include <state_estimation_nodes/steady_time_grid.hpp>
#include <state_estimation_nodes/visibility_control.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <limits>
#include <cstdint>
#include <memory>
#include <string>
#include <chrono>

namespace autoware
{
namespace prediction
{

///
/// @brief      This class provides a high level interface to the Kalman Filter allowing to predict
///             the state of the filter with time and observe it by receiving ROS messages.
///
/// @tparam     MotionModelT      An underlying motion model.
/// @tparam     kNumOfStates      Number of states of the system.
/// @tparam     kProcessNoiseDim  Dimentionality of the noise.
///
template<typename MotionModelT, std::int32_t kNumOfStates, std::int32_t kProcessNoiseDim>
class STATE_ESTIMATION_NODES_PUBLIC KalmanFilterWrapper
{
  using FilterT = prediction::kalman_filter::Esrcf<kNumOfStates, kProcessNoiseDim>;

  template<std::int32_t kRows, std::int32_t kCols>
  using RectangularMatrixT = Eigen::Matrix<common::types::float32_t, kRows, kCols>;

  template<std::int32_t kNum>
  using SquareMatrixT = Eigen::Matrix<common::types::float32_t, kNum, kNum>;

  template<std::int32_t kLength>
  using VectorT = Eigen::Matrix<common::types::float32_t, kLength, 1>;

  using HistoryT = History<
    FilterT,
    kNumOfStates,
    PredictionEvent,
    ResetEvent<FilterT>,
    MeasurementPose,
    MeasurementSpeed,
    MeasurementPoseAndSpeed>;

public:
  ///
  /// @brief      Create an EKF wrapper.
  ///
  /// @param[in]  initial_covariance_factor  The initial covariances for the state. This is usually
  ///                                        a diagonal matrix with sigmas for each state dimention
  ///                                        on the diagonal.
  /// @param[in]  process_noise              A thin matrix that has as many rows as there are states
  ///                                        and as many rows as the dimentionality of our space,
  ///                                        e.g. for state of position, velocity, and acceleration
  ///                                        in 2D this will be 6x2; for state of position and
  ///                                        velocity in 1D, it will be 2x1.
  /// @param[in]  expected_dt                Expected time difference between updates of the filter.
  /// @param[in]  frame_id                   The frame id in which tracking takes place.
  /// @param[in]  history_duration           Length of history of events.
  /// @param[in]  mahalanobis_threshold      The threshold on the Mahalanobis distance for ourlier
  ///                                        rejection.
  /// @param[in]  motion_model               The motion model that is to be used. Mostly present
  ///                                        here to avoid passign the type explicitly.
  ///
  KalmanFilterWrapper(
    const SquareMatrixT<kNumOfStates> & initial_covariance_factor,
    const RectangularMatrixT<kNumOfStates, kProcessNoiseDim> & process_noise,
    const std::chrono::nanoseconds & expected_dt,
    const std::string & frame_id,
    const std::chrono::nanoseconds & history_duration = std::chrono::milliseconds{5000},
    common::types::float32_t mahalanobis_threshold =
    std::numeric_limits<common::types::float32_t>::max(),
    const MotionModelT & motion_model = MotionModelT{})
  : m_motion_model{motion_model},
    m_initial_covariance_factor{initial_covariance_factor},
    m_frame_id{frame_id},
    m_mahalanobis_threshold{mahalanobis_threshold},
    m_expected_prediction_period{expected_dt}
  {
    static_assert(
      motion_model.get_num_states() == kNumOfStates,
      "Wrong number of states in the motion model.");

    SquareMatrixT<kNumOfStates> F;
    m_motion_model.compute_jacobian(F, expected_dt);
    m_GQ_left_factor = F * process_noise;
    m_filter = std::make_unique<FilterT>(m_motion_model, m_GQ_left_factor);
    m_history = std::make_unique<HistoryT>(
      *m_filter,
      static_cast<std::size_t>(history_duration / expected_dt),
      m_mahalanobis_threshold);
  }

  ///
  /// Reset the filter state using the default covariance and state derived from the measurement.
  ///
  /// @param[in]  measurement              The measurement from which we initialize the state.
  ///
  /// @tparam     MeasurementT             Type of measurement.
  ///
  template<typename MeasurementT>
  void add_reset_event_to_history(const MeasurementT & measurement);

  ///
  /// Reset the filter state. This must be called at least once to start / tracking.
  ///
  /// @param[in]  state                    The full state to set the system to.
  /// @param[in]  initial_covariance_chol  The initial covariance cholesky factor. For a diagonal
  ///                                      matrix with squared variances on the diagonal:
  ///                                      diag([s^2]), its cholesky factor is a diagonal matrix
  ///                                      with variances on the diagonal: diag([s]).
  /// @param[in]  event_timestamp          The event timestamp. Ideally this should be in the same
  ///                                      clock as the one that timestamps the messages.
  ///
  void add_reset_event_to_history(
    const VectorT<kNumOfStates> & state,
    const SquareMatrixT<kNumOfStates> & initial_covariance_chol,
    const std::chrono::system_clock::time_point & event_timestamp);

  ///
  /// Predict state of filter at the next timestep defined by the period of this node.
  ///
  /// @return     true if the update was successful and false otherwise. In case false is returned,
  ///             this update had no effect on the state of the filter.
  ///
  common::types::bool8_t add_next_temporal_update_to_history();

  ///
  /// Update the filter state with a measurement.
  ///
  /// @param[in]  measurement            The measurement. It is expected to be a concrete
  ///                                    instantiation of the Measurement class.
  ///
  /// @tparam     MeasurementT           Measurement type that is a concrete template specialization
  ///                                    of the Measurement class.
  ///
  /// @return     true if the observation was successful, false otherwise. In case of an
  ///             unsuccessful update, the state of the underlying filter has not been changed.
  ///
  template<typename MeasurementT>
  common::types::bool8_t add_observation_to_history(const MeasurementT & measurement);

  /// Check if the filter is is_initialized with a state.
  inline common::types::bool8_t is_initialized() const noexcept
  {
    return (m_history) && (!m_history->empty()) && m_time_grid.is_initialized();
  }

  /// Get the current state of the system as an odometry message.
  nav_msgs::msg::Odometry get_state() const;

private:
  /// An implementation of the filter used internally.
  std::unique_ptr<FilterT> m_filter{};
  /// Time represented in a frame based on the last measurement timestamp.
  SteadyTimeGrid m_time_grid{};
  /// We own our motion model and store it here.
  MotionModelT m_motion_model;
  /// We own the left factor of the matrix GQ = m_GQ_left_factor * m_GQ_left_factor.T
  RectangularMatrixT<kNumOfStates, kProcessNoiseDim> m_GQ_left_factor;
  /// We own the initial state covariance Cholesky factor. In the most common case, for the diagonal
  /// covariance matrix  with squared variances on the diagonal: diag([s^2]), it's Cholesky factor
  /// will be diag([s]). Otherwise this is a lower triangular Cholesky factor of the state
  /// covariance matrix.
  SquareMatrixT<kNumOfStates> m_initial_covariance_factor;
  /// Frame in which the estimation happens, e.g. "odom".
  std::string m_frame_id{};
  /// The threshold on the Mahalanobis distance used to reject outliers.
  common::types::float32_t m_mahalanobis_threshold{};
  /// History of all events is stored here.
  std::unique_ptr<HistoryT> m_history{};
  /// What duration passes between prediction events.
  std::chrono::nanoseconds m_expected_prediction_period{};
};

using ConstantAccelerationFilter =
  KalmanFilterWrapper<motion::motion_model::ConstantAcceleration, 6, 2>;

}  // namespace prediction
}  // namespace autoware

#endif  // STATE_ESTIMATION_NODES__KALMAN_FILTER_WRAPPER_HPP_


#pragma once

#include <armadillo>

class BoundaryConditions;

/*!
 * \brief The MatrixEquation class sets up the matrix equation from the system
 * of equations found from the governing equations.
 *
 * The governing equations are discretized using a implicit backward difference
 * method, as detailed in
 *  - Gas composition tracking in transient pipeline flow, Chaczykowski et. al., Journal of Natural Gas Science and Engineering 55 (2018)
 *
 * This class supports all combinations of inlet and outlet boundary conditions,
 * so we can for example use flow at the inlet, pressure at the outlet, and
 * temperature at the inlet, or any other combination. Also supports
 * over-determined systems.
 *
 * This class is designed for use together with a Discretizer instance,
 * which sets up the individual elements of the matrix. MatrixEquation just
 * fills in the matrix using results from Discretizer, and solves the matrix
 * equation.
 *
 * The matrix equation is solved using the sparse matrix solver arma::spsolve,
 * which uses superlu internally. This solver only works for critically
 * determined systems, not for over-determined systems. In that case the regular
 * solver is used, and the sparse matrix has to be converted to a regular
 * matrix.
 *
 * Most of the elements in the matrix are zero, so we use sparse matrices. The
 * non-zero elements are all located near the diagonal.
 *
 * \see Discretizer
 */
class MatrixEquation
{
public:
    //! Declared to avoid the inline compiler-generated default destructor.
    ~MatrixEquation();

    /*!
     * \brief Fill in the coefficient matrix A (MatrixEquation::m_coefficients)
     * and constants vector b (MatrixEquation::m_constants) in the matrix
     * equation Ax = b.
     *
     * Here `term_i` and `term_ipp` refer to the coefficients of the variables
     * in the discretized governing equations (flow, pressure and temperature --
     * \f$y_i\f$ in general). See for example eq. (14)-(16) in
     * Gas composition tracking in transient pipeline flow.
     * These coefficients are calculated by the Discretizer class.
     *
     * The boundary terms are the constant terms, the elements of the vector
     * \f$b\f$ in the matrix equation \f$Ax = b\f$.
     *
     * MatrixEquation supports all combinations of inlet and outlet boundary
     * conditions, but giving more than 3 boundary conditions lead to an
     * over-determined system, and giving less than 3 leads to a
     * under-determined system.
     *
     * \see Discretizer::discretize()
     *
     * \param nGridPoints Number of grid points
     * \param nEquationsAndVariables Number of equations and variables
     * \param boundaryConditions The boundary conditions
     * \param term_i Matrix coefficients at point i (see above)
     * \param term_ipp Matrix coefficients at point i+1 (see above)
     * \param boundaryTerms The boundary terms (see above)
     */
    void fillCoefficientMatrixAndConstantsVector(
            const arma::uword nGridPoints,
            const arma::uword nEquationsAndVariables,
            const BoundaryConditions& boundaryConditions,
            const arma::cube& term_i,
            const arma::cube& term_ipp,
            const arma::mat& boundaryTerms); // fills self matrix and vector

    /*!
     * \brief Solve the matrix equation Ax = b.
     *
     * This solves the matrix equation, after the coefficient matrix
     * MatrixEquation::m_coefficients and constant vector
     * MatrixEquation::m_coefficients has been filled in by
     * fillCoefficientMatrixAndConstantsVector().
     *
     * \param nGridPoints Number of grid points
     * \param nEquationsAndVariables Number of equations and variables
     * \param boundaryConditions The boundary conditions
     * \return The solution of the matrix equation (x), reshaped to contain
     * flow, pressure and temperature in separate columns.
     */
    arma::mat solve(
            const arma::uword nGridPoints,
            const arma::uword nEquationsAndVariables,
            const BoundaryConditions& boundaryConditions) const;

    //! Get coefficient matrix A. For testing purposes.
    const arma::sp_mat& coefficients() const { return m_coefficients; }
    //! Get constants vector b. For testing purposes.
    const arma::vec& constants() const { return m_constants; }

private:
    arma::sp_mat m_coefficients; //!< Coefficient matrix A
    arma::vec m_constants; //!< Constants vector b

    /*!
     * \brief Reshape output from solving the matrix equation into a matrix
     * containing flow, pressure and temperature as columns.
     *
     * Reshapes the vector x, the solution of the matrix equation Ax = b,
     * into a matrix with three columns. Also inserts the boundary conditions
     * where at the correct locations.
     *
     * Supports all combinations of inlet/outlet boundary settings.
     *
     * \param x Solution of matrix equation
     * \param boundaryConditions Boundary conditions
     * \param nGridPoints Number of grid points
     * \param nVariables Number of variables
     * \return Matrix containing flow, pressure and temperature columns.
     */
    arma::mat reshapeSolverOutput(
            const arma::vec& x,
            const BoundaryConditions& boundaryConditions,
            const arma::uword nGridPoints,
            const arma::uword nVariables) const;

    /*!
     * \brief Internal method that solves the matrix equation.
     *
     * \see solve()
     * \return Vector x, solution of the matrix equation Ax = b.
     */
    arma::vec solveMatrixEquation() const;
};


/* ----------------------------------------------------------------------- *//**
 *
 * @file matrix_decomp.cpp
 *
 * @brief Compute decomposition of a matrix in a single node
 *
 *//* ----------------------------------------------------------------------- */

#include <dbconnector/dbconnector.hpp>
#include <modules/shared/HandleTraits.hpp>
#include <utils/Math.hpp>
#include <Eigen/Core>
#include <Eigen/LU>

#include "matrix_decomp.hpp"

namespace madlib {

// Use Eigen
using namespace dbal::eigen_integration;

namespace modules {

namespace linalg {

/**
 * @brief Transition state for building a matrix
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length 3, and all elemenets are 0. Handle::operator[] will
 * perform bounds checking.
 */
template <class Handle>
class MatrixComposeState {
    template <class OtherHandle>
    friend class MatrixComposeState;

public:
    MatrixComposeState(const AnyType &inArray)
            : mStorage(inArray.getAs<Handle>()) {
        rebind(static_cast<uint64_t>(mStorage[0]), static_cast<uint64_t>(mStorage[1]));
    }

    operator AnyType() const {
        return mStorage;
    }

    void initialize(const Allocator& inAllocator, uint64_t inNumRows, uint64_t inNumCols) {
        // Allocate the storage for the matrix
        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
            dbal::DoZero, dbal::ThrowBadAlloc>(stateSize(inNumRows, inNumCols));
        rebind(inNumRows, inNumCols);
        numRows = inNumRows;
        numCols = inNumCols;
        matrix.fill(0);
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle>
    MatrixComposeState &operator=(
        const MatrixComposeState<OtherHandle> &inOtherState) {
        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object
     */
    template <class OtherHandle>
    MatrixComposeState &operator+=(
        const MatrixComposeState<OtherHandle> &inOtherState) {

        if (mStorage.size() != inOtherState.mStorage.size() ||
            numRows != inOtherState.numRows ||
            numCols != inOtherState.numCols)
            throw std::logic_error("Internal error: Incompatible transition "
                "states");
        // we assume that the number of rows and number of cols remain the same
        // between the merged states. We only need to add the matrices together
        // since each row/element of the matrix is set only once.
        matrix += inOtherState.matrix;
        return *this;
    }

private:
    static inline size_t stateSize(uint64_t inNumRows, uint64_t inNumCols) {
        return static_cast<size_t>(2 + inNumRows * inNumCols);
    }

    /**
     * @brief Rebind to a new storage array
     *
     * @param inNumRows The number of rows
     * @param inNumCols The number of columns
     *
     * Array layout (iteration refers to one aggregate-function call):
     * Inter-iteration components (updated in final function):
     * - 0: numRows (number of rows)
     * - 1: numCols (number of columns)
     * - 2: sumOfPoints (matrix with \c numRows rows and \c numCols columns)
     */
    void rebind(uint64_t inNumRows, uint64_t inNumCols) {
        numRows.rebind(&mStorage[0]);
        numCols.rebind(&mStorage[1]);
        matrix.rebind(&mStorage[2], static_cast<Index>(inNumRows),
                      static_cast<Index>(inNumCols));

        madlib_assert(mStorage.size()
            >= stateSize(inNumRows, inNumCols),
            std::runtime_error("Out-of-bounds array access detected."));
    }

    Handle mStorage;

public:
    typename HandleTraits<Handle>::ReferenceToUInt64 numRows;
    typename HandleTraits<Handle>::ReferenceToUInt64 numCols;
    typename HandleTraits<Handle>::MatrixTransparentHandleMap matrix;
};

AnyType
matrix_compose_dense_transition::run(AnyType& args) {
    MatrixComposeState<MutableArrayHandle<double> > state = args[0];
    uint32_t numRows = args[1].getAs<uint32_t>();
    Index row_id = args[2].getAs<uint32_t>();
    MappedColumnVector curr_row = args[3].getAs<MappedColumnVector>();

    if (state.numCols == 0)
        state.initialize(*this, numRows, static_cast<uint32_t>(curr_row.size()));
    else if (curr_row.size() != state.matrix.cols() ||
             state.numRows != static_cast<uint32_t>(state.matrix.rows()) ||
             state.numCols != static_cast<uint32_t>(state.matrix.cols()))
        throw std::invalid_argument("Invalid arguments: Dimensions of vectors "
                                    "not consistent.");
    if (row_id < 0 || row_id >= numRows)
            throw std::runtime_error("Invalid row id.");
    state.matrix.row(row_id) = curr_row;
    return state;
}

AnyType
matrix_compose_sparse_transition::run(AnyType& args) {
    MatrixComposeState<MutableArrayHandle<double> > state = args[0];
    uint32_t numRows = args[1].getAs<uint32_t>();
    uint32_t numCols = args[2].getAs<uint32_t>();
    Index row_id = args[3].getAs<uint32_t>();
    Index col_id = args[4].getAs<uint32_t>();
    double element = args[5].getAs<double>();

    if (state.numCols == 0)
        state.initialize(*this, numRows, numCols);
    else if (state.numRows != static_cast<uint32_t>(state.matrix.rows()) ||
             state.numCols != static_cast<uint32_t>(state.matrix.cols()))
        throw std::invalid_argument("Invalid arguments: Dimensions of vectors "
                                    "not consistent.");
    if (row_id < 0 || row_id >= numRows)
            throw std::runtime_error("Invalid row id.");
    if (col_id < 0 || col_id >= numCols)
            throw std::runtime_error("Invalid col id.");
    state.matrix(row_id, col_id) = element;
    return state;
}

/**
 * @brief Perform the preliminary aggregation function: Merge transition states
 */
AnyType
matrix_compose_merge::run(AnyType &args) {
    if (args[0].isNull()) { return args[1]; }
    if (args[1].isNull()) { return args[0]; }
    MatrixComposeState<MutableArrayHandle<double> > stateLeft = args[0];
    MatrixComposeState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numRows == 0)
        return stateRight;
    else if (stateRight.numRows == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

AnyType
matrix_inv::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix m_inverse = state.matrix.inverse().transpose();
    return m_inverse;
}

AnyType
matrix_eigen::run(AnyType& args) {
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const VectorXcd eigenvalues = state.matrix.eigenvalues();
    return MappedVectorXcd(eigenvalues.leftCols(static_cast<Index>(1)));
}

AnyType
matrix_cholesky::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const MatrixLDLT ldlt = state.matrix.ldlt();
    if (ldlt.info() != Success) {
        throw std::invalid_argument("Invalida arguments: Cholesky decomposition of input matrix"
                " does not exist");
    }
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix m_p(PermutationMatrix(ldlt.transpositionsP()));
    const Matrix m_l = ldlt.matrixL();
    const Matrix m_d(ldlt.vectorD().asDiagonal());
    Matrix m_cholesky(state.matrix.rows(), state.matrix.cols() * 3);
    m_cholesky.block(0, 0, state.matrix.rows(), state.matrix.cols()) << m_p;
    m_cholesky.block(0, state.matrix.cols(), state.matrix.rows(), state.matrix.cols()) << m_l;
    m_cholesky.block(0, state.matrix.cols() * 2, state.matrix.rows(), state.matrix.cols()) << m_d;
    const Matrix res = m_cholesky.transpose();
    return res;
}

AnyType
matrix_qr::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const HouseholderQR qr = state.matrix.householderQr();
    const Matrix &R = qr.matrixQR().triangularView<Upper>();
    const Matrix &Q =  qr.householderQ();

    madlib_assert(Q.rows() == Q.cols() && Q.cols() == R.rows(),
        std::runtime_error("Error QR decomposition result."));
    Matrix m(Q.rows(), Q.cols() + R.cols());
    m.block(0, 0, Q.rows(), Q.cols()) << Q;
    m.block(0, Q.cols(), R.rows(), R.cols()) << R;
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix & res = m.transpose();
    return res;
}

AnyType
matrix_rank::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    return static_cast<int64_t>(state.matrix.fullPivLu().rank());
}

AnyType
matrix_lu::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const FullPivLU &lu = state.matrix.fullPivLu();

    Matrix l = Matrix::Identity(state.numRows, state.numRows);
    l.block(0, 0, lu.matrixLU().rows(), lu.matrixLU().cols()).triangularView<StrictlyLower>() = lu.matrixLU();
    const Matrix &u = lu.matrixLU().triangularView<Upper>();
    const Matrix &p(lu.permutationP());
    const Matrix &q(lu.permutationQ());
    Matrix m(static_cast<Index>(std::max(state.numRows, state.numCols)),
             static_cast<Index>(state.numRows * 2 + state.numCols * 2));
    m.block(0, 0, state.numRows, state.numRows) << p;
    m.block(0, state.numRows, state.numRows, state.numRows) << l;
    m.block(0, state.numRows * 2, state.numRows, state.numCols) << u;
    m.block(0, state.numRows * 2 + state.numCols, state.numCols, state.numCols) << q;

    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix res = m.transpose();
    return res;
}

AnyType
matrix_nuclear_norm::run(AnyType& args){
    if (args.isNull()) {
        return Null();
    }
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const JacobiSVD &svd = state.matrix.jacobiSvd(EigenvaluesOnly);
    const Matrix u = svd.matrixU();
    const Matrix v = svd.matrixV();
    double norm = 0;
    for (Index i = 0; i < svd.singularValues().rows(); ++i)
        norm += svd.singularValues()(i);
    return norm;
}

AnyType
matrix_pinv::run(AnyType& args) {
    if (args.isNull()) {
        return Null();
    }
    using namespace std;
    MatrixComposeState<ArrayHandle<double> > state = args[0];
    const JacobiSVD &svd = state.matrix.jacobiSvd(ComputeFullU|ComputeFullV);
    const Matrix u = svd.matrixU();
    const Matrix v = svd.matrixV();
    Matrix s(svd.singularValues().asDiagonal());
    double pinvtoler = 1.e-6;
    for (Index i = 0; i < s.rows(); ++i) {
        for (Index j = 0; j < s.cols(); ++j) {
            // for very small singular values we treat the inverse as zero
            if (s(i, j) > pinvtoler)
                s(i, j) = 1.0 / s(i, j);
            else s(i, j) = 0;
        }
    }
    // we apply a transpose operation at the end since Eigen matrices are interpreted
    // as column-major when returned to the database
    const Matrix &res = (v * s * u.transpose()).transpose();
    return res;
}

} // namespace linalg

} // namespace modules

} // namespace regress


#pragma once

#include <cstdlib>
#include <utility>
#include <tuple>
#include <numeric>

#include <boost/unordered_map.hpp>
#include "types.hh"

/*!
  @brief Generate a region automaton from a timed automaton
  @tparam NVar the number of variable in TA

  TA to RA adds states with BFS. Initial configuration is the initial states of RA. The RA contain only the states reachable from initial states.
 */
template <int NVar>
void ta_to_ra (const TimedAutomaton<NVar> &TA,RegionAutomaton &RA)
{
  //! number of states of RA
  State numOfStates;
  Region initialRegion;
  initialRegion.integer_parts.resize (NVar);
  fill(initialRegion.integer_parts.begin(),
       initialRegion.integer_parts.end(),
       std::make_pair(0,0));
  initialRegion.max_constraints = TA.max_constraints;

  // make initial state
  numOfStates = TA.initialStates.size();
  RA.edges.resize(numOfStates);
  RA.initialStates.resize (numOfStates);
  iota (RA.initialStates.begin(), RA.initialStates.end(),0);

  RA.abstractedStates.reserve (numOfStates);
  for (const auto &conf : TA.initialStates ) {
    RA.abstractedStates.push_back (std::make_pair(conf,initialRegion));
  }

  /*!
    @brief Current configuration of BFS

    A configuration consistes of a tuple ((conf,\alpha),conf,alpha). Remark (conf,\alpha) is not a pair but just a number representing a state of RA.
   */
  std::vector<std::tuple<RAState,TAState,Region> > nextConf;
  nextConf.resize (RA.abstractedStates.size());
  for (std::size_t i = 0; i < nextConf.size();i++) {
    nextConf[i] = std::make_tuple(i,RA.abstractedStates[i].first,RA.abstractedStates[i].second);
  }

  /*!
    @brief translater from TAState and Region to its corresponding state in RA.

    The type is like this.
    (TAState,([Int,Int],[[Int]],[Int])) -> RAState
  */
  boost::unordered_map<std::pair<TAState,std::tuple<std::vector< std::pair<int,int> >, std::list<std::list<int> >, std::vector<int> > >,RAState> regions_in_ra;
  while (!nextConf.empty ()) {
    std::vector<std::tuple<State,State,Region> > currentConf = nextConf;
    nextConf.clear();
    for (const auto &conf : currentConf) {
      Region nowRegion,nextRegion;
      bool self_successor;
      nowRegion = std::get<2>(conf);
      const bool existsNextRegion = nowRegion.nextRegion (nextRegion,self_successor);

      nextRegion = self_successor ? nowRegion : nextRegion;
      if (!existsNextRegion && !self_successor)
        break;
      do {
        nowRegion = std::move(nextRegion);
        bool tooLarge = true;
#ifdef DEBUG
        cout << "====================="<< endl;
#endif
        const Region::Interpretation val = nowRegion;
        for (const auto &edge : TA.edges.at(std::get<1>(conf))) {
#ifdef DEBUG
          cout << (int)get<1>(conf) << ":"
               << (int)edge.source << "->"
               << (int)edge.target << " "
               << (edge.guard(val) ? "true" : "false")
               << endl;
#endif
          if (std::all_of(edge.guard.begin(),edge.guard.end(),
                          [&val] (const Constraint &delta){return delta(val) == Order::EQ;})) {
            tooLarge = false;
            Region targetRegion = nowRegion;
            for (auto x : edge.resetVars) {
              targetRegion.integer_parts[x] = {0,0};
              for (auto it = targetRegion.frac_order.begin();
                   it != targetRegion.frac_order.end(); it++) {
                const auto initialNum = it->size();
                it->remove(x);
                if (initialNum != it->size()) {
                  if (it->empty() ) {
                    targetRegion.frac_order.erase(it);
                  }
                  break;
                }
              }
            }

#ifdef DEBUG
            cout << "nowRegion" << endl;
            cout << nowRegion << endl;

            cout << "targetRegion" << endl;
            cout << targetRegion << endl;
#endif
            // nextRegion state is new
            const auto targetRegionState = std::make_pair(edge.target,std::make_tuple(targetRegion.integer_parts,targetRegion.frac_order,targetRegion.max_constraints));
            const auto targetStateInRA = regions_in_ra.find(targetRegionState);
#ifdef DEBUG
            cout << (targetStateInRA == RA.abstractedStates.end()) << endl;
            cout << (int)get<0>(conf) << "-" << edge.c << ">" << static_cast<int>(targetStateInRA - RA.abstractedStates.begin()) << endl;
#endif
            // targetRegionState is already added
            if (targetStateInRA != regions_in_ra.end()) {
              RA.edges[std::get<0>(conf)].push_back ({std::get<0>(conf),targetStateInRA->second,edge.c});
            } else {
#ifdef DEBUG
              cout << "added " << (int)numOfStates << " = " << targetStateInRA - RA.abstractedStates.begin()<< endl;
#endif
              RA.edges[std::get<0>(conf)].push_back ({std::get<0>(conf),static_cast<State>(numOfStates),edge.c});
              if (binary_search (TA.acceptingStates.begin(),
                                 TA.acceptingStates.end (),edge.target)) {
                RA.acceptingStates.push_back (numOfStates);
              }
              regions_in_ra[targetRegionState] = numOfStates;
              RA.abstractedStates.push_back (std::make_pair (edge.target,targetRegion));
              nextConf.push_back (std::make_tuple (numOfStates,edge.target,targetRegion));
              numOfStates++;
              RA.edges.resize(numOfStates);
            }
          } else if (std::none_of(edge.guard.begin(), edge.guard.end(),
                                  [val](const Constraint &delta){return delta (val) == Order::GT;})) {
            tooLarge = false;
          }
        }
        if (tooLarge) break;
      } while(nowRegion.nextRegion (nextRegion,self_successor));
    }
    //    std::cout << numOfStates << std::endl;
  }

}


///////////////////////////////////////////////////////////////////////////////
// rolling_mean.hpp
// Copyright (C) 2008 Eric Niebler.
// Copyright (C) 2012 Pieter Bastiaan Ober (Integricom).
// 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_ROLLING_MEAN_HPP_EAN_26_12_2008
#define BOOST_ACCUMULATORS_STATISTICS_ROLLING_MEAN_HPP_EAN_26_12_2008

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/rolling_sum.hpp>
#include <boost/accumulators/statistics/rolling_count.hpp>

namespace boost { namespace accumulators
{
   namespace impl
   {
      ///////////////////////////////////////////////////////////////////////////////
      // lazy_rolling_mean_impl
      //    returns the mean over the rolling window and is calculated only
      //    when the result is requested
      template<typename Sample>
      struct lazy_rolling_mean_impl
         : accumulator_base
      {
         // for boost::result_of
         typedef typename numeric::functional::fdiv<Sample, std::size_t, void, void>::result_type result_type;

         lazy_rolling_mean_impl(dont_care)
         {
         }

         template<typename Args>
         result_type result(Args const &args) const
         {
            return numeric::fdiv(rolling_sum(args), rolling_count(args));
         }
      };

      ///////////////////////////////////////////////////////////////////////////////
      // immediate_rolling_mean_impl
      //     The non-lazy version computes the rolling mean recursively when a new
      //     sample is added
      template<typename Sample>
      struct immediate_rolling_mean_impl
         : accumulator_base
      {
         // for boost::result_of
         typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

         template<typename Args>
         immediate_rolling_mean_impl(Args const &args)
            : mean_(numeric::fdiv(args[sample | Sample()],numeric::one<std::size_t>::value))
         {
         }

         template<typename Args>
         void operator()(Args const &args)
         {
            if(is_rolling_window_plus1_full(args))
            {
               mean_ += numeric::fdiv(args[sample]-rolling_window_plus1(args).front(),rolling_count(args));
            }
            else
            {
               result_type prev_mean = mean_;
               mean_ += numeric::fdiv(args[sample]-prev_mean,rolling_count(args));
            }
         }

         template<typename Args>
         result_type result(Args const &) const
         {
            return mean_;
         }

      private:

         result_type mean_;
      };
   } // namespace impl

   ///////////////////////////////////////////////////////////////////////////////
   // tag::lazy_rolling_mean
   // tag::immediate_rolling_mean
   // tag::rolling_mean
   //
   namespace tag
   {
      struct lazy_rolling_mean
         : depends_on< rolling_sum, rolling_count >
      {
         /// INTERNAL ONLY
         ///
         typedef accumulators::impl::lazy_rolling_mean_impl< mpl::_1 > impl;

#ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
         /// tag::rolling_window::window_size named parameter
         static boost::parameter::keyword<tag::rolling_window_size> const window_size;
#endif
      };

      struct immediate_rolling_mean
         : depends_on< rolling_window_plus1, rolling_count>
      {
         /// INTERNAL ONLY
         ///
         typedef accumulators::impl::immediate_rolling_mean_impl< mpl::_1> impl;

#ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
         /// tag::rolling_window::window_size named parameter
         static boost::parameter::keyword<tag::rolling_window_size> const window_size;
#endif
      };

      // make immediate_rolling_mean the default implementation
      struct rolling_mean : immediate_rolling_mean {};
   } // namespace tag

   ///////////////////////////////////////////////////////////////////////////////
   // extract::lazy_rolling_mean
   // extract::immediate_rolling_mean
   // extract::rolling_mean
   //
   namespace extract
   {
      extractor<tag::lazy_rolling_mean> const lazy_rolling_mean = {};
      extractor<tag::immediate_rolling_mean> const immediate_rolling_mean = {};
      extractor<tag::rolling_mean> const rolling_mean = {};

      BOOST_ACCUMULATORS_IGNORE_GLOBAL(lazy_rolling_mean)
         BOOST_ACCUMULATORS_IGNORE_GLOBAL(immediate_rolling_mean)
         BOOST_ACCUMULATORS_IGNORE_GLOBAL(rolling_mean)
   }

   using extract::lazy_rolling_mean;
   using extract::immediate_rolling_mean;
   using extract::rolling_mean;

   // rolling_mean(lazy) -> lazy_rolling_mean
   template<>
   struct as_feature<tag::rolling_mean(lazy)>
   {
      typedef tag::lazy_rolling_mean type;
   };

   // rolling_mean(immediate) -> immediate_rolling_mean
   template<>
   struct as_feature<tag::rolling_mean(immediate)>
   {
      typedef tag::immediate_rolling_mean type;
   };

   // for the purposes of feature-based dependency resolution,
   // immediate_rolling_mean provides the same feature as rolling_mean
   template<>
   struct feature_of<tag::immediate_rolling_mean>
      : feature_of<tag::rolling_mean>
   {
   };

   // for the purposes of feature-based dependency resolution,
   // lazy_rolling_mean provides the same feature as rolling_mean
   template<>
   struct feature_of<tag::lazy_rolling_mean>
      : feature_of<tag::rolling_mean>
   {
   };
}} // namespace boost::accumulators

#endif


// Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic
//
// This source code is part of DopeVector header library
// and it is subject to Apache 2.0 License.
//
// Author: Giorgio Marcias
// email: marcias.giorgio@gmail.com
// Author: Maurizio Kovacic
// email: maurizio.kovacic@gmail.com

#ifndef EigenExpression_hpp
#define EigenExpression_hpp

#ifdef DOPE_USE_EIGEN

#include <DopeVector/internal/Expression.hpp>
#include <Eigen/Core>

namespace dope {

	namespace internal {

		template < class Derived, class Er, typename T, SizeType Dimension, typename Op >
		class EigenStaticArrayBinaryExpression : public StaticArrayExpression<EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, Op>, T, Dimension> {
			static_assert((Eigen::MatrixBase<Derived>::RowsAtCompileTime == Dimension && Eigen::MatrixBase<Derived>::ColsAtCompileTime == 1) ||
						  (Eigen::MatrixBase<Derived>::RowsAtCompileTime == 1 && Eigen::MatrixBase<Derived>::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector.");
		private:
			const Eigen::MatrixBase<Derived>                  &_el;
			const Er	                                          &_er;
			mutable std::array<std::function<T()>, Dimension>  _values;
			static const Op                                    _op;

		public:
			inline EigenStaticArrayBinaryExpression(const Eigen::MatrixBase<Derived> &el, const Er &er);

			inline T operator[](const SizeType i) const;
		};

		template < class El, typename T, SizeType Dimension, class Derived, typename Op >
		class StaticArrayBinaryEigenExpression : public StaticArrayExpression<StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, Op>, T, Dimension> {
			static_assert((Eigen::MatrixBase<Derived>::RowsAtCompileTime == Dimension && Eigen::MatrixBase<Derived>::ColsAtCompileTime == 1) ||
						  (Eigen::MatrixBase<Derived>::RowsAtCompileTime == 1 && Eigen::MatrixBase<Derived>::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector.");
		private:
			const El                                          &_el;
			const Eigen::MatrixBase<Derived>                  &_er;
			mutable std::array<std::function<T()>, Dimension>  _values;
			static const Op                                    _op;

		public:
			inline StaticArrayBinaryEigenExpression(const El &el, const Eigen::MatrixBase<Derived> &er);

			inline T operator[](const SizeType i) const;
		};

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::plus<T>> operator+ (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::minus<T>> operator- (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::multiplies<T>> operator* (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::divides<T>> operator/ (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class Derived, class Er, typename T, SizeType Dimension >
		inline EigenStaticArrayBinaryExpression<Derived, Er, T, Dimension, std::modulus<T>> operator% (const Eigen::MatrixBase<Derived> &el, const StaticArrayExpression<Er, T, Dimension> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::plus<T>> operator+ (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::minus<T>> operator- (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::multiplies<T>> operator* (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::divides<T>> operator/ (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

		template < class El, typename T, SizeType Dimension, class Derived >
		inline StaticArrayBinaryEigenExpression<El, T, Dimension, Derived, std::modulus<T>> operator% (const StaticArrayExpression<El, T, Dimension> &el, const Eigen::MatrixBase<Derived> &er);

	}

}

#include <DopeVector/internal/inlines/eigen_support/EigenExpression.inl>

#endif

#endif


#ifndef KILLGEN_FIXPOINT_ITERATOR_HPP
#define KILLGEN_FIXPOINT_ITERATOR_HPP

/**
  * Specialized fixpoint iterators and domains for kill-gen problems.
  */

#include <crab/common/stats.hpp>
#include <crab/common/debug.hpp>
#include <crab/cfg/cfg_bgl.hpp>
#include <crab/analysis/graphs/sccg.hpp>
#include <crab/analysis/graphs/topo_order.hpp>

#include <crab/domains/discrete_domains.hpp>
#include <crab/domains/patricia_trees.hpp>

#include <boost/optional.hpp>

namespace crab {

  namespace domains {

    // A wrapper for discrete_domain (i.e,. set of Element)
    template<class Element>
    class flat_killgen_domain: public ikos::writeable {

     private:

      typedef flat_killgen_domain<Element> flat_killgen_domain_t;
      typedef ikos::discrete_domain<Element> discrete_domain_t;

     public:

      typedef typename discrete_domain_t::iterator iterator;
      typedef Element element_t;

     private:

      discrete_domain_t _inv;

     public:

      flat_killgen_domain(discrete_domain_t inv)
          : ikos::writeable(), _inv(inv){ }

      static flat_killgen_domain_t top() {
        return flat_killgen_domain(discrete_domain_t::top());
      }

      static flat_killgen_domain_t bottom() {
        return flat_killgen_domain(discrete_domain_t::bottom());
      }

      flat_killgen_domain()
          : ikos::writeable(), _inv(discrete_domain_t::bottom()){ }

      flat_killgen_domain(Element e)
          : ikos::writeable(), _inv(e) { }

      flat_killgen_domain(const flat_killgen_domain_t &o)
          : ikos::writeable(), _inv(o._inv) { }

      flat_killgen_domain(flat_killgen_domain_t &&o)
          : ikos::writeable(), _inv(std::move(o._inv)) { }

      flat_killgen_domain_t& operator=(const flat_killgen_domain_t &other) {
        if (this != &other)
          _inv = other._inv;
        return *this;
      }

      flat_killgen_domain_t& operator=(flat_killgen_domain_t &&other) {
        _inv = std::move(other._inv);
        return *this;
      }

      iterator begin() { return _inv.begin(); }

      iterator end() { return _inv.end(); }

      unsigned size() { return _inv.size(); }

      bool is_bottom() { return _inv.is_bottom(); }

      bool is_top() { return _inv.is_top(); }

      bool operator==(flat_killgen_domain_t other) {
	return *this <= other && other <= *this;
      }

      bool operator<=(flat_killgen_domain_t other) {
        if (is_bottom ())
          return true;
        else if (other.is_top ())
          return true;
        else
          return (_inv <= other._inv);
      }

      void operator-=(Element x) {
        if (is_bottom ())
          return;
        _inv -= x;
      }

      void operator-=(flat_killgen_domain_t other) {
        if (is_bottom () || other.is_bottom ())
          return;

        if (!other._inv.is_top()) {
          for (auto v : other)
            _inv -= v;
        }
      }

      void operator+=(Element x) {
        if (is_top ())
          return;
        _inv += x;
      }

      void operator+=(flat_killgen_domain_t other) {
        if (is_top () || other.is_bottom ()) {
          return;
        } else if (other.is_top ()) {
          _inv = discrete_domain_t::top();
        } else {
          _inv = (_inv | other._inv);
        }
      }

      flat_killgen_domain_t operator|(flat_killgen_domain_t other) {
        return (_inv | other._inv);
      }

      flat_killgen_domain_t operator&(flat_killgen_domain_t other) {
        return (_inv & other._inv);
      }

      void write(crab_os& o) { _inv.write(o); }

    };

    // To represent sets of pairs (Key,Value).
    // Bottom means empty set rather than failure.
    template <typename Key, typename Value>
    class separate_killgen_domain: public ikos::writeable {

    private:
      typedef ikos::patricia_tree<Key,Value> patricia_tree_t;
      typedef typename patricia_tree_t::unary_op_t unary_op_t;
      typedef typename patricia_tree_t::binary_op_t binary_op_t;
      typedef typename patricia_tree_t::partial_order_t partial_order_t;

    public:
      typedef separate_killgen_domain<Key,Value > separate_killgen_domain_t;
      typedef typename patricia_tree_t::iterator iterator;
      typedef Key key_type;
      typedef Value value_type;

    private:
      bool _is_top;
      patricia_tree_t _tree;

    public:
      class bottom_found { };

      class join_op: public binary_op_t {
	boost::optional< Value > apply(Value x, Value y) {
	  Value z = x.operator|(y);
	  if (z.is_top()) {
	    return boost::optional<Value>();
	  } else {
	    return boost::optional<Value>(z);
	  }
	}
	bool default_is_absorbing() { return false; }
      }; // class join_op

      class meet_op: public binary_op_t {
	boost::optional< Value > apply(Value x, Value y) {
	  Value z = x.operator&(y);
	  if (z.is_bottom()) {
	    throw bottom_found();
	  } else {
	    return boost::optional<Value>(z);
	  }
	};
	bool default_is_absorbing() { return true; }
      }; // class meet_op

      class domain_po: public partial_order_t {
	bool leq(Value x, Value y) { return x.operator<=(y); }
	bool default_is_top() { return false; }
      }; // class domain_po

   public:

      static separate_killgen_domain_t top() {
	return separate_killgen_domain_t (true);
      }

      static separate_killgen_domain_t bottom() {
	return separate_killgen_domain_t (false);
      }

   private:

      static patricia_tree_t apply_operation(binary_op_t& o,
					     patricia_tree_t t1,
					     patricia_tree_t t2) {
	t1.merge_with(t2, o);
	return t1;
      }

      separate_killgen_domain(patricia_tree_t t)
	: _is_top(false), _tree(t) { }

      separate_killgen_domain(bool b)
	: _is_top(b) { }

    public:

      separate_killgen_domain()
	: _is_top(false), _tree (patricia_tree_t()) { }

      separate_killgen_domain(const separate_killgen_domain_t& o)
	: _is_top(o._is_top), _tree(o._tree) { }

      separate_killgen_domain_t& operator=(separate_killgen_domain_t o) {
	this->_is_top = o._is_top;
	this->_tree = o._tree;
	return *this;
      }

      iterator begin() const {
	if (this->is_top()) {
	  CRAB_ERROR("Separate killgen domain: trying to invoke iterator on top");
	} else {
	  return this->_tree.begin();
	}
      }

      iterator end() const {
	if (this->is_top()) {
	  CRAB_ERROR("Separate killgen domain: trying to invoke iterator on top");
	} else {
	  return this->_tree.end();
	}
      }

      bool is_top() const {
	return _is_top;
      }

      bool is_bottom() const {
	return (!is_top () && _tree.empty ());
      }

      bool operator<=(separate_killgen_domain_t o) {
	domain_po po;
	return (o.is_top() || (!is_top() && (_tree.leq (o._tree, po))));
      }

      separate_killgen_domain_t operator|(separate_killgen_domain_t o) {
	if (is_top() || o.is_top ()) {
	  return separate_killgen_domain_t::top();
	} else {
	  join_op op;
	  return separate_killgen_domain_t(apply_operation(op, _tree, o._tree));
	}
      }

      separate_killgen_domain_t operator&(separate_killgen_domain_t o) {
	if (is_top ()) {
	  return o;
	} else if (o.is_top()) {
	  return *this;
	} else {
	  try {
	    meet_op op;
	    return separate_killgen_domain_t(apply_operation(op, _tree, o._tree));
	  }
	  catch (bottom_found& exc) {
	    return separate_killgen_domain_t::bottom ();
	  }
	}
      }

      void set(Key k, Value v) {
	if (!is_top ()) {
	  // if (v.is_bottom()) {
	  //   this->_tree.remove(k);
	  // } else {
	  //   this->_tree.insert(k, v);
	  // }
	  this->_tree.insert(k, v);
	}
      }

      separate_killgen_domain_t& operator-=(Key k) {
	if (!is_top ()) {
	  _tree.remove(k);
	}
	return *this;
      }

      Value operator[](Key k) {
	if (is_top ())
	  return Value::top ();
	else {
	  boost::optional< Value > v = _tree.lookup(k);
	  if (v) {
	    return *v;
	  } else {
	    return Value::bottom();
	  }
	}
      }

      void write(crab::crab_os& o) {
	if (this->is_top()) {
	  o << "{...}";
	} if (_tree.empty ()) {
	  o << "_|_";
	}
	else {
	  o << "{";
	  for (typename patricia_tree_t::iterator it = this->_tree.begin();
	       it != this->_tree.end(); ) {
	    Key k = it->first;
	    k.write(o);
	    o << " -> ";
          Value v = it->second;
          v.write(o);
          ++it;
          if (it != this->_tree.end()) {
            o << "; ";
	  }
	  }
	  o << "}";
	}
      }
    }; // class separate_killgen_domain

  } // end namespace domains

  namespace iterators {

    // API for a kill-gen analysis operations
    template<class CFG, class Dom>
    class killgen_operations_api {

     public:

      typedef typename CFG::basic_block_label_t basic_block_label_t;
      typedef Dom killgen_domain_t;

     protected:

      CFG _cfg;

     public:

      killgen_operations_api (CFG cfg): _cfg(cfg) { }

      virtual ~killgen_operations_api() { }

      // whether forward or backward analysis
      virtual bool is_forward () = 0;

      // initial state
      virtual Dom entry() = 0;

      // (optional) initialization for the fixpoint
      virtual void init_fixpoint () = 0;

      // confluence operator
      virtual Dom merge(Dom, Dom) = 0;

      // analyze a basic block
      virtual Dom analyze (basic_block_label_t, Dom) = 0;

      // analysis name
      virtual std::string name () = 0;
    };

    // A simple fixpoint for a killgen analysis
    template<class CFG, class KgAnalysisOps>
    class killgen_fixpoint_iterator {

     public:

      typedef typename CFG::basic_block_label_t basic_block_label_t;
      typedef typename KgAnalysisOps::killgen_domain_t killgen_domain_t;
      typedef boost::unordered_map<basic_block_label_t,killgen_domain_t> inv_map_t;
      typedef typename inv_map_t::iterator iterator;
      typedef typename inv_map_t::const_iterator const_iterator;

     protected:

      CFG _cfg;
      inv_map_t _in_map;
      inv_map_t _out_map;

     private:

      KgAnalysisOps _analysis;

      /// XXX: run_bwd_fixpo(G) is equivalent to run_fwd_fixpo(reverse(G)).
      ///      However, weak_rev_topo_sort(G) != weak_topo_sort(reverse(G))
      /// For instance, for a G=(V,E) where
      ///   V= {v1,v2, v3, v4, v5},
      ///   E= {(v1,v2), (v1,v3), (v2,v4), (v4,v1), (v3,v5)}
      /// (1) weak_rev_topo_sort(cfg)=[v5,v3,v4,v2,v1]
      /// (2) weak_topo_sort(reverse(cfg))=[v5,v3,v2,v4,v1] or even
      ///     worse [v5,v3,v1,v4,v2] if vertices in the same scc are
      ///     traversed in preorder.
      /// For a backward analysis, (1) will converge faster.
      /// For all of this, we decide not to reverse graphs and have
      /// two dual versions for the forward and backward analyses.

      void run_fwd_fixpo (std::vector<typename CFG::node_t> &order,
                          unsigned &iterations){

        order = crab::analyzer::graph_algo::weak_topo_sort(_cfg);
        assert ((int)order.size () == std::distance(_cfg.begin(), _cfg.end()));
        bool change = true;
        iterations = 0;
        while (change) {
          change = false;
          ++iterations;
          for (auto &n: order) {
            auto in = _analysis.entry();
            for (auto p: _cfg.prev_nodes (n))
              in = _analysis.merge(in, _out_map[p]);
            auto old_out = _out_map[n];
            auto out = _analysis.analyze(n, in);
            if (!(out <= old_out)) {
              _out_map[n] = _analysis.merge(out, old_out);
              change = true;
            } else
              _in_map[n] = in;
          }
        }
      }

      void run_bwd_fixpo (std::vector<typename CFG::node_t> &order,
                          unsigned &iterations){

        order = crab::analyzer::graph_algo::weak_rev_topo_sort(_cfg);
        assert ((int)order.size () == std::distance(_cfg.begin(), _cfg.end()));
        bool change = true;
        iterations = 0;
        while (change) {
          change = false;
          ++iterations;
          for (auto &n: order) {
            auto out = _analysis.entry();
            for (auto p: _cfg.next_nodes (n))
              out = _analysis.merge(out, _in_map[p]);
            auto old_in = _in_map[n];
            auto in = _analysis.analyze(n, out);
            if (!(in <= old_in)) {
              _in_map[n] = _analysis.merge(in, old_in);
              change = true;
            } else
              _out_map[n] = out;
          }
        }
      }

     public:

      killgen_fixpoint_iterator (CFG cfg)
	: _cfg (cfg), _analysis (_cfg) { }

      void release_memory () {
        _in_map.clear();
        _out_map.clear();
      }

      void run() {
        crab::ScopedCrabStats __st__(_analysis.name());

        _analysis.init_fixpoint();

        std::vector<typename CFG::node_t> order;
        unsigned iterations = 0;
        if (_analysis.is_forward())
          run_fwd_fixpo(order, iterations);
        else
          run_bwd_fixpo(order, iterations);

        CRAB_LOG(_analysis.name(),
                 crab::outs()  << "fixpoint ordering={";
                 bool first=true;
                 for (auto &v : order) {
                   if (!first) crab::outs() << ",";
                   first=false;
                   crab::outs() << cfg_impl::get_label_str(v);
                 }
                 crab::outs() << "}\n";);

        CRAB_LOG(_analysis.name(),
                 crab::outs() << _analysis.name() << ": "
                              << "fixpoint reached in " << iterations << " iterations.\n");

        CRAB_LOG(_analysis.name(),
                 crab::outs() << _analysis.name() << " sets:\n";
                 for (auto n: boost::make_iterator_range (_cfg.label_begin (),
                                                          _cfg.label_end ())) {
                   crab::outs() << cfg_impl::get_label_str(n) << " "
                                << "IN="  << _in_map[n]  << " "
                                << "OUT=" << _out_map[n] << "\n";
                 }
                 crab::outs() << "\n";);
      }

      iterator in_begin() { return _in_map.begin(); }
      iterator in_end() { return _in_map.end(); }
      const_iterator in_begin() const { return _in_map.begin(); }
      const_iterator in_end() const { return _in_map.end(); }

      iterator out_begin() { return _out_map.begin(); }
      iterator out_end() { return _out_map.end(); }
      const_iterator out_begin() const { return _out_map.begin(); }
      const_iterator out_end() const { return _out_map.end(); }

   };

  } // end namespace iterators
} // end namespace crab

#endif


#include "wire_mesh.h"

#include "../../list_to_matrix.h"
#include "../../slice.h"
#include "../../PI.h"
#include "convex_hull.h"
#include "mesh_boolean.h"
#include <Eigen/Geometry>
#include <vector>

template <
  typename DerivedWV,
  typename DerivedWE,
  typename DerivedV,
  typename DerivedF,
  typename DerivedJ>
IGL_INLINE void igl::copyleft::cgal::wire_mesh(
  const Eigen::MatrixBase<DerivedWV> & WV,
  const Eigen::MatrixBase<DerivedWE> & WE,
  const double th,
  const int poly_size,
  const bool solid,
  Eigen::PlainObjectBase<DerivedV> & V,
  Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedJ> & J)
{

  typedef typename DerivedWV::Scalar Scalar;
  // Canonical polygon to place at each endpoint
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,3> MatrixX3S;
  MatrixX3S PV(poly_size,3);
  for(int p =0;p<PV.rows();p++)
  {
    const Scalar phi = (Scalar(p)/Scalar(PV.rows()))*2.*igl::PI;
    PV(p,0) = 0.5*cos(phi);
    PV(p,1) = 0.5*sin(phi);
    PV(p,2) = 0;
  }

  V.resize(WV.rows() + PV.rows() * 2 * WE.rows(),3);
  V.topLeftCorner(WV.rows(),3) = WV;
  // Signed adjacency list
  std::vector<std::vector<std::pair<int,int> > > A(WV.rows());
  // Inputs:
  //   e  index of edge
  //   c  index of endpoint [0,1]
  //   p  index of polygon vertex
  // Returns index of corresponding vertex in V
  const auto index =
    [&PV,&WV](const int e, const int c, const int p)->int
  {
    return WV.rows() + e*2*PV.rows() + PV.rows()*c + p;
  };
  const auto unindex =
    [&PV,&WV](int v, int & e, int & c, int & p)
  {
    assert(v>=WV.rows());
    v = v-WV.rows();
    e = v/(2*PV.rows());
    v = v-e*(2*PV.rows());
    c = v/(PV.rows());
    v = v-c*(PV.rows());
    p = v;
  };
  // loop over all edges
  for(int e = 0;e<WE.rows();e++)
  {
    // Fill in adjacency list as we go
    A[WE(e,0)].emplace_back(e,0);
    A[WE(e,1)].emplace_back(e,1);
    typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
    const RowVector3S ev = WV.row(WE(e,1))-WV.row(WE(e,0));
    const Scalar len = ev.norm();
    // Unit edge vector
    const RowVector3S uv = ev.normalized();
    Eigen::Quaternion<Scalar> q;
    q = q.FromTwoVectors(RowVector3S(0,0,1),uv);
    // loop over polygon vertices
    for(int p = 0;p<PV.rows();p++)
    {
      RowVector3S qp = q*(PV.row(p)*th);
      // loop over endpoints
      for(int c = 0;c<2;c++)
      {
        // Direction moving along edge vector
        const Scalar dir = c==0?1:-1;
        // Amount (distance) to move along edge vector
        // Start with factor of thickness;
        // Max out amount at 1/3 of edge length so that there's always some
        // amount of edge
        Scalar dist = std::min(1.*th,len/3.0);
        // Move to endpoint, offset by amount
        V.row(index(e,c,p)) =
          qp+WV.row(WE(e,c)) + dist*dir*uv;
      }
    }
  }

  std::vector<std::vector<typename DerivedF::Index> > vF;
  std::vector<int> vJ;
  const auto append_hull =
    [&V,&vF,&vJ,&unindex,&WV](const Eigen::VectorXi & I, const int j)
  {
    MatrixX3S Vv;
    igl::slice(V,I,1,Vv);
    Eigen::MatrixXi Fv;
    convex_hull(Vv,Fv);
    for(int f = 0;f<Fv.rows();f++)
    {
      const Eigen::Array<int,1,3> face(I(Fv(f,0)), I(Fv(f,1)), I(Fv(f,2)));
      //const bool on_vertex = (face<WV.rows()).any();
      //if(!on_vertex)
      //{
      //  // This correctly prunes fcaes on the "caps" of convex hulls around
      //  // edges, but for convex hulls around vertices this will only work if
      //  // the incoming edges are not overlapping.
      //  //
      //  // Q: For convex hulls around vertices, is the correct thing to do:
      //  // check if all corners of face lie *on or _outside_* of plane of "cap"?
      //  //
      //  // H: Maybe, but if there's an intersection then the boundary of the
      //  // incoming convex hulls around edges is still not going to match up
      //  // with the boundary on the convex hull around the vertices.
      //  //
      //  // Might have to bite the bullet and always call self-union.
      //  bool all_same = true;
      //  int e0,c0,p0;
      //  unindex(face(0),e0,c0,p0);
      //  for(int i = 1;i<3;i++)
      //  {
      //    int ei,ci,pi;
      //    unindex(face(i),ei,ci,pi);
      //    all_same = all_same && (e0==ei && c0==ci);
      //  }
      //  if(all_same)
      //  {
      //    // don't add this face
      //    continue;
      //  }
      //}
      vF.push_back( { face(0),face(1),face(2)});
      vJ.push_back(j);
    }
  };
  // loop over each vertex
  for(int v = 0;v<WV.rows();v++)
  {
    // Gather together this vertex and the polygon vertices of all incident
    // edges
    Eigen::VectorXi I(1+A[v].size()*PV.rows());
    // This vertex
    I(0) = v;
    for(int n = 0;n<A[v].size();n++)
    {
      for(int p = 0;p<PV.rows();p++)
      {
        const int e = A[v][n].first;
        const int c = A[v][n].second;
        I(1+n*PV.rows()+p) = index(e,c,p);
      }
    }
    append_hull(I,v);
  }
  // loop over each edge
  for(int e = 0;e<WE.rows();e++)
  {
    // Gether together polygon vertices of both endpoints
    Eigen::VectorXi I(PV.rows()*2);
    for(int c = 0;c<2;c++)
    {
      for(int p = 0;p<PV.rows();p++)
      {
        I(c*PV.rows()+p) = index(e,c,p);
      }
    }
    append_hull(I,WV.rows()+e);
  }

  list_to_matrix(vF,F);
  if(solid)
  {
    // Self-union to clean up
    igl::copyleft::cgal::mesh_boolean(
      Eigen::MatrixXd(V),Eigen::MatrixXi(F),Eigen::MatrixXd(),Eigen::MatrixXi(),
      "union",
      V,F,J);
    for(int j=0;j<J.size();j++) J(j) = vJ[J(j)];
  }else
  {
    list_to_matrix(vJ,J);
  }
}

template <
  typename DerivedWV,
  typename DerivedWE,
  typename DerivedV,
  typename DerivedF,
  typename DerivedJ>
IGL_INLINE void igl::copyleft::cgal::wire_mesh(
  const Eigen::MatrixBase<DerivedWV> & WV,
  const Eigen::MatrixBase<DerivedWE> & WE,
  const double th,
  const int poly_size,
  Eigen::PlainObjectBase<DerivedV> & V,
  Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedJ> & J)
{
  return wire_mesh(WV,WE,th,poly_size,true,V,F,J);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::copyleft::cgal::wire_mesh<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, double, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
#endif


/*Copyright (c) 2021 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/*
 * molcas_interface.cpp
 */

#include "molcas_interface.h"

#include <h5pp/h5pp.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <unsupported/Eigen/CXX11/Tensor>

#include "BasisSet.h"
#include "gto_ordering.h"
#include "opencap_exception.h"
#include "utils.h"

template<typename T>
using  MatrixType = Eigen::Matrix<T,Eigen::Dynamic, Eigen::Dynamic>;

template<typename Scalar,int rank, typename sizeType>
auto Tensor_to_Matrix(const Eigen::Tensor<Scalar,rank> &tensor,const sizeType rows,const sizeType cols)
{
    return Eigen::Map<const MatrixType<Scalar>> (tensor.data(), rows,cols);
}

Eigen::Map<const Eigen::MatrixXd> reshape (const Eigen::VectorXd& b, const uint n, const uint m) {
    return Eigen::Map<const Eigen::MatrixXd>(b.data(), n, m);
}

void read_rassi_tdms(std::vector<std::vector<Eigen::MatrixXd>> &alpha_opdms,
		std::vector<std::vector<Eigen::MatrixXd>> &beta_opdms,
		std::string filename, BasisSet bs,size_t nstates)
{
    alpha_opdms = std::vector< std::vector<Eigen::MatrixXd>>(nstates, std::vector<Eigen::MatrixXd> (nstates));
    beta_opdms = std::vector< std::vector<Eigen::MatrixXd>>(nstates, std::vector<Eigen::MatrixXd> (nstates));
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	//first lets check if the dimensions are correct
	std::vector<long> nbas_vec = file.readAttribute<std::vector<long>>("NBAS","/");
	long nbas=0;
	for(auto num_bas:nbas_vec)
		nbas+=num_bas;
	if(nbas!=bs.Nbasis)
		opencap_throw("Error: dimensions of RASSI basis set does not match specified basis set.");
	auto nsym = file.readAttribute<long>("NSYM", "/");
	//now lets load the densities
	Eigen::Tensor<double,3> rass_data, spin_dens;
	try
	{
		file.readDataset(rass_data,"SFS_TRANSITION_DENSITIES");
		file.readDataset(spin_dens,"SFS_TRANSITION_SPIN_DENSITIES");
	}
	catch(exception &e)
	{
		opencap_throw("Error: SFS_TRANSITION_DENSITIES dataset not found. Use the TRD1 keyword"
				" in the RASSI module to activate transition densities.")
	}
    const auto& d = rass_data.dimensions();

    if (nsym>1)
    {
    	//Step 1: get the desymmetrization matrix
		Eigen::VectorXd desym_vec;
		file.readDataset(desym_vec,"DESYM_MATRIX");
		Eigen::MatrixXd desym_mat;
		std::vector<long> nbas_vec = file.readAttribute<std::vector<long>>("NBAS","/");
		desym_mat = reshape(desym_vec,nbas,nbas);
		long mat_size=0;
		for(auto num_bas:nbas_vec)
			mat_size+=num_bas*num_bas;
		//now lets loop over the matrices
		for(long i=0;i<d[0];i++)
		{
            if(d[0]!=nstates)
                opencap_throw("Error: Found " + std::to_string(d[0]) + " states in RASSI file, but "
                          + std::to_string(nstates) + " states were specified.");
			std::vector<Eigen::MatrixXd> alpha_state_row;
			std::vector<Eigen::MatrixXd> beta_state_row;
			for (long j=0;j<d[1];j++)
			{
				//step 2: get the raw matrices
				Eigen::array<long,3> offset = {i,j,0};    //Starting point
				Eigen::array<long,3> extent = {1,1,1}; //end point
				Eigen::Tensor<double, 2> dmt_slice = rass_data.slice(offset, extent).reshape(Eigen::array<long,2>{mat_size,(long)1});
				Eigen::MatrixXd  dmt_vec =  Tensor_to_Matrix(dmt_slice,mat_size,(long)1);
				Eigen::Tensor<double, 2> spin_slice = spin_dens.slice(offset, extent).reshape(Eigen::array<long,2>{mat_size,(long)1});
				Eigen::MatrixXd  spin_vec =  Tensor_to_Matrix(dmt_slice,mat_size,(long)1);

				//step 3: turn into bigger matrix by adding zeros
				Eigen::MatrixXd dmat,spin;
				dmat = Eigen::MatrixXd::Zero(nbas,nbas);
				spin = Eigen::MatrixXd::Zero(nbas,nbas);
				//fill the blocks
				size_t elements_index = 0;
				size_t bf_index = 0;
				for(size_t isym=0;isym<nsym;isym++)
				{
					size_t n_elements = nbas_vec[isym] * nbas_vec[isym];
					Eigen::VectorXd dmat_block_vec(n_elements),spin_block_vec(n_elements);
					Eigen::MatrixXd dmat_block,spin_block;
					//grab the elements we need
					for(size_t k=0;k<n_elements;k++)
					{
						dmat_block_vec(k) = dmt_vec(elements_index+k);
						spin_block_vec(k) = spin_vec(elements_index+k);
					}
					dmat_block = reshape(dmat_block_vec,nbas_vec[isym],nbas_vec[isym]);
					spin_block = reshape(spin_block_vec,nbas_vec[isym],nbas_vec[isym]);
					dmat.block(bf_index,bf_index,nbas_vec[isym],nbas_vec[isym]) = dmat_block;
					spin.block(bf_index,bf_index,nbas_vec[isym],nbas_vec[isym]) = spin_block;
					//update our index counters
					elements_index+=n_elements;
					bf_index+= nbas_vec[isym];
				}

				//step 4: desymmetrize
				Eigen::MatrixXd desym_dmat = desym_mat * dmat * desym_mat.transpose();
				Eigen::MatrixXd desym_spin = desym_mat * spin * desym_mat.transpose();

				//step 5: save
				Eigen::MatrixXd alpha_opdm,beta_opdm;
				alpha_opdm= 0.5*(desym_dmat+desym_spin);
				beta_opdm = 0.5*(desym_dmat-desym_spin);
				to_opencap_ordering(alpha_opdm,bs,get_molcas_ids(bs,filename));
				to_opencap_ordering(beta_opdm,bs,get_molcas_ids(bs,filename));
                alpha_opdms[i][j] = alpha_opdm;
                beta_opdms[i][j] = beta_opdm;
			}
		}

    }
    else
    {
		//now lets loop over the matrices
		for(long i=0;i<d[0];i++)
		{
            if(d[0]!=nstates)
            opencap_throw("Error: Found " + std::to_string(d[0]) + " states in RASSI file, but "
                          + std::to_string(nstates) + " states were specified.");
			for (long j=0;j<d[1];j++)
			{
				Eigen::array<long,3> offset = {i,j,0};    //Starting point
				Eigen::array<long,3> extent = {1,1,1}; //end point
				Eigen::Tensor<double, 2> dmt_slice = rass_data.slice(offset, extent).reshape(Eigen::array<long,2>{nbas,nbas});
				Eigen::MatrixXd  dmt_mat =  Tensor_to_Matrix(dmt_slice,nbas,nbas);
				Eigen::Tensor<double, 2> spin_slice = spin_dens.slice(offset, extent).reshape(Eigen::array<long,2>{nbas,nbas});
				Eigen::MatrixXd  spin_mat =  Tensor_to_Matrix(dmt_slice,nbas,nbas);
				Eigen::MatrixXd alpha_opdm,beta_opdm;
				alpha_opdm= 0.5*(dmt_mat+spin_mat);
				beta_opdm = 0.5*(dmt_mat-spin_mat);
				to_opencap_ordering(alpha_opdm,bs,get_molcas_ids(bs,filename));
				to_opencap_ordering(beta_opdm,bs,get_molcas_ids(bs,filename));
                alpha_opdms[i][j] = alpha_opdm;
                beta_opdms[i][j] = beta_opdm;
			}
		}
    }

    std::cout << "Warning: TDM M-->N is assumed to be conjugate transpose of "
    << "TDM N-->M where M>N" << std::endl;
    for (size_t i=0;i<nstates;i++)
    {
    	for(size_t j=i+1;j<nstates;j++)
    	{
    		alpha_opdms[j][i]= alpha_opdms[i][j].adjoint();
    		beta_opdms[j][i]= beta_opdms[i][j].adjoint();
    	}
    }
}

Eigen::MatrixXd read_rassi_overlap(std::string filename,BasisSet bs)
{
	Eigen::VectorXd overlap_vec;
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	file.readDataset(overlap_vec,"AO_OVERLAP_MATRIX");
	auto nsym = file.readAttribute<long>("NSYM", "/");
	if(nsym >1)
	{
		Eigen::VectorXd desym_vec;
		file.readDataset(desym_vec,"DESYM_MATRIX");
		Eigen::MatrixXd desym_mat;
		std::vector<long> nbas_vec = file.readAttribute<std::vector<long>>("NBAS","/");
		long nbas=0;
		for(auto num_bas:nbas_vec)
			nbas+=num_bas;
		desym_mat = reshape(desym_vec,nbas,nbas);
		Eigen::MatrixXd smat;
		smat = Eigen::MatrixXd::Zero(nbas,nbas);
		//fill the blocks
		size_t elements_index = 0;
		size_t bf_index = 0;
		for(size_t isym=0;isym<nsym;isym++)
		{
			size_t n_elements = nbas_vec[isym] * nbas_vec[isym];
			Eigen::VectorXd block_vec(n_elements);
			Eigen::MatrixXd block_mat;
			//grab the elements we need
			for(size_t i=0;i<n_elements;i++)
				block_vec(i) = overlap_vec(elements_index+i);
			block_mat = reshape(block_vec,nbas_vec[isym],nbas_vec[isym]);
			smat.block(bf_index,bf_index,nbas_vec[isym],nbas_vec[isym]) = block_mat;
			//update our index counters
			elements_index+=n_elements;
			bf_index+= nbas_vec[isym];
		}
		Eigen::MatrixXd desym_overlap = desym_mat * smat * desym_mat.transpose();
		to_opencap_ordering(desym_overlap,bs,get_molcas_ids(bs,filename));
		return desym_overlap;
	}
	else
	{
		int nbas = sqrt(overlap_vec.size());
		Eigen::MatrixXd smat;
		smat = reshape(overlap_vec,nbas,nbas);
		to_opencap_ordering(smat,bs,get_molcas_ids(bs,filename));
		return smat;
	}
}

Eigen::MatrixXd read_rotation_matrix(size_t nstates, std::ifstream &is)
{
	Eigen::MatrixXd rotation_matrix(nstates,nstates);
	rotation_matrix = Eigen::MatrixXd::Zero(nstates,nstates);
	std::string line, rest;
	size_t num_groups = nstates%5==0 ? nstates/5 : nstates/5+1;
	for (size_t i=1;i<=num_groups;i++)
	{
		std::getline(is,line);
        if(i>1)
            std::getline(is,line);
		for (size_t j=1;j<=nstates;j++)
		{
			std::getline(is,line);
			std::vector<std::string> tokens = split(line,' ');
			size_t row_idx = std::stoul(tokens[0]);
			for(size_t k=1;k<tokens.size();k++)
			{
				size_t col_idx = k-1+(i-1)*5;
				if(col_idx>=rotation_matrix.cols() || row_idx-1 >= rotation_matrix.rows())
					opencap_throw("Error: State index of out bounds. There is a problem with the OpenMolcas output file. Exiting...");
				rotation_matrix(row_idx-1,col_idx)=std::stod(tokens[k]);
			}
		}
	}
	return rotation_matrix;
}

Eigen::MatrixXd read_mscaspt2_heff(size_t nstates, std::string filename, Eigen::MatrixXd &rotation_matrix)
{
	Eigen::MatrixXd ZERO_ORDER_H(nstates,nstates);
	ZERO_ORDER_H= Eigen::MatrixXd::Zero(nstates,nstates);
	std::ifstream is(filename);
	if (is.good())
	{
		std::string line, rest;
		std::getline(is,line);
		while (line.find("Number of CI roots used")== std::string::npos && is.peek()!=EOF)
			std::getline(is,line);
		size_t num_states = stoi(split(line,' ').back());
		if (num_states!=nstates)
			opencap_throw("Error: "+std::to_string(num_states)+ " roots were found in the OpenMolcas "
					"output file, but " + std::to_string(nstates) +" states were specified in the input. "
							"Exiting...");
		while (line.find("MULTI-STATE CASPT2 SECTION")== std::string::npos && is.peek()!=EOF)
		{
			std::getline(is,line);
			if (line.find("H0 eigenvectors:")!= std::string::npos)
				rotation_matrix = read_rotation_matrix(nstates,is);
		}
		if (is.peek()==EOF)
			opencap_throw("Error: Reached end of file before MULTI-STATE CASPT2 SECTION.");
		//get diagonal shift
		for (size_t i=1;i<=3;i++)
			std::getline(is,line);
        double E_shift = 0.0;
        if(line.find("Output diagonal")!=std::string::npos)
        {
            std::vector<std::string> split_line = split(line,' ');
            E_shift = std::stod(split_line[split_line.size()-1]);
            for(size_t i=1;i<=2;i++)
                std::getline(is,line);
        }
		size_t num_groups = nstates%5==0 ? nstates/5 : nstates/5+1;
		for (size_t i=1;i<=num_groups;i++)
		{
			for (size_t j=1;j<=2;j++)
				std::getline(is,line);
			//now time to start reading in the matrix elements
			size_t states_in_group = nstates - (i-1)*5;
			for (size_t j=1;j<=states_in_group;j++)
			{
				std::getline(is,line);
				std::vector<std::string> tokens = split(line,' ');
				size_t row_idx = std::stoul(tokens[0]);
				for(size_t k=1;k<tokens.size();k++)
				{
					size_t col_idx = k-1+(i-1)*5;
					if(col_idx>=ZERO_ORDER_H.cols() || row_idx-1 >= ZERO_ORDER_H.rows())
						opencap_throw("Error: State index of out bounds. There is a problem with the OpenMolcas output file. Exiting...");
					ZERO_ORDER_H(row_idx-1,col_idx)=std::stod(tokens[k]);
					ZERO_ORDER_H(col_idx,row_idx-1)=std::stod(tokens[k]);
				}
			}
		}
		for (size_t i=0;i<nstates;i++)
			ZERO_ORDER_H(i,i)+=E_shift;
	}
	else
	{
    	opencap_throw("Error: I couldn't read:" + filename);
	}
	return ZERO_ORDER_H;
}

Eigen::MatrixXd read_nevpt2_heff(size_t nstates, std::string filename, std::string method)
{
	std::string line_to_find;
	if(compare_strings(method,"pc-nevpt2"))
		line_to_find = "Zero + second order effective Hamiltonian (PC)";
	else if(compare_strings(method,"sc-nevpt2"))
		line_to_find = "Zero + second order Effective Hamiltonian (SC)";
	else
		opencap_throw("Either pc-nevpt2 or sc-nevpt2 should be selected.");
	Eigen::MatrixXd ZERO_ORDER_H(nstates,nstates);
	ZERO_ORDER_H= Eigen::MatrixXd::Zero(nstates,nstates);
	std::ifstream is(filename);
	if (is.good())
	{
		std::string line, rest;
		std::getline(is,line);
		while (line.find(line_to_find)== std::string::npos && is.peek()!=EOF)
			std::getline(is,line);
		if(is.peek()==EOF)
			opencap_throw("Error: Unable to find QD-NEVPT2 effective Hamiltonian.");
		std::getline(is,line);
		std::getline(is,line);
		size_t num_groups = nstates%5==0 ? nstates/5 : nstates/5+1;
		for (size_t i=1;i<=num_groups;i++)
		{
			std::getline(is,line);
			for (size_t j=1;j<=nstates;j++)
			{
				std::getline(is,line);
				std::vector<std::string> tokens = split(line,' ');
				size_t row_idx = std::stoul(tokens[0]);
				for(size_t k=1;k<tokens.size();k++)
				{
					size_t col_idx = k-1+(i-1)*5;
					if(col_idx>=ZERO_ORDER_H.cols() || row_idx-1 >= ZERO_ORDER_H.rows())
						opencap_throw("Error: State index of out bounds. There is a problem with the OpenMolcas output file. Exiting...");
					ZERO_ORDER_H(row_idx-1,col_idx)=std::stod(tokens[k]);
				}
			}
		}
	}
	else
	{
    	opencap_throw("Error: I couldn't read:" + filename);
	}
	return ZERO_ORDER_H;
}

std::vector<Atom> read_geometry_from_rassi(std::string filename)
{
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	auto nsym = file.readAttribute<long>("NSYM", "/");
	std::string atom_nums_tag,coords_tag;
	if (nsym>1)
	{
		atom_nums_tag="DESYM_CENTER_ATNUMS";
		coords_tag = "DESYM_CENTER_COORDINATES";
	}
	else
	{
		atom_nums_tag="CENTER_ATNUMS";
		coords_tag = "CENTER_COORDINATES";
	}
	std::vector<Atom> atoms;
	std::vector<long> atomic_nums;
	Eigen::MatrixXd coords_mat;
	file.readDataset(atomic_nums,atom_nums_tag);
	file.readDataset(coords_mat,coords_tag);
	for(size_t i=0;i<coords_mat.rows();i++)
		atoms.push_back(Atom(atomic_nums[i],coords_mat(i,0),coords_mat(i,1),coords_mat(i,2)));
	return atoms;
}

BasisSet read_basis_from_rassi(std::string filename,std::vector<Atom> atoms)
{
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	Eigen::Matrix<long,Eigen::Dynamic,Eigen::Dynamic> prim_ids,basis_ids;
	Eigen::MatrixXd prims;
	auto nsym = file.readAttribute<long>("NSYM", "/");
	file.readDataset(prims,"PRIMITIVES");
	file.readDataset(prim_ids,"PRIMITIVE_IDS");
	std::string bf_ids_tag;
	if(nsym>1)
		bf_ids_tag="DESYM_BASIS_FUNCTION_IDS";
	else
		bf_ids_tag ="BASIS_FUNCTION_IDS";
	file.readDataset(basis_ids,bf_ids_tag);
	BasisSet bs;
	for(auto atm:atoms)
		bs.centers.push_back(atm.coords);
	for(size_t i=0;i<basis_ids.rows();i++)
	{
		long ctr = basis_ids(i,0);
		long shell_num = basis_ids(i,1);
		int l = basis_ids(i,2);
		shell_id id(ctr,shell_num,l);
		int bs_idx = bs.get_index_of_shell_id(id);
		if (bs_idx==-1)
		{
			Shell new_shell(l,bs.centers[ctr-1]);
			new_shell.l=abs(l);
			if(l<0 && abs(l)>1)
				new_shell.pure=false;
			bs.add_shell(new_shell);
		}
	}
	for(size_t i=0;i<prim_ids.rows();i++)
	{
		long ctr = prim_ids(i,0);
		int l = prim_ids(i,1);
		long shell_num = prim_ids(i,2);
		double exp = prims(i,0);
		double coeff = prims(i,1);
		shell_id id(ctr,shell_num,l);
		int bs_idx = bs.get_index_of_shell_id(id);
		if (coeff!=0)
			bs.basis[bs_idx].add_primitive(exp,coeff);
	}
	bs.normalize();
	return bs;
}