// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>
#include <cmath>
#include <limits>
#include <memory>
#include <optional>
#include <stdexcept>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

#include "pyinterp/detail/axis/container.hpp"
#include "pyinterp/detail/math.hpp"
#include "pyinterp/eigen.hpp"

namespace pyinterp::axis {

/// Type of boundary handling on an Axis.
enum Boundary : uint8_t {
  kExpand,  //!< Expand the boundary as a constant.
  kWrap,    //!< Circular boundary conditions.
  kSym,     //!< Symmetrical boundary conditions.
  kUndef,   //!< Boundary violation is not defined.
};

}  // namespace pyinterp::axis

namespace pyinterp::detail {

/// A coordinate axis is a Variable that specifies one of the coordinates
/// of a variable's values.
///
/// @tparam T Type of data handled by this Axis
template <typename T>
class Axis {
 public:
  /// Default constructor
  Axis() = default;

  /// Create a coordinate axis from evenly spaced numbers over a specified
  /// interval.
  ///
  /// @param start the first value of the axis
  /// @param stop the last value of the axis
  /// @param num number of samples in the axis
  /// @param epsilon Maximum allowed difference between two numbers in
  /// order to consider them equal.
  /// @param is_circle True, if the axis can represent a circle.
  Axis(const T start, const T stop, const T num, const T epsilon,
       const bool is_circle)
      : circle_(is_circle ? T(360) : math::Fill<T>::value()),
        axis_(std::make_shared<axis::container::Regular<T>>(
            axis::container::Regular<T>(start, stop, num))) {
    compute_properties(epsilon);
  }

  /// Create a coordinate axis from values.
  ///
  /// @param values Axis values
  /// @param epsilon Maximum allowed difference between two real numbers in
  /// order to consider them equal.
  /// @param is_circle True, if the axis can represent a circle.
  explicit Axis(const Eigen::Ref<const Vector<T>>& values, T epsilon,
                bool is_circle)
      : circle_(is_circle ? T(360) : math::Fill<T>::value()) {
    // Axis size control
    if (values.size() > std::numeric_limits<int64_t>::max()) {
      throw std::invalid_argument(
          "The size of the axis must not contain more than " +
          std::to_string(std::numeric_limits<int64_t>::max()) + "elements.");
    }

    if (is_angle()) {
      auto normalized_values = normalize_longitude(values);
      normalized_values
          ? initialize_from_values(*normalized_values, epsilon, true)
          : initialize_from_values(values, epsilon, false);
    } else {
      initialize_from_values(values, epsilon, false);
    }
  }

  /// Destructor
  virtual ~Axis() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Axis(const Axis& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Axis(Axis&& rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Axis& rhs) -> Axis& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Axis&& rhs) noexcept -> Axis& = default;

  /// Get the ith coordinate value.
  ///
  /// @param index which coordinate. Between 0 and size()-1 inclusive
  /// @return coordinate value
  /// @throw std::out_of_range if index in not in range [0, size() - 1].
  [[nodiscard]] constexpr auto coordinate_value(const int64_t index) const
      -> T {
    if (index < 0 || index >= size()) {
      throw std::out_of_range("axis index out of range");
    }
    return axis_->coordinate_value(index);
  }

  /// Get the minimum coordinate value.
  ///
  /// @return minimum coordinate value
  [[nodiscard]] constexpr auto min_value() const -> T {
    return axis_->min_value();
  }

  /// Get the maximum coordinate value.
  ///
  /// @return maximum coordinate value
  [[nodiscard]] constexpr auto max_value() const -> T {
    return axis_->max_value();
  }

  /// Get the number of values for this axis
  ///
  /// @return the number of values
  [[nodiscard]] constexpr auto size() const noexcept -> int64_t {
    return axis_->size();
  }

  /// Check if this axis values are spaced regularly
  [[nodiscard]] constexpr auto is_regular() const noexcept -> bool {
    return dynamic_cast<axis::container::Regular<T>*>(axis_.get()) != nullptr;
  }

  /// Returns true if this axis represents a circle.
  [[nodiscard]] constexpr auto is_circle() const noexcept -> bool {
    return is_circle_;
  }

  /// Does the axis represent an angle?
  ///
  /// @return true if the axis represent an angle
  [[nodiscard]] constexpr auto is_angle() const noexcept -> bool {
    return math::Fill<T>::is_not(circle_);
  }

  /// Get the first value of this axis
  ///
  /// @return the first value
  [[nodiscard]] constexpr auto front() const -> T { return axis_->front(); }

  /// Get the last value of this axis
  ///
  /// @return the last value
  [[nodiscard]] constexpr auto back() const -> T { return axis_->back(); }

  /// Test if the data is sorted in ascending order.
  ///
  /// @return True if the data is sorted in ascending order.
  [[nodiscard]] constexpr auto is_ascending() const -> bool {
    return axis_->is_ascending();
  }

  /// Reverse the order of elements in this axis
  auto flip() -> void { axis_->flip(); }

  /// Get increment value if is_regular()
  ///
  /// @return increment value if is_regular()
  /// @throw std::logic_error if this instance does not represent a regular axis
  [[nodiscard]] constexpr auto increment() const -> T {
    auto ptr = dynamic_cast<axis::container::Regular<T>*>(axis_.get());
    if (ptr == nullptr) {
      throw std::logic_error("this axis is not regular.");
    }
    return ptr->step();
  }

  /// compare two variables instances
  ///
  /// @param rhs an other axis to compare
  /// @return if axis are equals
  constexpr auto operator==(Axis const& rhs) const -> bool {
    return *axis_ == *rhs.axis_ && is_circle_ == rhs.is_circle_;
  }

  /// compare two variables instances
  ///
  /// @param rhs an other axis to compare
  /// @return if axis are equals
  constexpr auto operator!=(Axis const& rhs) const -> bool {
    return !this->operator==(rhs);
  }

  /// Get the ith coordinate value.
  ///
  /// @param index which coordinate. Between 0 and size()-1 inclusive
  /// @return coordinate value
  constexpr auto operator()(const int64_t index) const -> T {
    return axis_->coordinate_value(index);
  }

  /// Returns the normalized value of the coordinate with the respect to the
  /// axis definition.
  [[nodiscard]] constexpr auto normalize_coordinate(const T coordinate,
                                                    const T min) const noexcept
      -> T {
    if (is_angle() && (coordinate >= min + circle_ || coordinate < min)) {
      return math::normalize_angle(coordinate, min, circle_);
    }
    return coordinate;
  }

  /// Returns the normalized value with respect to the axis definition. This
  /// means if the axis defines a circle, this method returns a value within the
  /// interval [font(), back()] otherwise it returns the value supplied.
  [[nodiscard]] constexpr auto normalize_coordinate(
      const T coordinate) const noexcept -> T {
    return normalize_coordinate(coordinate, axis_->min_value());
  }

  /// Given a coordinate position, find what axis element contains it.
  ///
  /// @param coordinate position in this coordinate system
  /// @param bounded if true, returns "-1" if the value is located outside this
  /// coordinate system, otherwise the value of the first element if the value
  /// is located before, or the value of the last element of this container if
  /// the requested value is located after.
  /// @return index of the grid point containing it or -1 if outside grid area
  [[nodiscard]] constexpr auto find_index(const T coordinate,
                                          const bool bounded) const -> int64_t {
    return axis_->find_index(normalize_coordinate(coordinate), bounded);
  }

  /// Given a coordinate position, find what axis element contains it.
  ///
  /// This function is identical to the "index_function" except that the values
  /// located between the last and the first point of the axis representing a
  /// circle will not return the value -1.
  ///
  /// @param coordinate position in this coordinate system
  /// @param bounded if true, returns "-1" if the value is located outside this
  /// coordinate system, otherwise the value of the first element if the value
  /// is located before, or the value of the last element of this container if
  /// the requested value is located after.
  /// @return index of the grid point containing it or -1 if outside grid area
  [[nodiscard]] constexpr auto find_nearest_index(T coordinate,
                                                  const bool bounded) const
      -> int64_t {
    coordinate = normalize_coordinate(coordinate);
    auto result = axis_->find_index(coordinate, bounded);
    if (result == -1 && is_circle_) {
      result = (coordinate - max_value()) < (min_value() + 360 - coordinate)
                   ? this->size() - 1
                   : 0;
    }
    return result;
  }

  /// Given a coordinate position, find grids elements around it.
  /// This mean that
  /// @code
  /// (*this)(i0) <= coordinate < (*this)(i1)
  /// @endcode
  ///
  /// @param coordinate position in this coordinate system
  /// @return None if coordinate is outside the axis definition domain otherwise
  /// the tuple (i0, i1)
  [[nodiscard]] auto find_indexes(T coordinate) const
      -> std::optional<std::tuple<int64_t, int64_t>> {
    coordinate = normalize_coordinate(coordinate);
    auto length = size();
    auto i0 = find_index(coordinate, false);

    /// If the value is outside the circle, then the value is between the last
    /// and first index.
    if (i0 == -1) {
      return is_circle_ ? std::make_tuple(static_cast<int64_t>(length - 1), 0LL)
                        : std::optional<std::tuple<int64_t, int64_t>>();
    }

    // Given the delta between the found coordinate and the given coordinate,
    // chose the other index that frames the coordinate
    auto delta = coordinate - (*this)(i0);
    auto i1 = i0;
    if (delta == 0) {
      // The requested coordinate is located on an element of the axis.
      i1 == length - 1 ? --i0 : ++i1;
    } else {
      if (delta < 0) {
        // The found point is located after the coordinate provided.
        is_ascending() ? --i0 : ++i0;
        if (is_circle_) {
          i0 = math::remainder(i0, length);
        }
      } else {
        // The found point is located before the coordinate provided.
        is_ascending() ? ++i1 : --i1;
        if (is_circle_) {
          i1 = math::remainder(i1, length);
        }
      }
    }

    if (i0 >= 0 && i0 < length && i1 >= 0 && i1 < length) {
      return std::make_tuple(i0, i1);
    }
    return std::optional<std::tuple<int64_t, int64_t>>{};
  }

  /// Create a table of "size" indices located on either side of the required
  /// position.
  ///
  /// @param coordinate Position in this coordinate system
  /// @param size Size of the half window to be built.
  /// @param boundary How to handle boundaries (this parameter is not used if
  /// the manipulated axis is a circle.)
  /// @return A table of size "2*size" containing the indices of the axis
  /// framing the value provided or an empty table if the value is located
  /// outside the axis definition domain.
  [[nodiscard]] auto find_indexes(T coordinate, uint32_t size,
                                  ::pyinterp::axis::Boundary boundary) const
      -> std::vector<int64_t> {
    if (size == 0) {
      throw std::invalid_argument("The size must not be zero.");
    }

    // Axis size
    auto len = this->size();

    // Searches the initial indexes and populate the result
    auto indexes = find_indexes(coordinate);
    if (!indexes) {
      return {};
    }
    auto result = std::vector<int64_t>(size << 1U);
    std::tie(result[size - 1], result[size]) = *indexes;

    // Offset in relation to the first indexes found
    uint32_t shift = 1;

    // Construction of window indexes based on the initial indexes found
    while (shift < size) {
      int64_t before = std::get<0>(*indexes) - shift;
      if (before < 0) {
        if (!is_circle_) {
          switch (boundary) {
            case ::pyinterp::axis::kExpand:
              before = 0;
              break;
            case ::pyinterp::axis::kWrap:
              before = math::remainder(len + before, len);
              break;
            case ::pyinterp::axis::kSym:
              before = math::remainder(-before, len);
              break;
            default:
              return {};
          }
        } else {
          before = math::remainder(before, len);
        }
      }
      int64_t after = std::get<1>(*indexes) + shift;
      if (after >= len) {
        if (!is_circle_) {
          switch (boundary) {
            case ::pyinterp::axis::kExpand:
              after = len - 1;
              break;
            case ::pyinterp::axis::kWrap:
              after = math::remainder(after, len);
              break;
            case ::pyinterp::axis::kSym:
              after = len - 2 - math::remainder(after - len, len);
              break;
            default:
              return {};
          }
        } else {
          after = math::remainder(after, len);
        }
      }
      result[size - shift - 1] = before;
      result[size + shift] = after;
      ++shift;
    }
    return result;
  }

  /// Get a string representation of a coordinate handled by this axis.
  ///
  /// @param value Value to be converted to string
  /// @return a string representation of the value
  [[nodiscard]] virtual inline auto coordinate_repr(const T value) const
      -> std::string {
    return std::to_string(value);
  }

 protected:
  /// Gets the axis handler
  ///
  /// @return the axis handler
  [[nodiscard]] inline auto handler() const noexcept
      -> const std::shared_ptr<axis::container::Abstract<T>>& {
    return axis_;
  }

  /// Construction of a serialized instance.
  ///
  /// @param axis Axis handler
  /// @param is_circle True, if the axis can represent a circle.
  Axis(std::shared_ptr<axis::container::Abstract<T>> axis, const bool is_circle)
      : is_circle_(is_circle),
        circle_(is_circle_ ? T(360) : math::Fill<T>::value()),
        axis_(std::move(axis)) {}

 private:
  /// True, if the axis represents a circle.
  bool is_circle_{false};

  /// The value of the circle (360, π)
  T circle_{math::Fill<T>::value()};

  /// The object that handles access and searches for the values defined by
  /// the axis.
  std::shared_ptr<axis::container::Abstract<T>> axis_{
      std::make_shared<axis::container::Undefined<T>>()};

  /// Computes axis's properties
  void compute_properties(T epsilon) {
    // An axis can be represented by an empty set of values
    if (axis_->size() == 0) {
      throw std::invalid_argument("unable to create an empty axis.");
    }
    // Axis values must be sorted.
    if (!axis_->is_monotonic()) {
      throw std::invalid_argument("axis values are not ordered");
    }
    // If this axis represents an angle, determine if it represents the entire
    // trigonometric circle.
    if (is_angle()) {
      auto ptr = dynamic_cast<axis::container::Regular<T>*>(axis_.get());
      if (ptr != nullptr) {
        is_circle_ = math::is_same(
            static_cast<T>(std::fabs(ptr->step() * size())), circle_, epsilon);
      } else {
        auto increment = (axis_->back() - axis_->front()) /
                         static_cast<T>(axis_->size() - 1);
        is_circle_ = std::fabs((axis_->max_value() - axis_->min_value()) -
                               circle_) <= increment;
      }
    }
  }

  /// Put longitude into the range [0, circle_] degrees.
  auto normalize_longitude(const Vector<T>& points)
      -> std::unique_ptr<Vector<T>> {
    auto monotonic = true;
    auto ascending = points.size() < 2 ? true : points[0] < points[1];

    for (Eigen::Index ix = 1; ix < points.size(); ++ix) {
      monotonic =
          ascending ? points[ix - 1] < points[ix] : points[ix - 1] > points[ix];

      if (!monotonic) {
        break;
      }
    }

    if (!monotonic) {
      auto result = std::make_unique<Vector<T>>(points);
      auto cross = false;

      for (Eigen::Index ix = 1; ix < result->size(); ++ix) {
        if (!cross) {
          cross = ascending ? (*result)[ix - 1] > (*result)[ix]
                            : (*result)[ix - 1] < (*result)[ix];
        }

        if (cross) {
          (*result)[ix] += ascending ? circle_ : -circle_;
        }
      }
      return result;
    }
    return nullptr;
  }

  /// Initializes the axis container from values.
  auto initialize_from_values(const Eigen::Ref<const Vector<T>>& values,
                              const T epsilon, const bool move) -> void {
    // Determines whether the set of data provided can be represented as an
    // interval.
    auto increment = Axis<T>::is_evenly_spaced(values, epsilon);
    if (increment) {
      axis_ = std::make_shared<axis::container::Regular<T>>(
          values[0], values[values.size() - 1], static_cast<T>(values.size()));
    } else {
      // Avoid data copy if possible.
      axis_ = std::make_shared<axis::container::Irregular<T>>(
          move ? std::move(values) : values);
    }
    compute_properties(epsilon);
  }

  /// Determines whether the values contained in the vector are evenly spaced
  /// from each other.
  ///
  /// @param points Values to be tested
  /// @param epsilon Maximum allowed difference between two numbers in
  /// order to consider them equal
  /// @return The increment between two values if the values are evenly spaced
  static auto is_evenly_spaced(const Eigen::Ref<const Vector<T>>& points,
                               const T epsilon) -> std::optional<T> {
    size_t n = points.size();

    // The axis is defined by a single value.
    if (n < 2) {
      return {};
    }

    T increment =
        (points[points.size() - 1] - points[0]) / static_cast<T>(n - 1);

    // If the first two values are constant, the values are not evenly spaced.
    if (std::abs(increment) <= epsilon) {
      return {};
    }

    for (size_t ix = 1; ix < n; ++ix) {
      if (!math::is_same(points[ix] - points[ix - 1], increment, epsilon)) {
        return {};
      }
    }
    return increment;
  }
};

}  // namespace pyinterp::detail


#include <boost/array.hpp>
#include <uav_ros_lib/estimation/constant_acceleration_ukf.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <list>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>

int main(int argc, char **argv) {
  ros::init(argc, argv, "velocity_estimation_node");

  ros::NodeHandle nhPrivate("~");
  ros::NodeHandle nh;
  ConstantAccelerationUKF kWrapperXPos("const_acc_x", nhPrivate);
  ConstantAccelerationUKF kWrapperYPos("const_acc_y", nhPrivate);
  ConstantAccelerationUKF kWrapperZPos("const_acc_z", nhPrivate);

  geometry_msgs::PoseStamped poseMsg;
  bool newMeasurementFlag = false;
  const auto poseCb = [&](const geometry_msgs::PoseStampedConstPtr &msg) {
    poseMsg = *msg;
    newMeasurementFlag = true;
  };

  sensor_msgs::Imu myImuMsg;
  sensor_msgs::Imu oldImuMsg;
  const auto imuCb = [&](const sensor_msgs::ImuConstPtr &msg) {
    // Only do acceleration correction when acceleration measurement changes
    if (oldImuMsg.linear_acceleration.x != msg->linear_acceleration.x ||
        oldImuMsg.linear_acceleration.y != msg->linear_acceleration.y ||
        oldImuMsg.linear_acceleration.z != msg->linear_acceleration.z) {
      myImuMsg = *msg;
      auto quat = Eigen::Quaternionf(msg->orientation.w, msg->orientation.x,
                                     msg->orientation.y, msg->orientation.z);
      auto lin_acc = Eigen::Vector3f(msg->linear_acceleration.x,
                                     msg->linear_acceleration.y,
                                     msg->linear_acceleration.z);
      auto correct_acc = quat * lin_acc;
      myImuMsg.linear_acceleration.x = correct_acc.x();
      myImuMsg.linear_acceleration.y = correct_acc.y();
      myImuMsg.linear_acceleration.z = correct_acc.z();
    }

    oldImuMsg = *msg;
  };
  auto imuSub = nh.subscribe<sensor_msgs::Imu>("imu", 1, imuCb);

  auto odomPub = nh.advertise<nav_msgs::Odometry>("odometry", 1);
  auto imuPub = nh.advertise<sensor_msgs::Imu>("/imu_est", 1);
  static constexpr auto dt = 0.02;
  static constexpr auto throttle_time = 2.0;
  const auto timerCb = [&](const ros::TimerEvent & /* unused */) {
    kWrapperXPos.estimateState(
        dt, {poseMsg.pose.position.x, myImuMsg.linear_acceleration.x},
        newMeasurementFlag);
    kWrapperYPos.estimateState(
        dt, {poseMsg.pose.position.y, myImuMsg.linear_acceleration.y},
        newMeasurementFlag);
    kWrapperZPos.estimateState(
        dt,
        {poseMsg.pose.position.z, (myImuMsg.linear_acceleration.z - 9.80689)},
        newMeasurementFlag);

    nav_msgs::Odometry kalmanOdomMsg;
    kalmanOdomMsg.header.stamp = poseMsg.header.stamp;
    kalmanOdomMsg.header.frame_id = poseMsg.header.frame_id;
    kalmanOdomMsg.pose.pose.position.x = kWrapperXPos.getState()[0];
    kalmanOdomMsg.pose.pose.position.y = kWrapperYPos.getState()[0];
    kalmanOdomMsg.pose.pose.position.z = kWrapperZPos.getState()[0];
    kalmanOdomMsg.pose.pose.orientation = poseMsg.pose.orientation;
    kalmanOdomMsg.twist.twist.linear.x = kWrapperXPos.getState()[1];
    kalmanOdomMsg.twist.twist.linear.y = kWrapperYPos.getState()[1];
    kalmanOdomMsg.twist.twist.linear.z = -kWrapperZPos.getState()[1];
    kalmanOdomMsg.pose.covariance[0] = kWrapperXPos.getStateCovariance()(0, 0);
    kalmanOdomMsg.pose.covariance[1] = kWrapperYPos.getStateCovariance()(0, 0);
    kalmanOdomMsg.pose.covariance[2] = kWrapperZPos.getStateCovariance()(0, 0);
    kalmanOdomMsg.pose.covariance[3] = kWrapperXPos.getStateCovariance()(1, 1);
    kalmanOdomMsg.pose.covariance[4] = kWrapperYPos.getStateCovariance()(1, 1);
    kalmanOdomMsg.pose.covariance[5] = kWrapperZPos.getStateCovariance()(1, 1);
    kalmanOdomMsg.pose.covariance[6] = kWrapperXPos.getStateCovariance()(2, 2);
    kalmanOdomMsg.pose.covariance[7] = kWrapperYPos.getStateCovariance()(2, 2);
    kalmanOdomMsg.pose.covariance[8] = kWrapperZPos.getStateCovariance()(2, 2);
    kalmanOdomMsg.pose.covariance[9] = kWrapperXPos._nis;
    kalmanOdomMsg.pose.covariance[10] = kWrapperYPos._nis;
    kalmanOdomMsg.pose.covariance[11] = kWrapperZPos._nis;
    odomPub.publish(kalmanOdomMsg);

    sensor_msgs::Imu estImuMsg;
    estImuMsg.header.frame_id = myImuMsg.header.frame_id;
    estImuMsg.header.stamp = myImuMsg.header.stamp;
    estImuMsg.linear_acceleration.x = kWrapperXPos.getState()[2];
    estImuMsg.linear_acceleration.y = kWrapperYPos.getState()[2];
    estImuMsg.linear_acceleration.z = kWrapperZPos.getState()[2];
    estImuMsg.angular_velocity.x = myImuMsg.linear_acceleration.x;
    estImuMsg.angular_velocity.y = myImuMsg.linear_acceleration.y;
    estImuMsg.angular_velocity.z = myImuMsg.linear_acceleration.z;
    imuPub.publish(estImuMsg);

    if (newMeasurementFlag) {
      newMeasurementFlag = false;
    }
  };

  auto poseSub =
      nh.subscribe<geometry_msgs::PoseStamped>("poseStamped", 1, poseCb);
  while (poseSub.getNumPublishers() == 0) {
    ROS_WARN_STREAM("Waiting for " << poseSub.getTopic()
                                   << " topic publisher.");
    ros::Duration(0.5).sleep();
  }

  ROS_INFO("Starting velocity_estimation_node...");
  auto kalmanTimer = nh.createTimer(ros::Duration(dt), timerCb);
  ros::spin();
}


//
//  Copyright Toon Knapen
//
// 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_NUMERIC_BINDINGS_AMOS_AMOS_HPP
#define BOOST_NUMERIC_BINDINGS_AMOS_AMOS_HPP

#include <boost/numeric/bindings/amos/amos_overloads.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/vector_traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace amos {

  template < typename vector_type, typename value_type >
  int besi(const value_type&                                             z,   // std::complex< float > or std::complex< double >
           const typename traits::type_traits< value_type >::real_type   fnu, // float or double
           int                                                           kode,
           vector_type&                                                  cy,
           int&                                                          nz)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename traits::vector_traits<vector_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename vector_type::value_type >::value ) ) ;
#endif

    int n = traits::vector_size( cy ) ;
    value_type * cy_ptr = traits::vector_storage( cy ) ;

    int error = 0 ;
    detail::besi( z, fnu, kode, n, cy_ptr, nz, error ) ;
    return error ;
  }

  template < typename vector_type, typename value_type >
  int besj(const value_type&                                            z,
           const typename traits::type_traits< value_type >::real_type  fnu,
           int                                                          kode,
           vector_type&                                                 cy,
           int&                                                         nz)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename traits::vector_traits<vector_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename vector_type::value_type >::value ) ) ;
#endif

    int n = traits::vector_size( cy ) ;
    value_type * cy_ptr = traits::vector_storage( cy ) ;

    int error = 0 ;
    detail::besj( z, fnu, kode, n, cy_ptr, nz, error ) ;
    return error ;
  }

  template < typename vector_type, typename value_type >
  int besy(const value_type&                                           z,
           const typename traits::type_traits< value_type >::real_type fnu,
           int                                                         kode,
           vector_type&                                                cy,
           int&                                                        nz)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename traits::vector_traits<vector_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename vector_type::value_type >::value ) ) ;
#endif

    int n = traits::vector_size( cy ) ;
    value_type * cy_ptr = traits::vector_storage( cy ) ;

    int error = 0 ;
    value_type * cwrk = new value_type[n];
    detail::besy( z, fnu, kode, n, cy_ptr, nz, cwrk, error ) ;
    delete[] cwrk ;
    return error ;
  }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_AMOS_AMOS_HPP


#pragma once

#include <armadillo>

namespace container
{
    class Variant;
    typedef Variant* Variant_t;

    class Variant
    {
    public:
        template<typename T>
        static Variant_t Create(const int& nRows, const int& nCols);

        ~Variant();

        template<typename T>
        arma::Mat<T>* Get();

    private:

        Variant(arma::Mat<int>* const obj);
        Variant(arma::Mat<double>* const obj);

        arma::Mat<int>* const obj_i;
        arma::Mat<double>* const obj_f;
    };
}


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::generate.hpp                                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_GENERATE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_GENERATE_HPP_ER_2009
#include <iterator>
#include <functional>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/utility.hpp>
#include <boost/statistics/detail/importance_sampling/random/sampler.hpp>
#include <boost/random/ref_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace importance_sampling{

    template<typename ItT,typename N,typename ItW,typename ItP,typename U>
    ItT generate(
        ItT b_t,        // target values (output)
        N n,            // sample size
        ItW b_w,        // unnormalized weights
        ItW e_w,        // unnormalized weights
        ItP b_p,        // proposal values
        U& urng
    )
    {
        typedef boost::iterator_range<ItW>                  range_w_;
        typedef boost::iterator_range<ItP>                  range_p_;
        typedef typename boost::iterator_value<ItW>::type   w_;
        typedef sampler<range_p_,w_>                        iss_;
        typedef boost::random::ref_distribution<iss_&>      ref_iss_;
        typedef boost::variate_generator<U&,ref_iss_>       gen_iss_;

        range_w_    range_w(b_w,e_w);
        range_p_    range_p(
            b_p,
            boost::next(
                b_p,
                std::distance(
                    b_w,
                    e_w
                )
            )
        );

        iss_        iss( range_w, range_p );
        ref_iss_    ref_iss( iss );
        gen_iss_    gen_iss( urng, iss );

        return std::generate_n(
            b_t,
            n,
            gen_iss
        );
    }

}// importance_sampling
}// statistics
}// detail
}// boost

#endif


/*Copyright (c) 2020 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.
*/

/*
 * gto_ordering.cpp
 */
#include "BasisSet.h"
#include "Shell.h"
#include "utils.h"
#include <array>
#include <vector>
#include <algorithm>
#include <math.h>
#include <tuple>
#include "gto_ordering.h"
#include "opencap_exception.h"
#include <h5pp/h5pp.h>
#include <iostream>
#include <Eigen/Dense>

std::vector<std::array<size_t,3>> opencap_carts_ordering(int l)
{
	//s
	if(l == 0)
		return {{0,0,0}};
	//p
	if(l==1)
		return {{1,0,0},{0,1,0},{0,0,1}};
	//d
	else if (l==2)
		return {{2,0,0},{0,2,0},{0,0,2},{1,1,0},{1,0,1},{0,1,1}};
	//f
	else if (l==3)
		return {{3,0,0},{0,3,0},{0,0,3},{1,2,0},{2,1,0},{2,0,1},{1,0,2},
				{0,1,2},{0,2,1},{1,1,1}};
	//g
	else if(l==4)
		return {{4,0,0},{0,4,0},{0,0,4},{3,1,0},{3,0,1},{1,3,0},{0,3,1},
		        {1,0,3},{0,1,3},{2,2,0},{2,0,2},{0,2,2},{2,1,1},{1,2,1},{1,1,2}};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

std::vector<int> opencap_harmonic_ordering(int l)
{
	//s
	if(l == 0)
		return {0};
	//p
	if(l==1)
		return {1,-1,0};
	//d
	else if (l==2)
		return {0,1,-1,2,-2};
	//f
	else if (l==3)
		return {0,1,-1,2,-2,3,-3};
	//g
	else if (l==4)
		return {0,1,-1,2,-2,3,-3,4,-4};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

// From: pyscf tools/molden.py
std::vector<std::array<size_t,3>> pyscf_carts_ordering(int l)
{
	//s
	if(l == 0)
		return {{0,0,0}};
	//p
	if(l==1)
		return {{1,0,0},{0,1,0},{0,0,1}};
	//d
	else if (l==2)
		return {{2,0,0},{1,1,0},{1,0,1},{0,2,0},{0,1,1},{0,0,2}};
	//f
	else if (l==3)
		return {{3,0,0},{2,1,0},{2,0,1},{1,2,0},{1,1,1},{1,0,2},{0,3,0},
				{0,2,1},{0,1,2},{0,0,3}};
	//g
	else if(l==4)
		return {{4,0,0},{3,1,0},{3,0,1},{2,2,0},{2,1,1},{2,0,2},{1,3,0},
		        {1,2,1},{1,1,2},{1,0,3},{0,4,0},{0,3,1},{0,2,2},{0,1,3},{0,0,4}};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

std::vector<int> pyscf_harmonic_ordering(int l)
{
	//s
	if(l == 0)
		return {0};
	//p
	if(l==1)
		return {1,-1,0};
	//d
	else if (l==2)
		return {-2,-1,0,1,2};
	//f
	else if (l==3)
		return {-3,-2,-1,0,1,2,3};
	//g
	else if (l==4)
		return {-4,-3,-2,-1,0,1,2,3,4};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

std::vector<bf_id> get_pyscf_ids(BasisSet &bs)
{
	std::vector<bf_id> ids;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		if (my_shell.pure)
		{
			std::vector<int> pyscf_order = pyscf_harmonic_ordering(my_shell.l);
			for(int m:pyscf_order)
				ids.push_back(bf_id(bs.shell_ids[i],m));
		}
		else
		{
			std::vector<std::array<size_t,3>> opencap_order = opencap_carts_ordering(my_shell.l);
			std::vector<std::array<size_t,3>> pyscf_order = pyscf_carts_ordering(my_shell.l);
			for(auto m:pyscf_order)
			{
				auto it = std::find(opencap_order.begin(), opencap_order.end(), m);
				if (it != opencap_order.end())
				{
					int index = std::distance(opencap_order.begin(), it);
					ids.push_back(bf_id(bs.shell_ids[i],index));
				}
				else
				    opencap_throw("Something's gone wrong.");
			}
		}
	}
	return ids;
}

//from:http://cheminf.cmbi.ru.nl/molden/molden_format.html
std::vector<std::array<size_t,3>> molden_carts_ordering(int l)
{
	//s
	if(l == 0)
		return {{0,0,0}};
	//p
	if(l==1)
		return {{1,0,0},{0,1,0},{0,0,1}};
	//d
	else if (l==2)
		return {{2,0,0},{0,2,0},{0,0,2},{1,1,0},{1,0,1},{0,1,1}};
	//f
	else if (l==3)
		return {{3,0,0},{0,3,0},{0,0,3},{1,2,0},{2,1,0},{2,0,1},{1,0,2},
				{0,1,2},{0,2,1},{1,1,1}};
	//g
	else if(l==4)
		return {{4,0,0},{0,4,0},{0,0,4},{3,1,0},{3,0,1},{1,3,0},{0,3,1},
		        {1,0,3},{0,1,3},{2,2,0},{2,0,2},{0,2,2},{2,1,1},{1,2,1},{1,1,2}};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

//from:http://cheminf.cmbi.ru.nl/molden/molden_format.html
std::vector<int> molden_harmonic_ordering(int l)
{
	//s
	if(l == 0)
		return {0};
	//p
	if(l==1)
		return {1,-1,0};
	//d
	else if (l==2)
		return {0,1,-1,2,-2};
	//f
	else if (l==3)
		return {0,1,-1,2,-2,3,-3};
	//g
	else if (l==4)
		return {0,1,-1,2,-2,3,-3,4,-4};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

std::vector<bf_id> get_molden_ids(BasisSet &bs)
{
	std::vector<bf_id> ids;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		if (my_shell.pure)
		{
			std::vector<int> molden_order = molden_harmonic_ordering(my_shell.l);
			for(int m:molden_order)
				ids.push_back(bf_id(bs.shell_ids[i],m));
		}
		else
		{
			std::vector<std::array<size_t,3>> opencap_order = opencap_carts_ordering(my_shell.l);
			std::vector<std::array<size_t,3>> molden_order = molden_carts_ordering(my_shell.l);
			for(auto m:molden_order)
			{
				auto it = std::find(opencap_order.begin(), opencap_order.end(), m);
				if (it != opencap_order.end())
				{
					int index = std::distance(opencap_order.begin(), it);
					ids.push_back(bf_id(bs.shell_ids[i],index));
				}
				else
				    opencap_throw("Something's gone wrong.");
			}
		}
	}
	return ids;
}

//same as molden except for g-type orbitals
std::vector<std::array<size_t,3>> qchem_carts_ordering(int l)
{
	//s
	if(l == 0)
		return {{0,0,0}};
	//p
	if(l==1)
		return {{1,0,0},{0,1,0},{0,0,1}};
	//d
	else if (l==2)
		return {{2,0,0},{0,2,0},{0,0,2},{1,1,0},{1,0,1},{0,1,1}};
	//f
	else if (l==3)
		return {{3,0,0},{0,3,0},{0,0,3},{1,2,0},{2,1,0},{2,0,1},{1,0,2},
				{0,1,2},{0,2,1},{1,1,1}};
	//g
	else if(l==4)
		return {{0,0,4},{0,1,3},{0,2,2},{0,3,1},{0,4,0},{1,0,3},{1,1,2},
		        {1,2,1},{1,3,0},{2,0,2},{2,1,1},{2,2,0},{3,0,1},{3,1,0},{4,0,0}};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

//should be same as molden
std::vector<int> qchem_harmonic_ordering(int l)
{
	//s
	if(l == 0)
		return {0};
	//p
	if(l==1)
		return {1,-1,0};
	//d
	else if (l==2)
		return {0,1,-1,2,-2};
	//f
	else if (l==3)
		return {0,1,-1,2,-2,3,-3};
	//g
	else if (l==4)
		return {0,1,-1,2,-2,3,-3,4,-4};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

std::vector<bf_id> get_qchem_ids(BasisSet &bs)
{
	std::vector<bf_id> ids;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		if (my_shell.pure)
		{
			std::vector<int> qchem_order = qchem_harmonic_ordering(my_shell.l);
			for(int m:qchem_order)
				ids.push_back(bf_id(bs.shell_ids[i],m));
		}
		else
		{
			std::vector<std::array<size_t,3>> opencap_order = opencap_carts_ordering(my_shell.l);
			std::vector<std::array<size_t,3>> qchem_order = qchem_carts_ordering(my_shell.l);
			for(auto m:qchem_order)
			{
				auto it = std::find(opencap_order.begin(), opencap_order.end(), m);
				if (it != opencap_order.end())
				{
					int index = std::distance(opencap_order.begin(), it);
					ids.push_back(bf_id(bs.shell_ids[i],index));
				}
				else
				    opencap_throw("Something's gone wrong.");
			}
		}
	}
	return ids;
}

std::vector<std::array<size_t,3>> molcas_carts_ordering(int l)
{
	//s
	if(l == 0)
		return {{0,0,0}};
	//p
	if(l==1)
		return {{1,0,0},{0,1,0},{0,0,1}};
	//d
	else if (l==2)
		return {{2,0,0},{1,1,0},{1,0,1},{0,2,0},{0,1,1},{0,0,2}};
	//f
	else if (l==3)
		return {{3,0,0},{2,1,0},{2,0,1},{1,2,0},{1,1,1},{1,0,2},{0,3,0},
				{0,2,1},{0,1,2},{0,0,3}};
	//g
	else if(l==4)
		return { {4,0,0},{3,1,0},{3,0,1},{2,2,0},{2,1,1},{2,0,2},{1,3,0},
		         {1,2,1},{1,1,2},{1,0,3},{0,4,0},{0,3,1},{0,2,2},{0,1,3},{0,0,4}};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

std::vector<int> molcas_harmonic_ordering(int l)
{
	//s
	if(l == 0)
		return {0};
	//p
	if(l==1)
		return {1,-1,0};
	//d
	else if (l==2)
		return {-2,-1,0,1,2};
	//f
	else if (l==3)
		return {-3,-2,-1,0,1,2,3};
	//g
	else if (l==4)
		return {-4,-3,-2,-1,0,1,2,3,4};
	else
		opencap_throw("Error: Only up to G type orbitals are supported.")
}

size_t get_molcas_cart_idx(int l,int m)
{
	int cur = -1*(opencap_harmonic_ordering(l).size()/2);
	size_t num_functions = molcas_carts_ordering(l).size();
	std::vector<int> indices;
	for(size_t i=1;i<=num_functions;i++)
	{
		indices.push_back(cur);
		cur++;
	}
	auto it = std::find(indices.begin(), indices.end(), m);
	if (it != indices.end())
	{
		return std::distance(indices.begin(), it);
	}
	else
	    opencap_throw("Something's gone wrong.");
}

std::vector<bf_id> get_molcas_ids(BasisSet &bs,std::string rassi_filename)
{
	std::vector<bf_id> ids;
	h5pp::File file(rassi_filename, h5pp::FilePermission::READONLY);
	Eigen::Matrix<long,Eigen::Dynamic,Eigen::Dynamic> basis_ids;
	auto nsym = file.readAttribute<long>("NSYM", "/");
	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);
	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);
		size_t shell_idx = bs.get_index_of_shell_id(shell_id(ctr,shell_num,l));
		Shell my_shell = bs.basis[shell_idx];
		long m = basis_ids(i,3);
		//cartesian, need to translate molcas cartesian IDs to opencap IDs
		if(l<0)
		{
			std::vector<std::array<size_t,3>> molcas_order = molcas_carts_ordering(my_shell.l);
			std::vector<std::array<size_t,3>> opencap_order = opencap_carts_ordering(my_shell.l);
			std::array<size_t,3> cart_m = molcas_order[get_molcas_cart_idx(my_shell.l,m)];
			auto it = std::find(opencap_order.begin(), opencap_order.end(), cart_m);
			if (it != opencap_order.end())
			{
				int index = std::distance(opencap_order.begin(), it);
				ids.push_back(bf_id(bs.shell_ids[shell_idx],index));
			}
			else
			    opencap_throw("I didn't find that shell.");
		}
		//spherical harmonic, don't need to do anything fancy
		else
		{
			ids.push_back(bf_id(bs.shell_ids[shell_idx],m));
		}
	}
	return ids;
}

void to_opencap_ordering(Eigen::MatrixXd &mat,BasisSet &bs,std::vector<bf_id> input_ids)
{
	std::vector<std::tuple<int,int>> swap_indices;
	for(size_t i=0;i<input_ids.size();i++)
	{
		for(size_t j=0;j<bs.bf_ids.size();j++)
		{
			if(input_ids[i]==bs.bf_ids[j])
			{
				swap_indices.push_back(std::make_tuple(i, j));
			}
		}
	}
	Eigen::MatrixXd per_mat(bs.Nbasis,bs.Nbasis);
	per_mat= Eigen::MatrixXd::Zero(bs.Nbasis,bs.Nbasis);
	for(auto t:swap_indices)
		per_mat(std::get<0>(t),std::get<1>(t))=1;
	// permute indices: P^T * A * P
	mat = per_mat.transpose()* mat * per_mat;
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_GGES_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_GGES_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for gges is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t gges( const char jobvsl, const char jobvsr,
        const char sort, external_fp selctg, const fortran_int_t n, float* a,
        const fortran_int_t lda, float* b, const fortran_int_t ldb,
        fortran_int_t& sdim, float* alphar, float* alphai, float* beta,
        float* vsl, const fortran_int_t ldvsl, float* vsr,
        const fortran_int_t ldvsr, float* work, const fortran_int_t lwork,
        fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_SGGES( &jobvsl, &jobvsr, &sort, selctg, &n, a, &lda, b, &ldb,
            &sdim, alphar, alphai, beta, vsl, &ldvsl, vsr, &ldvsr, work,
            &lwork, bwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t gges( const char jobvsl, const char jobvsr,
        const char sort, external_fp selctg, const fortran_int_t n, double* a,
        const fortran_int_t lda, double* b, const fortran_int_t ldb,
        fortran_int_t& sdim, double* alphar, double* alphai, double* beta,
        double* vsl, const fortran_int_t ldvsl, double* vsr,
        const fortran_int_t ldvsr, double* work, const fortran_int_t lwork,
        fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_DGGES( &jobvsl, &jobvsr, &sort, selctg, &n, a, &lda, b, &ldb,
            &sdim, alphar, alphai, beta, vsl, &ldvsl, vsr, &ldvsr, work,
            &lwork, bwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
inline std::ptrdiff_t gges( const char jobvsl, const char jobvsr,
        const char sort, external_fp selctg, const fortran_int_t n,
        std::complex<float>* a, const fortran_int_t lda,
        std::complex<float>* b, const fortran_int_t ldb, fortran_int_t& sdim,
        std::complex<float>* alpha, std::complex<float>* beta,
        std::complex<float>* vsl, const fortran_int_t ldvsl,
        std::complex<float>* vsr, const fortran_int_t ldvsr,
        std::complex<float>* work, const fortran_int_t lwork, float* rwork,
        fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_CGGES( &jobvsl, &jobvsr, &sort, selctg, &n, a, &lda, b, &ldb,
            &sdim, alpha, beta, vsl, &ldvsl, vsr, &ldvsr, work, &lwork, rwork,
            bwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
inline std::ptrdiff_t gges( const char jobvsl, const char jobvsr,
        const char sort, external_fp selctg, const fortran_int_t n,
        std::complex<double>* a, const fortran_int_t lda,
        std::complex<double>* b, const fortran_int_t ldb, fortran_int_t& sdim,
        std::complex<double>* alpha, std::complex<double>* beta,
        std::complex<double>* vsl, const fortran_int_t ldvsl,
        std::complex<double>* vsr, const fortran_int_t ldvsr,
        std::complex<double>* work, const fortran_int_t lwork, double* rwork,
        fortran_bool_t* bwork ) {
    fortran_int_t info(0);
    LAPACK_ZGGES( &jobvsl, &jobvsr, &sort, selctg, &n, a, &lda, b, &ldb,
            &sdim, alpha, beta, vsl, &ldvsl, vsr, &ldvsr, work, &lwork, rwork,
            bwork, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to gges.
//
template< typename Value, typename Enable = void >
struct gges_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct gges_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
            typename VectorALPHAI, typename VectorBETA, typename MatrixVSL,
            typename MatrixVSR, typename WORK, typename BWORK >
    static std::ptrdiff_t invoke( const char jobvsl, const char jobvsr,
            const char sort, external_fp selctg, MatrixA& a, MatrixB& b,
            fortran_int_t& sdim, VectorALPHAR& alphar,
            VectorALPHAI& alphai, VectorBETA& beta, MatrixVSL& vsl,
            MatrixVSR& vsr, detail::workspace2< WORK, BWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVSL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVSR >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorALPHAR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorALPHAI >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorBETA >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVSL >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVSR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorALPHAR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorALPHAI >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorBETA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVSL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVSR >::value) );
        BOOST_ASSERT( bindings::size(alphai) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(alphar) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(work.select(fortran_bool_t())) >=
                min_size_bwork( bindings::size_column(a), sort ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(vsl) == 1 ||
                bindings::stride_minor(vsl) == 1 );
        BOOST_ASSERT( bindings::size_minor(vsr) == 1 ||
                bindings::stride_minor(vsr) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( jobvsl == 'N' || jobvsl == 'V' );
        BOOST_ASSERT( jobvsr == 'N' || jobvsr == 'V' );
        BOOST_ASSERT( sort == 'N' || sort == 'S' );
        return detail::gges( jobvsl, jobvsr, sort, selctg,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), sdim,
                bindings::begin_value(alphar), bindings::begin_value(alphai),
                bindings::begin_value(beta), bindings::begin_value(vsl),
                bindings::stride_major(vsl), bindings::begin_value(vsr),
                bindings::stride_major(vsr),
                bindings::begin_value(work.select(real_type())),
                bindings::size(work.select(real_type())),
                bindings::begin_value(work.select(fortran_bool_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
            typename VectorALPHAI, typename VectorBETA, typename MatrixVSL,
            typename MatrixVSR >
    static std::ptrdiff_t invoke( const char jobvsl, const char jobvsr,
            const char sort, external_fp selctg, MatrixA& a, MatrixB& b,
            fortran_int_t& sdim, VectorALPHAR& alphar,
            VectorALPHAI& alphai, VectorBETA& beta, MatrixVSL& vsl,
            MatrixVSR& vsr, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< real_type > tmp_work( min_size_work(
                bindings::size_column(a) ) );
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                bindings::size_column(a), sort ) );
        return invoke( jobvsl, jobvsr, sort, selctg, a, b, sdim, alphar,
                alphai, beta, vsl, vsr, workspace( tmp_work, tmp_bwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
            typename VectorALPHAI, typename VectorBETA, typename MatrixVSL,
            typename MatrixVSR >
    static std::ptrdiff_t invoke( const char jobvsl, const char jobvsr,
            const char sort, external_fp selctg, MatrixA& a, MatrixB& b,
            fortran_int_t& sdim, VectorALPHAR& alphar,
            VectorALPHAI& alphai, VectorBETA& beta, MatrixVSL& vsl,
            MatrixVSR& vsr, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        real_type opt_size_work;
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                bindings::size_column(a), sort ) );
        detail::gges( jobvsl, jobvsr, sort, selctg,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), sdim,
                bindings::begin_value(alphar), bindings::begin_value(alphai),
                bindings::begin_value(beta), bindings::begin_value(vsl),
                bindings::stride_major(vsl), bindings::begin_value(vsr),
                bindings::stride_major(vsr), &opt_size_work, -1,
                bindings::begin_value(tmp_bwork) );
        bindings::detail::array< real_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( jobvsl, jobvsr, sort, selctg, a, b, sdim, alphar,
                alphai, beta, vsl, vsr, workspace( tmp_work, tmp_bwork ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        if ( n == 0 ) {
            return 1;
        } else {
            return std::max< std::ptrdiff_t >( 8*n, 6*n + 16 );
        }
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array bwork.
    //
    static std::ptrdiff_t min_size_bwork( const std::ptrdiff_t n,
            const char sort ) {
        if ( sort == 'N' )
            return 0;
        else
            return n;
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct gges_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHA,
            typename VectorBETA, typename MatrixVSL, typename MatrixVSR,
            typename WORK, typename RWORK, typename BWORK >
    static std::ptrdiff_t invoke( const char jobvsl, const char jobvsr,
            const char sort, external_fp selctg, MatrixA& a, MatrixB& b,
            fortran_int_t& sdim, VectorALPHA& alpha, VectorBETA& beta,
            MatrixVSL& vsl, MatrixVSR& vsr, detail::workspace3< WORK, RWORK,
            BWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVSL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixVSR >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorALPHA >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorBETA >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVSL >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixVSR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorALPHA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorBETA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVSL >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixVSR >::value) );
        BOOST_ASSERT( bindings::size(alpha) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(beta) >= bindings::size_column(a) );
        BOOST_ASSERT( bindings::size(work.select(fortran_bool_t())) >=
                min_size_bwork( bindings::size_column(a), sort ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_rwork( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_work( bindings::size_column(a) ));
        BOOST_ASSERT( bindings::size_column(a) >= 0 );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(vsl) == 1 ||
                bindings::stride_minor(vsl) == 1 );
        BOOST_ASSERT( bindings::size_minor(vsr) == 1 ||
                bindings::stride_minor(vsr) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(a)) );
        BOOST_ASSERT( jobvsl == 'N' || jobvsl == 'V' );
        BOOST_ASSERT( jobvsr == 'N' || jobvsr == 'V' );
        BOOST_ASSERT( sort == 'N' || sort == 'S' );
        return detail::gges( jobvsl, jobvsr, sort, selctg,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), sdim, bindings::begin_value(alpha),
                bindings::begin_value(beta), bindings::begin_value(vsl),
                bindings::stride_major(vsl), bindings::begin_value(vsr),
                bindings::stride_major(vsr),
                bindings::begin_value(work.select(value_type())),
                bindings::size(work.select(value_type())),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(fortran_bool_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHA,
            typename VectorBETA, typename MatrixVSL, typename MatrixVSR >
    static std::ptrdiff_t invoke( const char jobvsl, const char jobvsr,
            const char sort, external_fp selctg, MatrixA& a, MatrixB& b,
            fortran_int_t& sdim, VectorALPHA& alpha, VectorBETA& beta,
            MatrixVSL& vsl, MatrixVSR& vsr, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< value_type > tmp_work( min_size_work(
                bindings::size_column(a) ) );
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork(
                bindings::size_column(a) ) );
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                bindings::size_column(a), sort ) );
        return invoke( jobvsl, jobvsr, sort, selctg, a, b, sdim, alpha, beta,
                vsl, vsr, workspace( tmp_work, tmp_rwork, tmp_bwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename VectorALPHA,
            typename VectorBETA, typename MatrixVSL, typename MatrixVSR >
    static std::ptrdiff_t invoke( const char jobvsl, const char jobvsr,
            const char sort, external_fp selctg, MatrixA& a, MatrixB& b,
            fortran_int_t& sdim, VectorALPHA& alpha, VectorBETA& beta,
            MatrixVSL& vsl, MatrixVSR& vsr, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        value_type opt_size_work;
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork(
                bindings::size_column(a) ) );
        bindings::detail::array< fortran_bool_t > tmp_bwork( min_size_bwork(
                bindings::size_column(a), sort ) );
        detail::gges( jobvsl, jobvsr, sort, selctg,
                bindings::size_column(a), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), sdim, bindings::begin_value(alpha),
                bindings::begin_value(beta), bindings::begin_value(vsl),
                bindings::stride_major(vsl), bindings::begin_value(vsr),
                bindings::stride_major(vsr), &opt_size_work, -1,
                bindings::begin_value(tmp_rwork),
                bindings::begin_value(tmp_bwork) );
        bindings::detail::array< value_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( jobvsl, jobvsr, sort, selctg, a, b, sdim, alpha, beta,
                vsl, vsr, workspace( tmp_work, tmp_rwork, tmp_bwork ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return std::max< std::ptrdiff_t >( 1, 2*n );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array rwork.
    //
    static std::ptrdiff_t min_size_rwork( const std::ptrdiff_t n ) {
        return 8*n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array bwork.
    //
    static std::ptrdiff_t min_size_bwork( const std::ptrdiff_t n,
            const char sort ) {
        if ( sort == 'N' )
            return 0;
        else
            return n;
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the gges_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for gges. Its overload differs for
// * User-defined workspace
//
template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
        typename VectorALPHAI, typename VectorBETA, typename MatrixVSL,
        typename MatrixVSR, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
gges( const char jobvsl, const char jobvsr, const char sort,
        external_fp selctg, MatrixA& a, MatrixB& b, fortran_int_t& sdim,
        VectorALPHAR& alphar, VectorALPHAI& alphai, VectorBETA& beta,
        MatrixVSL& vsl, MatrixVSR& vsr, Workspace work ) {
    return gges_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobvsl, jobvsr, sort, selctg, a, b,
            sdim, alphar, alphai, beta, vsl, vsr, work );
}

//
// Overloaded function for gges. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixA, typename MatrixB, typename VectorALPHAR,
        typename VectorALPHAI, typename VectorBETA, typename MatrixVSL,
        typename MatrixVSR >
inline typename boost::disable_if< detail::is_workspace< MatrixVSR >,
        std::ptrdiff_t >::type
gges( const char jobvsl, const char jobvsr, const char sort,
        external_fp selctg, MatrixA& a, MatrixB& b, fortran_int_t& sdim,
        VectorALPHAR& alphar, VectorALPHAI& alphai, VectorBETA& beta,
        MatrixVSL& vsl, MatrixVSR& vsr ) {
    return gges_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobvsl, jobvsr, sort, selctg, a, b,
            sdim, alphar, alphai, beta, vsl, vsr, optimal_workspace() );
}

//
// Overloaded function for gges. Its overload differs for
// * User-defined workspace
//
template< typename MatrixA, typename MatrixB, typename VectorALPHA,
        typename VectorBETA, typename MatrixVSL, typename MatrixVSR,
        typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
gges( const char jobvsl, const char jobvsr, const char sort,
        external_fp selctg, MatrixA& a, MatrixB& b, fortran_int_t& sdim,
        VectorALPHA& alpha, VectorBETA& beta, MatrixVSL& vsl, MatrixVSR& vsr,
        Workspace work ) {
    return gges_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobvsl, jobvsr, sort, selctg, a, b,
            sdim, alpha, beta, vsl, vsr, work );
}

//
// Overloaded function for gges. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixA, typename MatrixB, typename VectorALPHA,
        typename VectorBETA, typename MatrixVSL, typename MatrixVSR >
inline typename boost::disable_if< detail::is_workspace< MatrixVSR >,
        std::ptrdiff_t >::type
gges( const char jobvsl, const char jobvsr, const char sort,
        external_fp selctg, MatrixA& a, MatrixB& b, fortran_int_t& sdim,
        VectorALPHA& alpha, VectorBETA& beta, MatrixVSL& vsl,
        MatrixVSR& vsr ) {
    return gges_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( jobvsl, jobvsr, sort, selctg, a, b,
            sdim, alpha, beta, vsl, vsr, optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


/*
 * outlier_rejector.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/background/simple/outlier_rejector.h>
#include <dials/algorithms/background/simple/normal_outlier_rejector.h>
#include <dials/algorithms/background/simple/truncated_outlier_rejector.h>
#include <dials/algorithms/background/simple/nsigma_outlier_rejector.h>
#include <dials/algorithms/background/simple/mosflm_outlier_rejector.h>
#include <dials/algorithms/background/simple/tukey_outlier_rejector.h>

namespace dials { namespace algorithms { namespace background { namespace boost_python {

  using namespace boost::python;

  template <typename FloatType>
  bool is_normally_distributed_wrapper(const af::const_ref<FloatType> &data,
                                       double n_sigma) {
    if (n_sigma <= 0) {
      return is_normally_distributed(data);
    }
    return is_normally_distributed(data, n_sigma);
  }

  template <typename FloatType>
  void normal_discriminator_suite() {
    def("maximum_n_sigma", &maximum_n_sigma<FloatType>, (boost::python::arg("data")));

    def("minimum_n_sigma", &minimum_n_sigma<FloatType>, (boost::python::arg("data")));

    def("absolute_maximum_n_sigma",
        &maximum_n_sigma<FloatType>,
        (boost::python::arg("data")));

    def("is_normally_distributed",
        &is_normally_distributed_wrapper<FloatType>,
        (boost::python::arg("data"), boost::python::arg("n_sigma") = -1));
  }

  void export_outlier_rejector() {
    def("normal_expected_n_sigma",
        &normal_expected_n_sigma,
        (boost::python::arg("n_obs")));

    normal_discriminator_suite<float>();
    normal_discriminator_suite<double>();

    class_<OutlierRejector, boost::noncopyable>("OutlierRejector", no_init)
      .def("__call__",
           &OutlierRejector::mark,
           (boost::python::arg("data"), boost::python::arg("mask")));

    class_<TruncatedOutlierRejector, bases<OutlierRejector> >(
      "TruncatedOutlierRejector", no_init)
      .def(init<double, double>(
        (boost::python::arg("lower") = 0.01, boost::python::arg("upper") = 0.01)));

    class_<NSigmaOutlierRejector, bases<OutlierRejector> >("NSigmaOutlierRejector",
                                                           no_init)
      .def(init<double, double>(
        (boost::python::arg("lower") = 3.0, boost::python::arg("upper") = 3.0)));

    class_<NormalOutlierRejector, bases<OutlierRejector> >("NormalOutlierRejector",
                                                           no_init)
      .def(init<std::size_t>((boost::python::arg("min_data") = 10)));

    class_<MosflmOutlierRejector, bases<OutlierRejector> >("MosflmOutlierRejector",
                                                           no_init)
      .def(init<double, double>(
        (boost::python::arg("fraction") = 1.0, boost::python::arg("n_sigma") = 3)));

    class_<TukeyOutlierRejector, bases<OutlierRejector> >("TukeyOutlierRejector",
                                                          no_init)
      .def(init<double, double>(
        (boost::python::arg("lower") = 1.5, boost::python::arg("upper") = 1.5)));
  }

}}}}  // namespace dials::algorithms::background::boost_python


#pragma once

#include <crest/basis/homogenized_basis.hpp>

#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>

#include <memory>

namespace crest
{
    namespace detail
    {

        template <
                typename _Scalar,
                typename StiffnessSolver,
                typename StiffnessMatrixType = Eigen::SparseMatrix<_Scalar>,
                typename InterpolatorMatrixType = Eigen::SparseMatrix<_Scalar> >
        class SchurComplement
                : public Eigen::EigenBase<SchurComplement<_Scalar, StiffnessSolver, InterpolatorMatrixType>>
        {
        public:
            typedef _Scalar Scalar;
            typedef Scalar RealScalar;
            typedef int StorageIndex;

            enum
            {
                ColsAtCompileTime = Eigen::Dynamic,
                MaxColsAtCompileTime = Eigen::Dynamic,
                IsRowMajor = false
            };

            SchurComplement() : _stiffness_solver(nullptr), _A(nullptr), _I_H(nullptr) {}

            Eigen::Index rows() const { return _I_H->rows(); }
            Eigen::Index cols() const { return _I_H->rows(); }

            template <typename Rhs>
            Eigen::Product<SchurComplement<Scalar, StiffnessSolver>, Rhs, Eigen::AliasFreeProduct>
            operator*(const Eigen::MatrixBase<Rhs> & x) const
            {
                return Eigen::Product<
                        SchurComplement<Scalar, StiffnessSolver>,
                        Rhs,
                        Eigen::AliasFreeProduct>(*this, x.derived());
            }

            const InterpolatorMatrixType &  quasiInterpolator() const { return *_I_H; }
            const StiffnessMatrixType    &  stiffnessMatrix() const { return *_A; }
            const StiffnessSolver        &  stiffnessSolver() const { return *_stiffness_solver; }

            SchurComplement & withQuasiInterpolator(const InterpolatorMatrixType & I_H)
            {
                _I_H = &I_H;
                return *this;
            }

            SchurComplement & withStiffness(const StiffnessMatrixType & stiffness_matrix,
                                            const StiffnessSolver & stiffness_solver)
            {
                _A = &stiffness_matrix;
                _stiffness_solver = &stiffness_solver;
                return *this;
            }

            bool isInitialized() const
            {
                return _stiffness_solver != nullptr && _I_H != nullptr && _A != nullptr;
            }

        private:
            const StiffnessSolver           * _stiffness_solver;
            const StiffnessMatrixType       * _A;
            const InterpolatorMatrixType    * _I_H;
        };

        /**
         * Preconditioner for the Schur complement problem S y = b,
         * where S = I_H A^-1 I_H^T,
         * where I_H is a quasi-interpolation operator and A is an SPD stiffness matrix.
         *
         * The preconditioner is defined by the approximation
         * S_diag = I_H diag(A)^{-1} I_H^T.
         */
        template <typename Scalar>
        class DiagonalizedSchurComplementPreconditioner
        {
        public:
            DiagonalizedSchurComplementPreconditioner() {}

            template<typename MatrixType>
            DiagonalizedSchurComplementPreconditioner& analyzePattern(const MatrixType& ) { return *this; }

            template <
                    typename StiffnessSolver,
                    typename StiffnessMatrixType,
                    typename InterpolatorMatrixType>
            DiagonalizedSchurComplementPreconditioner& compute(
                    const SchurComplement<
                            Scalar,
                            StiffnessSolver,
                            StiffnessMatrixType,
                            InterpolatorMatrixType> & schur_complement)
            {
                _approx_stiffness_solver.compute(schur_complement.stiffnessMatrix());
                _diagonalized_schur.withStiffness(schur_complement.stiffnessMatrix(), _approx_stiffness_solver);
                _diagonalized_schur.withQuasiInterpolator(schur_complement.quasiInterpolator());
                _outer_solver.compute(_diagonalized_schur);
                return *this;
            }

            template<typename MatrixType>
            DiagonalizedSchurComplementPreconditioner& factorize(const MatrixType & mat) { return *this; }

            // Note: this copies the output, so it's not currently optimal. Can maybe use the Solve<> struct
            // instead? TODO
            template<typename Rhs>
            inline const Rhs solve(const Rhs& b) const
            {
                return _outer_solver.solve(b);
            }

            Eigen::ComputationInfo info() { return Eigen::Success; }

        private:
            Eigen::DiagonalPreconditioner<Scalar> _approx_stiffness_solver;
            SchurComplement<Scalar, Eigen::DiagonalPreconditioner<Scalar>> _diagonalized_schur;
            Eigen::ConjugateGradient<
                    SchurComplement<Scalar, Eigen::DiagonalPreconditioner<Scalar>>,
                    Eigen::Lower|Eigen::Upper,
                    Eigen::IdentityPreconditioner> _outer_solver;
        };

        template <typename Scalar>
        class SchurComplementCoarseStiffnessPreconditioner
        {
        public:
            SchurComplementCoarseStiffnessPreconditioner() {}

            template<typename MatrixType>
            SchurComplementCoarseStiffnessPreconditioner& analyzePattern(const MatrixType& ) { return *this; }

            template<typename MatrixType>
            SchurComplementCoarseStiffnessPreconditioner& factorize(const MatrixType&) { return *this; }

            template<typename MatrixType>
            SchurComplementCoarseStiffnessPreconditioner& compute(const MatrixType&) { return *this; }

            // Note: this copies the output, so it's not currently optimal. Can maybe use the Solve<> struct
            // instead? TODO
            template<typename Rhs>
            inline const Rhs solve(const Rhs& b) const
            {
                return _A_H * b;
            }

            Eigen::ComputationInfo info() { return Eigen::Success; }

            template <typename MatrixType>
            void setCoarseStiffnessMatrix(const MatrixType & A)
            {
                _A_H = A;
            }

        private:
            Eigen::SparseMatrix<Scalar> _A_H;
        };
    }

    template <typename Scalar>
    class SchurCorrectorSolver : public CorrectorSolver<Scalar>
    {
    public:
        virtual std::vector<Eigen::Triplet<Scalar>> resolve_element_correctors_for_patch(
                const BiscaleMesh<Scalar, int> & mesh,
                const std::vector<int> & coarse_patch_interior,
                const std::vector<int> & fine_patch_interior,
                const Eigen::SparseMatrix<Scalar> & global_coarse_stiffness,
                const Eigen::SparseMatrix<Scalar> & global_fine_stiffness,
                const Eigen::SparseMatrix<Scalar> & global_quasi_interpolator,
                int coarse_element) const override
        {
            const auto I_H = sparse_submatrix(global_quasi_interpolator,
                                              coarse_patch_interior,
                                              fine_patch_interior);
            const auto A_h = sparse_submatrix(global_fine_stiffness,
                                              fine_patch_interior,
                                              fine_patch_interior);
            const auto A_H = sparse_submatrix(global_coarse_stiffness,
                                              coarse_patch_interior,
                                              coarse_patch_interior);

            assert(A_h.rows() > 0);

            using Eigen::SparseMatrix;
            using Eigen::ConjugateGradient;
            using Eigen::SimplicialLDLT;
            using detail::SchurComplementCoarseStiffnessPreconditioner;

            // Define the type of the solver used to solve the "stiffness problem" Ax = b.
            typedef SimplicialLDLT<SparseMatrix<Scalar>> StiffnessSolver;

            // The SchurComplement uses the StiffnessSolver internally
            typedef detail::SchurComplement<Scalar, StiffnessSolver> SchurComplement;

            // Finally we define the type of solver used for the Schur complement
            typedef ConjugateGradient<
                    SchurComplement,
                    Eigen::Lower|Eigen::Upper,
                    SchurComplementCoarseStiffnessPreconditioner<Scalar>> SchurComplementSolver;

            std::vector<Eigen::Triplet<Scalar>> triplets;

            StiffnessSolver stiffness_solver(A_h);
            const auto schur_operator = SchurComplement()
                    .withQuasiInterpolator(I_H)
                    .withStiffness(A_h, stiffness_solver);
            SchurComplementSolver schur_solver;
            schur_solver.setTolerance(CorrectorSolver<Scalar>::iterative_tolerance());
            schur_solver.preconditioner().setCoarseStiffnessMatrix(A_H);
            schur_solver.compute(schur_operator);

            for (int i = 0; i < 3; ++i)
            {
                const auto b = this->local_rhs(mesh, fine_patch_interior, coarse_element, i);
                VectorX<Scalar> corrector(A_h.rows());

                if (I_H.rows() == 0)
                {
                    corrector = stiffness_solver.solve(b);
                } else
                {
                    const VectorX<Scalar> y = stiffness_solver.solve(b);
                    const VectorX<Scalar> kappa = schur_solver.solve(I_H * y);
                    corrector = stiffness_solver.solve(b - I_H.transpose() * kappa);
                }

                assert(static_cast<size_t>(corrector.rows()) == fine_patch_interior.size());
                const auto global_index = mesh.coarse_mesh().elements()[coarse_element].vertex_indices[i];
                for (size_t k = 0; k < fine_patch_interior.size(); ++k)
                {
                    const auto component = corrector(k);
                    // Due to rounding issues, some components that should perhaps be zero in exact arithmetic
                    // may be non-zero, and as such we might end up with a denser matrix than we should actually
                    // have. To prevent this, we introduce a threshold which determines whether to keep the entry.
                    // TODO: Make this threshold configurable?
                    if (std::abs(component) > 1e-12)
                    {
                        triplets.emplace_back(Eigen::Triplet<Scalar>(global_index, fine_patch_interior[k], component));
                    }
                }
            }

            return triplets;
        }
    };

}

/**
 * The code below is necessary for matrix-free iterative solvers in Eigen.
 * See https://eigen.tuxfamily.org/dox/group__MatrixfreeSolverExample.html
 */
namespace Eigen {
    namespace internal {
        // MatrixReplacement looks- a SparseMatrix, so let's inherits its traits:
        template <typename _Scalar, typename _StiffnessSolver>
        struct traits<crest::detail::SchurComplement<_Scalar, _StiffnessSolver>>
        {
            typedef _Scalar Scalar;
            typedef int StorageIndex;
            typedef Eigen::Sparse StorageKind;
            typedef Eigen::MatrixXpr XprKind;
            enum {
                RowsAtCompileTime = Dynamic,
                ColsAtCompileTime = Dynamic,
                MaxRowsAtCompileTime = Dynamic,
                MaxColsAtCompileTime = Dynamic,
                Flags = 0
            };
        };

        template<typename _Scalar, typename _StiffnessSolver, typename Rhs>
        struct generic_product_impl<crest::detail::SchurComplement<_Scalar, _StiffnessSolver>, Rhs, SparseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
                : generic_product_impl_base<crest::detail::SchurComplement<_Scalar, _StiffnessSolver>,
                        Rhs,
                        generic_product_impl<crest::detail::SchurComplement<_Scalar, _StiffnessSolver>, Rhs> >
        {
            typedef typename Product<crest::detail::SchurComplement<_Scalar, _StiffnessSolver>, Rhs>::Scalar Scalar;
            template<typename Dest>
            static void scaleAndAddTo(Dest& dst,
                                      const crest::detail::SchurComplement<_Scalar, _StiffnessSolver>& lhs,
                                      const Rhs& rhs,
                                      const Scalar& alpha)
            {
                // Apply the Schur complement S = I_H A^-1 I_H^T
                // to the vector x.

                // First solve
                // Ay = I_H^T x
                // such that y = A^1 I_H^T x,
                // then multiply by I_H
                assert(lhs.isInitialized() && "Must initialize SchurComplement stiffness solver "
                        "and quasi interpolator before using.");

                const auto & A = lhs.stiffnessSolver();
                const auto & I_H = lhs.quasiInterpolator();
                dst = alpha * I_H * A.solve(I_H.transpose() * rhs);
            }
        };
    }
}


#include <boost/test/unit_test.hpp>

#include <util/math.h>
#include <cmath>
#include <boost/random.hpp>

using namespace izenelib::util;

BOOST_AUTO_TEST_SUITE(t_math_util)

BOOST_AUTO_TEST_CASE(math_test)
{
    boost::mt19937 engine ;
    boost::uniform_int<> distribution(1,1000000);
    boost::variate_generator<boost::mt19937, boost::uniform_int<> > random(engine,distribution);

    int size = 10;
    for(int i = 0; i < size; ++i)
    {
        int r = random();
        std::cout<<log10(r)<<" "<<fastlog10(r)<<" "<<fasterlog10(r)<<std::endl;
    }
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <cmath>
#include <limits>
#include <vector>

namespace gil = boost::gil;

int main()
{
    const std::ptrdiff_t size = 256;
    gil::gray8_image_t buffer_image(size, size);
    auto buffer = gil::view(buffer_image);

    const std::ptrdiff_t radius = 64;
    const auto rasterizer = gil::trigonometric_circle_rasterizer{};
    std::vector<gil::point_t> circle_points(rasterizer.point_count(radius));
    rasterizer(radius, {128, 128}, circle_points.begin());
    for (const auto& point : circle_points)
    {
        buffer(point) = std::numeric_limits<gil::uint8_t>::max();
    }

    gil::write_view("circle.png", buffer, gil::png_tag{});
}


#include "../Solver_Parameters.hxx"

#include <boost/program_options.hpp>

boost::program_options::options_description Solver_Parameters::options()
{
  boost::program_options::options_description result("Solver parameters");
  // We set default parameters using El::BigFloat("1e-10",10)
  // rather than a straight double precision 1e-10 so that results
  // are more reproducible at high precision.  Using double
  // precision defaults results in differences of about 1e-15 in
  // primalObjective after one step.

  result.add_options()(
    "precision",
    boost::program_options::value<size_t>(&precision)->default_value(400),
    "The precision, in the number of bits, for numbers in the "
    "computation. "
    " This should be less than or equal to the precision used when "
    "preprocessing the XML input files with 'pvm2sdp'.  GMP will round "
    "this up to a multiple of 32 or 64, depending on the system.");
  result.add_options()(
    "findPrimalFeasible",
    boost::program_options::bool_switch(&find_primal_feasible)
      ->default_value(false),
    "Terminate once a primal feasible solution is found.");
  result.add_options()("findDualFeasible",
                       boost::program_options::bool_switch(&find_dual_feasible)
                         ->default_value(false),
                       "Terminate once a dual feasible solution is found.");
  result.add_options()(
    "detectPrimalFeasibleJump",
    boost::program_options::bool_switch(&detect_primal_feasible_jump)
      ->default_value(false),
    "Terminate if a primal-step of 1 is taken. This often indicates that "
    "a "
    "primal feasible solution would be found if the precision were high "
    "enough. Try increasing either primalErrorThreshold or precision "
    "and run from the latest checkpoint.");
  result.add_options()(
    "detectDualFeasibleJump",
    boost::program_options::bool_switch(&detect_dual_feasible_jump)
      ->default_value(false),
    "Terminate if a dual-step of 1 is taken. This often indicates that a "
    "dual feasible solution would be found if the precision were high "
    "enough. Try increasing either dualErrorThreshold or precision "
    "and run from the latest checkpoint.");
  result.add_options()(
    "maxIterations",
    boost::program_options::value<int64_t>(&max_iterations)->default_value(500),
    "Maximum number of iterations to run the solver.");
  result.add_options()("maxRuntime",
                       boost::program_options::value<int64_t>(&max_runtime)
                         ->default_value(std::numeric_limits<int64_t>::max()),
                       "Maximum amount of time to run the solver in seconds.");
  result.add_options()(
    "dualityGapThreshold",
    boost::program_options::value<El::BigFloat>(&duality_gap_threshold)
      ->default_value(El::BigFloat("1e-30", 10)),
    "Threshold for duality gap (roughly the difference in primal and dual "
    "objective) at which the solution is considered "
    "optimal. Corresponds to SDPA's epsilonStar.");
  result.add_options()(
    "primalErrorThreshold",
    boost::program_options::value<El::BigFloat>(&primal_error_threshold)
      ->default_value(El::BigFloat("1e-30", 10)),
    "Threshold for feasibility of the primal problem. Corresponds to "
    "SDPA's epsilonBar.");
  result.add_options()(
    "dualErrorThreshold",
    boost::program_options::value<El::BigFloat>(&dual_error_threshold)
      ->default_value(El::BigFloat("1e-30", 10)),
    "Threshold for feasibility of the dual problem. Corresponds to SDPA's "
    "epsilonBar.");
  result.add_options()(
    "initialMatrixScalePrimal",
    boost::program_options::value<El::BigFloat>(&initial_matrix_scale_primal)
      ->default_value(El::BigFloat("1e20", 10)),
    "The primal matrix X begins at initialMatrixScalePrimal times the "
    "identity matrix. Corresponds to SDPA's lambdaStar.");
  result.add_options()(
    "initialMatrixScaleDual",
    boost::program_options::value<El::BigFloat>(&initial_matrix_scale_dual)
      ->default_value(El::BigFloat("1e20", 10)),
    "The dual matrix Y begins at initialMatrixScaleDual times the "
    "identity matrix. Corresponds to SDPA's lambdaStar.");
  result.add_options()(
    "feasibleCenteringParameter",
    boost::program_options::value<El::BigFloat>(&feasible_centering_parameter)
      ->default_value(El::BigFloat("0.1", 10)),
    "Shrink the complementarity X Y by this factor when the primal and "
    "dual "
    "problems are feasible. Corresponds to SDPA's betaStar.");
  result.add_options()(
    "infeasibleCenteringParameter",
    boost::program_options::value<El::BigFloat>(
      &infeasible_centering_parameter)
      ->default_value(El::BigFloat("0.3", 10)),
    "Shrink the complementarity X Y by this factor when either the primal "
    "or dual problems are infeasible. Corresponds to SDPA's betaBar.");
  result.add_options()(
    "stepLengthReduction",
    boost::program_options::value<El::BigFloat>(&step_length_reduction)
      ->default_value(El::BigFloat("0.7", 10)),
    "Shrink each newton step by this factor (smaller means slower, more "
    "stable convergence). Corresponds to SDPA's gammaStar.");
  result.add_options()(
    "minPrimalStep",
    boost::program_options::value<El::BigFloat>(&min_primal_step)
      ->default_value(El::BigFloat(0)),
    "Terminate if the primal step size becomes smaller than this value.");
  result.add_options()(
    "minDualStep",
    boost::program_options::value<El::BigFloat>(&min_dual_step)
      ->default_value(El::BigFloat(0)),
    "Terminate if the dual step size becomes smaller than this value.");
  result.add_options()(
    "maxComplementarity",
    boost::program_options::value<El::BigFloat>(&max_complementarity)
      ->default_value(El::BigFloat("1e100", 10)),
    "Terminate if the complementarity mu = Tr(X Y)/dim(X) "
    "exceeds this value.");
  result.add_options()(
    "checkpointDir,c",
    boost::program_options::value<boost::filesystem::path>(&checkpoint_out),
    "Checkpoints are saved to this directory every checkpointInterval.  Set "
    "to the empty string to inhibit checkpoints.  Defaults to sdpDir with "
    "'.ck' extension.");
  result.add_options()(
    "initialCheckpointDir,i",
    boost::program_options::value<boost::filesystem::path>(&checkpoint_in),
    "The initial checkpoint directory to load. Defaults to "
    "checkpointDir.");
  result.add_options()(
    "checkpointInterval",
    boost::program_options::value<int64_t>(&checkpoint_interval)
      ->default_value(3600),
    "Save checkpoints to checkpointDir every checkpointInterval "
    "seconds.");

  return result;
}


#include <boost/random/subtract_with_carry.hpp>