#include "sev/acp_ros_conversions/conversions.h"

#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <limits>
#include <sstream>
#include <utility>

#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "minkindr_conversions/kindr_msg.h"
#include "std_msgs/Header.h"

#include "sev/acp/types.h"

#include <Eigen/Core>

namespace sev_conversions {

using Transformation = kindr::minimal::QuatTransformation;
using Position3D = kindr::minimal::Position;

// Conversion from rotation matrix to roll, pitch, yaw angles.
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
RotationMatrixToRollPitchYaw(const Eigen::MatrixBase<Derived>& rot) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3);
  Eigen::Matrix<typename Derived::Scalar, 3, 1> rpy;
  rpy(1, 0) =
      atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)));
  if (std::abs(cos(rpy(1, 0))) >
      static_cast<typename Derived::Scalar>(1.0e-12)) {
    rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0)));
    rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0)));
  } else if (sin(rpy(1, 0)) > static_cast<typename Derived::Scalar>(0)) {
    rpy(2, 0) = static_cast<typename Derived::Scalar>(0);
    rpy(0, 0) = atan2(rot(0, 1), rot(1, 1));
  } else {
    rpy(2, 0) = static_cast<typename Derived::Scalar>(0);
    rpy(0, 0) = -atan2(rot(0, 1), rot(1, 1));
  }
  return rpy;
}

// Conversion from roll, pitch, yaw to rotation matrix.
template <typename Derived>
inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
RollPitchYawToRotationMatrix(const Eigen::MatrixBase<Derived>& roll_pitch_yaw) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 1);
  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_x;
  typename Derived::Scalar zero = static_cast<typename Derived::Scalar>(0.);
  typename Derived::Scalar one = static_cast<typename Derived::Scalar>(1.);
  rotation_matrix_x << one, zero, zero, zero, cos(roll_pitch_yaw(0)),
      -sin(roll_pitch_yaw(0)), zero, sin(roll_pitch_yaw(0)),
      cos(roll_pitch_yaw(0));

  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_y;
  rotation_matrix_y << cos(roll_pitch_yaw(1)), zero, sin(roll_pitch_yaw(1)),
      zero, one, zero, -sin(roll_pitch_yaw(1)), 0, cos(roll_pitch_yaw(1));

  Eigen::Matrix<typename Derived::Scalar, 3, 3> rotation_matrix_z;
  rotation_matrix_z << cos(roll_pitch_yaw(2)), -sin(roll_pitch_yaw(2)), zero,
      sin(roll_pitch_yaw(2)), cos(roll_pitch_yaw(2)), zero, zero, zero, one;

  return rotation_matrix_z * rotation_matrix_y * rotation_matrix_x;
}

using TimePoint = std::chrono::time_point<std::chrono::system_clock>;
using Duration = std::chrono::nanoseconds;
constexpr double kNanosecondsToSeconds = 1e-9;
constexpr double kSecondsToNanoSeconds = 1e9;

inline TimePoint toTimePoint(const int64_t timestamp_ns) {
  // Making sure that conversion between TimePoint and int64_t comes at no
  // runtime cost.
  static_assert(std::is_same<TimePoint::rep, int64_t>::value, "");
  static_assert(
      std::is_same<
          std::chrono::system_clock::period, std::ratio<1, 1000000000>>::value,
      "");
  return TimePoint{std::chrono::nanoseconds{timestamp_ns}};
}

inline TimePoint toTimePoint(const ros::Time& ros_time) {
  const int64_t timestamp_ns = ros_time.toNSec();
  return toTimePoint(timestamp_ns);
}

inline int64_t toNs(const TimePoint timestamp) {
  return timestamp.time_since_epoch().count();
}

inline ros::Time toRosTime(const TimePoint& time_point) {
  const int64_t timestamp_ns = toNs(time_point);
  const int32_t ros_timestamp_sec = timestamp_ns * kNanosecondsToSeconds;
  const int32_t ros_timestamp_nsec =
      timestamp_ns % static_cast<int64_t>(kSecondsToNanoSeconds);
  return ros::Time(ros_timestamp_sec, ros_timestamp_nsec);
}

inline ros::Time convertNanosecondsInt64ToRosTime(
    const int64_t timestamp_nanoseconds) {
  return toRosTime(toTimePoint(timestamp_nanoseconds));
}

inline int64_t convertRosTimeToNanosecondsInt64(
    const ros::Time& ros_timestamp) {
  return toNs(toTimePoint(ros_timestamp));
}
}  // namespace sev_conversions

namespace sev::acp::udp_ros_bridge {
constexpr double M_TO_MM = 1000;
#ifndef M_PI
#error "M_PI is not defined (issue with _USE_MATH_DEFINES?)"
#endif
constexpr double RAD_TO_CDEG = 100 * 180 / M_PI;

std_msgs::Header convertToRosStamped(sev::acp::MessageHeader const& mh) {
  std_msgs::Header retval;
  retval.seq = mh.seq;
  retval.stamp =
      sev_conversions::convertNanosecondsInt64ToRosTime(mh.timestamp);
  return retval;
}

sev::acp::MessageHeader convertToAcp(const std_msgs::Header& header) {
  sev::acp::MessageHeader mh;
  mh.seq = header.seq;
  mh.timestamp =
      sev_conversions::convertRosTimeToNanosecondsInt64(header.stamp);
  return mh;
}

auto getRotationMatrix(const geometry_msgs::Pose& ros_pose) {
  // Protection against invalid data.
  const auto compute_sq_norm = [](const auto& o) {
    return o.x * o.x + o.y * o.y + o.z * o.z + o.w * o.w;
  };
  const auto norm_sq = compute_sq_norm(ros_pose.orientation);

  // Copied from minkindr.
  // The decision boundary should be at the (binary represented) same number,
  // therefore using abs() would lead to slightly different decision than in
  // kindr's CHECK.
  if ((norm_sq > (1. - 1.e-4)) && (norm_sq < (1. + 1.e-4))) {
    sev_conversions::Transformation T_M_I;
    tf::poseMsgToKindr(ros_pose, &T_M_I);
    return T_M_I.getRotation().getRotationMatrix();
  } else {
    std::stringstream ss;
    ss << "Encountered non-normalized orientation during conversion:\n";
    ss << ros_pose.orientation;
    throw ConversionError(ss.str());
  }
}

geometry_msgs::Pose poseFromMatrix(
    const std::array<double, 3>& position_array,
    const sev_conversions::Transformation::RotationMatrix& rotation_matrix) {
  // TODO(pseyfert): Simplify until poseKindrToMsg.
  if (sev_conversions::Transformation::Rotation::isValidRotationMatrix(
          rotation_matrix)) {
    sev_conversions::Position3D position(&position_array[0]);
    const auto quat_trafo = sev_conversions::Transformation(
        sev_conversions::Transformation::Rotation{std::move(rotation_matrix)},
        std::move(position));
    geometry_msgs::Pose ros_pose;
    tf::poseKindrToMsg(std::move(quat_trafo), &ros_pose);
    return ros_pose;
  } else {
    std::stringstream ss;
    ss << "Encountered invalid rotation during conversion:\n";
    ss << rotation_matrix;
    throw ConversionError(ss.str());
  }
}

geometry_msgs::Pose poseFromMatrix(
    const std::array<double, 3>& position_array, const sev::acp::Rot3& R_m_r) {
  // TODO(pseyfert): Simplify until poseKindrToMsg.
  sev_conversions::Transformation::RotationMatrix rotation_matrix;
  for (std::size_t i = 0; i < 9; ++i) {
    rotation_matrix.data()[i] = R_m_r.mat[i];
  }
  return poseFromMatrix(position_array, rotation_matrix);
}

std::tuple<sev::acp::Pose> convertToAcp(
    const geometry_msgs::PoseStamped& ros_message) {
  sev::acp::Pose out_pose;
  const auto& ros_pose = ros_message.pose;
  out_pose.position[0] = ros_pose.position.x;
  out_pose.position[1] = ros_pose.position.y;
  out_pose.position[2] = ros_pose.position.z;

  for (std::size_t i = 0; i < 3; ++i) {
    out_pose.velocity[i] = std::numeric_limits<double>::quiet_NaN();
  }

  const auto rotation_matrix = getRotationMatrix(ros_pose);

  for (std::size_t i = 0; i < 9; ++i) {
    out_pose.R_m_r.mat[i] = rotation_matrix.data()[i];
  }
  out_pose.yaw =
      sev_conversions::RotationMatrixToRollPitchYaw(rotation_matrix)[2];

  out_pose.header = convertToAcp(ros_message.header);
  return std::make_tuple(std::move(out_pose));
}

std::tuple<geometry_msgs::PoseStamped> convertToRosStamped(
    const sev::acp::Pose& acp_pose) {
  geometry_msgs::PoseStamped ros_pose;

  std::array<double, 3> position;
  for (std::size_t i = 0; i < position.size(); i++) {
    position[i] = acp_pose.position[i];
  }
  ros_pose.pose = poseFromMatrix(position, acp_pose.R_m_r);
  // TODO(pseyfert): acp_pose.velocity is ignored.

  ros_pose.header = convertToRosStamped(acp_pose.header);
  return std::make_tuple(std::move(ros_pose));
}

std::tuple<sev::acp::WheelOdometryIntegrated, sev::acp::WheelOdometryInt>
convertToAcp(const geometry_msgs::TwistStamped& ros_message) {
  const auto& linear = ros_message.twist.linear;
  const auto& angular = ros_message.twist.angular;
  const auto header = convertToAcp(ros_message.header);

  const auto woi_int = [&]() {
    sev::acp::WheelOdometryInt woi;
    const auto buffer =
        std::array<int, 6>{linear.x * M_TO_MM,      linear.y * M_TO_MM,
                           linear.z * M_TO_MM,      angular.x * RAD_TO_CDEG,
                           angular.y * RAD_TO_CDEG, angular.z * RAD_TO_CDEG};
    // Can't use std::copy because woi.twist is packed and STL algorithms don't
    // handle packed structs.
    for (std::size_t i = 0; i < buffer.size(); ++i) {
      woi.twist[i] = buffer[i];
    }
    woi.header = header;
    return woi;
  }();
  const auto woi_integrated = [&]() {
    sev::acp::WheelOdometryIntegrated woi;
    const auto buffer = std::array{linear.x,  linear.y,  linear.z,
                                   angular.x, angular.y, angular.z};
    // Can't use std::copy because woi.twist is packed and STL algorithms don't
    // handle packed structs.
    for (std::size_t i = 0; i < buffer.size(); ++i) {
      woi.twist[i] = buffer[i];
    }
    woi.header = header;
    return woi;
  }();
  return {std::move(woi_integrated), std::move(woi_int)};
}

std::tuple<geometry_msgs::TwistStamped> convertToRosStamped(
    const sev::acp::WheelOdometryIntegrated& woi) {
  geometry_msgs::TwistStamped ros_message;

  std::size_t i = 0;
  ros_message.twist.linear.x = woi.twist[i++];
  ros_message.twist.linear.y = woi.twist[i++];
  ros_message.twist.linear.z = woi.twist[i++];
  ros_message.twist.angular.x = woi.twist[i++];
  ros_message.twist.angular.y = woi.twist[i++];
  ros_message.twist.angular.z = woi.twist[i++];

  ros_message.header = convertToRosStamped(woi.header);
  return std::make_tuple(std::move(ros_message));
}

std::tuple<sev::acp::OperationState> convertToAcp(
    const state_machine_msgs::State& status) {
  sev::acp::OperationState acp;
  acp.task = status.state_int;
  acp.stage = status.substate_int;
  // TODO(pseyfert): Fill with meaningful data.
  for (std::size_t i = 0; i < 16; ++i) {
    acp.task_uuid[i] = 0;
  }
  acp.optimization_progress = -1;
  acp.header = convertToAcp(status.header);
  return std::make_tuple(std::move(acp));
}

std::tuple<state_machine_msgs::State> convertToRosStamped(
    const sev::acp::OperationState& os) {
  state_machine_msgs::State state;
  state.state_int = os.task;
  state.substate_int = os.stage;
  // TODO(pseyfert): Fill with meaningful data.
  // state.state =
  // state.substate =
  state.header = convertToRosStamped(os.header);
  return std::make_tuple(std::move(state));
}

// sev::acp::Notifications
std::tuple<sev::acp::Notifications> convertToAcp(
    const state_machine_msgs::Status& status) {
  sev::acp::Notifications acp;
  acp.module_id = status.module;
  acp.status_code = status.status_code;
  acp.severity = status.severity;
  // TODO(pseyfert): Fill with meaningful data.
  acp.logId = -1;
  acp.logEntryId = -1;
  acp.header = convertToAcp(status.header);
  return std::make_tuple(std::move(acp));
}

std::tuple<state_machine_msgs::Status> convertToRosStamped(
    const sev::acp::Notifications& notifications) {
  state_machine_msgs::Status status;
  status.severity = notifications.severity;
  status.module = notifications.module_id;
  status.status_code = notifications.status_code;
  // TODO(pseyfert): Fill with meaningful data.
  // status.status_text =
  status.header = convertToRosStamped(notifications.header);
  return std::make_tuple(std::move(status));
}

std::tuple<atlas_msgs::PositioningUpdate> convertToRosStamped(
    const sev::acp::PoseInt& acp_pose) {
  atlas_msgs::PositioningUpdate retval;
  retval.header = convertToRosStamped(acp_pose.header);
  std::array<double, 3> position;
  for (std::size_t i = 0; i < 3; ++i) {
    position[i] = acp_pose.position_mm[i] / M_TO_MM;
  }
  Eigen::Matrix<double, 3, 1> angles{
      acp_pose.roll_cdeg / RAD_TO_CDEG,
      acp_pose.pitch_cdeg / RAD_TO_CDEG,
      acp_pose.yaw_cdeg / RAD_TO_CDEG,
  };
  const auto rotation_matrix =
      sev_conversions::RollPitchYawToRotationMatrix(angles);
  retval.pose = poseFromMatrix(position, rotation_matrix);
  retval.quality.level = acp_pose.quality;
  retval.is_relocalization_event = acp_pose.relocalization;
  return std::make_tuple(std::move(retval));
}

std::tuple<sev::acp::PoseFloat, sev::acp::PoseInt> convertToAcp(
    const atlas_msgs::PositioningUpdate& ros_msg) {
  const auto rotation_matrix = getRotationMatrix(ros_msg.pose);
  const auto angles =
      sev_conversions::RotationMatrixToRollPitchYaw(rotation_matrix);
  const auto header = convertToAcp(ros_msg.header);

  const auto pose_int = [&]() {
    sev::acp::PoseInt retval;

    retval.position_mm[0] = ros_msg.pose.position.x * M_TO_MM;
    retval.position_mm[1] = ros_msg.pose.position.y * M_TO_MM;
    retval.position_mm[2] = ros_msg.pose.position.z * M_TO_MM;

    for (std::size_t i = 0; i < 3; ++i) {
      // TODO(pseyfert): Fill with meaningful data.
      retval.velocity_mmps[i] = 0;
    }
    retval.quality = ros_msg.quality.level;

    retval.roll_cdeg = angles[0] * RAD_TO_CDEG;
    retval.pitch_cdeg = angles[1] * RAD_TO_CDEG;
    retval.yaw_cdeg = angles[2] * RAD_TO_CDEG;

    retval.relocalization = ros_msg.is_relocalization_event ? 1 : 0;

    retval.header = header;
    return retval;
  }();

  const auto pose_float = [&]() {
    sev::acp::PoseFloat retval;

    retval.position[0] = ros_msg.pose.position.x;
    retval.position[1] = ros_msg.pose.position.y;
    retval.position[2] = ros_msg.pose.position.z;

    for (std::size_t i = 0; i < 3; ++i) {
      retval.velocity[i] = std::numeric_limits<double>::quiet_NaN();
    }
    retval.quality = ros_msg.quality.level;

    for (std::size_t i = 0; i < 9; ++i) {
      retval.rot.mat[i] = rotation_matrix.data()[i];
    }
    retval.roll = angles[0];
    retval.pitch = angles[1];
    retval.yaw = angles[2];

    retval.relocalization = ros_msg.is_relocalization_event ? 1 : 0;

    retval.header = header;
    return retval;
  }();

  return {std::move(pose_float), std::move(pose_int)};
}

std::tuple<geometry_msgs::PoseStamped, atlas_msgs::PositioningUpdate>
convertToRosStamped(const sev::acp::PoseFloat& acp_pose) {
  atlas_msgs::PositioningUpdate positioning_update;
  geometry_msgs::PoseStamped ros_pose;

  std::array<double, 3> position;
  for (std::size_t i = 0; i < position.size(); i++) {
    position[i] = acp_pose.position[i];
  }
  positioning_update.pose = poseFromMatrix(position, acp_pose.rot);
  ros_pose.pose = poseFromMatrix(position, acp_pose.rot);

  positioning_update.header = convertToRosStamped(acp_pose.header);
  ros_pose.header = convertToRosStamped(acp_pose.header);
  positioning_update.quality.level = acp_pose.quality;
  positioning_update.is_relocalization_event = acp_pose.relocalization;
  return std::make_tuple(std::move(ros_pose), std::move(positioning_update));
}

std::tuple<geometry_msgs::TwistStamped> convertToRosStamped(
    const sev::acp::WheelOdometryInt& woi) {
  geometry_msgs::TwistStamped ros_message;

  std::size_t i = 0;
  ros_message.twist.linear.x = woi.twist[i++] / M_TO_MM;
  ros_message.twist.linear.y = woi.twist[i++] / M_TO_MM;
  ros_message.twist.linear.z = woi.twist[i++] / M_TO_MM;
  ros_message.twist.angular.x = woi.twist[i++] / RAD_TO_CDEG;
  ros_message.twist.angular.y = woi.twist[i++] / RAD_TO_CDEG;
  ros_message.twist.angular.z = woi.twist[i++] / RAD_TO_CDEG;

  ros_message.header = convertToRosStamped(woi.header);
  return std::make_tuple(std::move(ros_message));
}
}  // namespace sev::acp::udp_ros_bridge


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "clipped_read_fraction.hpp"

#include <cassert>

#include <boost/variant.hpp>

#include "basics/aligned_read.hpp"
#include "io/variant/vcf_record.hpp"
#include "utils/read_stats.hpp"
#include "../facets/overlapping_reads.hpp"

namespace octopus { namespace csr {

const std::string ClippedReadFraction::name_ = "CRF";

std::unique_ptr<Measure> ClippedReadFraction::do_clone() const
{
    return std::make_unique<ClippedReadFraction>(*this);
}

Measure::ValueType ClippedReadFraction::get_value_type() const
{
    return double {};
}

namespace {

auto clip_fraction(const AlignedRead& read) noexcept
{
    assert(sequence_size(read) > 0);
    return static_cast<double>(total_clip_size(read)) / sequence_size(read);
}

bool is_significantly_clipped(const AlignedRead& read) noexcept
{
    return is_soft_clipped(read) && clip_fraction(read) > 0.25;
}

} // namespace

Measure::ResultType ClippedReadFraction::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& reads = get_value<OverlappingReads>(facets.at("OverlappingReads"));
    unsigned num_reads {0}, num_soft_clipped_reads {0};
    for (const auto& p : reads) {
        for (const auto& read : overlap_range(p.second, call)) {
            if (is_significantly_clipped(read)) ++num_soft_clipped_reads;
            ++num_reads;
        }
    }
    Optional<ValueType> result {};
    if (num_reads > 0) {
        result = static_cast<double>(num_soft_clipped_reads) / num_reads;
    }
    return result;
}

Measure::ResultCardinality ClippedReadFraction::do_cardinality() const noexcept
{
    return ResultCardinality::one;
}

const std::string& ClippedReadFraction::do_name() const
{
    return name_;
}

std::string ClippedReadFraction::do_describe() const
{
    return "Fraction of clipped reads covering the call";
}

std::vector<std::string> ClippedReadFraction::do_requirements() const
{
    return {"OverlappingReads"};
}

} // namespace csr
} // namespace octopus


#include <iostream>                  // for std::cout
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each
#include <boost/graph/graph_traits.hpp> // for creation of descriptors vertex and edges.
#include <boost/graph/adjacency_list.hpp> //for usage of adjacency list
#include <boost/graph/graphml.hpp>
#include <boost/graph/connected_components.hpp>
#include <ilcplex/ilocplex.h>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/program_options.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/exception/all.hpp>
#include <exception>
#include <vector>
ILOSTLBEGIN //initialization make vs work properly

using namespace boost;
namespace po = boost::program_options;
//basic definitions
typedef IloArray<IloNumVarArray> IloVarMatrix;

typedef dynamic_bitset<> db;
typedef std::map<int, std::size_t> rank_t; // => order on Element
typedef std::map<int, int> parent_t;

template <typename EdgeColorMap, typename ValidColorsMap>
struct valid_edge_color {
	valid_edge_color() { }
	valid_edge_color(EdgeColorMap color, ValidColorsMap v_colors) : m_color(color), v_map(v_colors) { }
	template <typename Edge>
	bool operator()(const Edge& e) const {
		return v_map.test(get(m_color, e));
	}
	EdgeColorMap m_color;
	ValidColorsMap v_map;
};

template<class Graph, class Mask>
void print_filtered_graph(Graph &g, Mask valid) { //pay atention to the position of the bits and the colors positions in array
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef typename boost::dynamic_bitset<> db;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;

	valid_edge_color<EdgeColorMap, Mask> filter(get(edge_color, g), valid);
	fg tg(g, filter);
	print_edges(edges(tg).first, edges(tg).second, tg);
}
//template function to print edges.
template<class EdgeIter, class Graph>
void print_edges(EdgeIter first, EdgeIter last, const Graph& G) {
	typedef typename property_map<Graph, edge_color_t>::const_type ColorMap;
	ColorMap colors = get(edge_color, G);
	//make color type generic
	//typedef typename property_traits<ColorMap>::value_type ColorType;
	//ColorType edge_color;
	for (auto it = first; it != last; ++it) {
		std::cout << "Edge: " << "(" << source(*it, G) << "," << target(*it, G) << ") " << " Color: " << colors[*it] << "\n";
		std::cout << "Edge: " << "(" << target(*it, G) << "," << source(*it, G) << ") " << " Color: " << colors[*it] << "\n";
	}
	std::cout << " Number of vertex: " << num_vertices(G) << std::endl;
	std::cout << " Number of edges: " << num_edges(G) << std::endl;
	std::vector<int> components(num_vertices(G));
	int num = connected_components(G, &components[0]);
	std::vector<int>::size_type i;
	std::cout << "Total number of components: " << num << std::endl;
	for (i = 0; i != components.size(); ++i)
		std::cout << "Vertex " << i << " is in component " << components[i] << std::endl;
	std::cout << std::endl;
}
template<class Graph, class Mask>
int get_components(Graph &g, Mask &m, vector<int> &components) {
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef typename boost::dynamic_bitset<> db;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	valid_edge_color<EdgeColorMap, Mask> filter(get(edge_color, g), m);
	fg tg(g, filter);
	int num = connected_components(tg, &components[0]);
	return num;
}

//MVCA modified always has k colors
template <class Graph>
int kLSFMVCA(Graph &g, int k_sup, int n_labels) {
	std::vector<int> components(num_vertices(g));
	db temp(n_labels);
	int f_colors = n_labels - num_vertices(g) + 1;
	int num_c = get_components(g, temp, components);
	int num_c_best = num_c;
	while (temp.count() < k_sup) {
		int best_label = 0;
		for (int i = 0; i < f_colors; ++i) {
			if (!temp.test(i)) {
				temp.set(i);
				int nc = get_components(g, temp, components);
				if (nc <= num_c_best) {
					num_c_best = nc;
					best_label = i;
				}
				temp.flip(i);
			}
		}
		temp.set(best_label);
	}
	num_c_best = get_components(g, temp, components);
	//print_filtered_graph(g,temp);
	return  num_c_best;//just to be right
}

template <class Graph>
db MkLSFMVCA(Graph &g, int k_sup, int n_labels) {
	std::vector<int> components(num_vertices(g));
	int f_colors = size - num_vertices(g) + 1;
	db temp(n_labels);
	int num_c = get_components(g, temp, components);
	int num_c_best = num_c;
	while (temp.count() < k_sup) {
		int best_label = 0;
		for (int i = 0; i < f_colors; ++i) {
			if (!temp.test(i)) {
				temp.set(i);
				int nc = get_components(g, temp, components);
				if (nc <= num_c_best) {
					num_c_best = nc;
					best_label = i;
				}
				temp.flip(i);
			}
		}
		temp.set(best_label);
	}
	//num_c_best = get_components(g, temp, components);
	//print_filtered_graph(g,temp);
	return  temp;//just to be right
}

int root(int current, std::vector<int> &parent) {
	while (parent[current] != current) {
		current = parent[current];
	}
	return current;
}

template<class Graph>
int max_reduce(Graph &g, int n_curr, int n_colors, std::vector<int> &comp, int label) {
	std::vector<int> parent(n_curr), level(n_curr);
	volatile int comp_a, comp_b; //so i could debug dont know why.
	int result;
	for (int i = 0; i < n_curr; ++i) {
		parent[i] = i;
		level[i] = 0;
	}
	result = 0;

	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	db mask(n_colors);
	mask.set(label);
	valid_edge_color<EdgeColorMap, db> filter(get(edge_color, g), mask);
	fg G(g, filter);
	std::tie(it, end) = boost::edges(G);

	while (it != end) {
		comp_a = comp[source(*it, G)];
		comp_b = comp[target(*it, G)];
		if (comp_a != comp_b) {
			volatile int root_a, root_b;
			root_a = root(comp_a, parent);
			root_b = root(comp_b, parent);
			if (root(comp_a, parent) != root(comp_b, parent)) {
				if (level[root(comp_a, parent)] > level[root(comp_b, parent)]) parent[root(comp_b, parent)] = root(comp_a, parent);
				else {
					if (level[root(comp_a, parent)] == level[root(comp_b, parent)]) {
						level[root(comp_b, parent)]++;
					}
					parent[root(comp_a, parent)] = root(comp_b, parent);
				}
				result++;
			}
		}
		++it;
	}
	return result;
}

typedef typename adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_color_t, int>> graph_t;

template<class Graph>
property_map<graph_t, edge_color_t>::type get_colors(Graph &g) {
	typedef typename property_map<Graph, edge_color_t>::type ColorMap;
	ColorMap colors = get(edge_color, g);
	//make color type generic
	return colors;
}

template<class Graph,class Model,class Mask>
void generateCuts(Graph g, Model &mod,Mask temp,int n_colors,IloBoolVarArray& z) {
	std::vector<int> components(num_vertices(g));
	auto colors = get_colors(g);
	graph_traits<graph_t>::edge_iterator it, end;
	//std::cout << " user cutting" << std::endl;
	int no = get_components(g, temp, components);
	int num_c_best = no;
	int best_label = -1;
	boost::random::mt19937 gen(std::time(0));
	boost::random::uniform_int_distribution<> dist(3, 10);
	int i = dist(gen);
	while (num_c_best >= i) {//peharps temp.count() < k_sup
														   /*boost::random::uniform_int_distribution<> gen(0, size-1);
														   int i = gen(rng);													   if (!temp.test(i))temp.set(i);
														   */
		int diff;
		int best_diff = num_vertices(g);
		no = num_c_best;
		best_label = -1;
		for (int i = 0; i < n_colors; ++i) {
			if (!temp.test(i)) {
				temp.set(i);
				num_c_best = get_components(g, temp, components);
				diff = no - num_c_best;
				if (diff < best_diff) {
					best_diff = diff;
					best_label = i;
					temp.flip(i);
					break;
				}
				temp.flip(i);
			}
		}
		if (best_label >= 0)temp.set(best_label);
		num_c_best = get_components(g, temp, components);
	}
	if (best_label >= 0)temp.flip(best_label);
	num_c_best = get_components(g, temp, components);
	if (num_c_best > 1) {
		//std::cout << "add user cut" << std::endl;
		//db temp1(size);
		std::tie(it, end) = edges(g);
		IloExpr expr(mod.getEnv());
		vector<db> masks(num_c_best);
		for (int i = 0; i < num_c_best; ++i) masks[i].resize(n_colors);
		while (it != end) {
			if (components[source(*it, g)] != components[target(*it, g)]) {
				masks[components[source(*it, g)]].set(colors[*it]);
				masks[components[target(*it, g)]].set(colors[*it]);
			}
			++it;
		}
		for (int i = 0; i < num_c_best; ++i) {
			for (int j = 0; j < n_colors; ++j) if (masks[i].test(j))expr += z[j];
			mod.add(expr >= 1);
			expr.clear();
		}
		expr.end();
	}

}

// preprocessing functions
template<class Graph>
void treefy(Graph& g, int n_colors) {
	Graph result(num_vertices(g));
	typedef boost::graph_traits<Graph>::edge_descriptor edge_t;
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	for (int l = 0; l < n_colors; ++l) {
		db mask(n_colors);
		mask.set(l);
		std::vector<int> components(num_vertices(g));// components graph
		int n_curr = get_components(g, mask, components);
		std::vector<int> my_mapping(n_curr, -1);
		for (int u = 0; u < num_vertices(g); ++u) {
			if (my_mapping[components[u]] == -1)my_mapping[components[u]] = u;
			else add_edge(my_mapping[components[u]], u, property<edge_color_t, int>(l), result);
		}
	}
	g.clear();
	copy_graph(result, g);
}
template<class Graph>
void completefy(Graph& g, int n_colors) {
	Graph result(num_vertices(g));
	typedef boost::graph_traits<Graph>::edge_descriptor edge_t;
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	for (int l = 0; l < n_colors; ++l) {
		db mask(n_colors);
		mask.set(l);
		std::vector<int> components(num_vertices(g));// components graph
		int n_curr = get_components(g, mask, components);
		std::vector<int> my_mapping(n_curr, -1);
		for (int u = 0; u < num_vertices(g); ++u) {
			for (int v = u + 1; v < num_vertices(g); ++v) {
				if (components[u] == components[v]) {
					add_edge(u, v, property<edge_color_t, int>(l), result);
				}
			}
		}
	}
	g.clear();
	copy_graph(result, g);
}
// preprocessing functions
template<class Graph>
void MCR(Graph& g, int n_colors) {
	Graph result(num_vertices(g));
	typedef boost::graph_traits<Graph>::edge_descriptor edge_t;
	typedef typename property_map<Graph, edge_color_t>::type EdgeColorMap;
	typedef filtered_graph<Graph, valid_edge_color<EdgeColorMap, db> > fg;
	typedef typename fg::edge_iterator eit;
	eit it, end;
	for (int l = 0; l < n_colors; ++l) {
		db mask(n_colors);
		mask.set(l);
		valid_edge_color<EdgeColorMap, db> filter(get(edge_color, g), mask);
		fg H(g, filter);
		typedef typename property_map<fg, vertex_index_t>::type IndexMap;
		IndexMap index = get(vertex_index, H);
		//disjoint_sets ds(num_vertices(g))

		rank_t rank_map;
		parent_t parent_map;
		boost::associative_property_map<rank_t>   rank_pmap(rank_map);
		boost::associative_property_map<parent_t> parent_pmap(parent_map);
		boost::disjoint_sets<
			associative_property_map<rank_t>,
			associative_property_map<parent_t> > ds(
				rank_pmap,
				parent_pmap);
		//std::vector<Element> elements;
		//elements.push_back(Element(...));
		//rank_t rank_map;
		//parent_t parent_map;

		//boost::associative_property_map<rank_t>   rank_pmap(rank_map);
		//boost::associative_property_map<parent_t> parent_pmap(parent_map);

		for (int i = 0; i < num_vertices(g); ++i) {
			ds.make_set(i);
		}
		std::tie(it, end) = edges(H);
		while (it != end) {
			int u = index[source(*it, H)];
			int v = index[target(*it, H)];
			if (ds.find_set(u) != ds.find_set(v)) {
				add_edge(u, v, property<edge_color_t, int>(l), result);
				ds.union_set(u, v);
			}
			else {
				std::cout << "MCR removed edge:" << " (" << u << "," << v << ") " << " Color: " << l << std::endl;
			}
			++it;
		}
	}
	g.clear();
	copy_graph(result, g);
}

template<class Graph> // dont work with multigraph
void buildFlowModel(IloModel mod, IloBoolVarArray Z, IloVarMatrix F, const int k, const Graph &g,int opt,int lower_bound) {
	IloEnv env = mod.getEnv();
	int n_colors = Z.getSize();
	typedef typename property_map<Graph, edge_color_t>::const_type ColorMap;
	typedef typename graph_traits<Graph>::edge_descriptor edge_desc;
	ColorMap colors = get(edge_color, g);
	/*std::vector<Graph> monocromatic_graphs(n_colors, Graph(num_vertices(g)));
	//creating new colored graphs mantain diferent colors
	auto[it_edges, last_edge] = edges(g);
	ColorMap colors = get(edge_color, g);
	while (it_edges != last_edge) {
		add_edge(source(*it_edges, g), target(*it_edges, g), property<edge_color_t, int>(colors[*it_edges]), monocromatic_graphs[colors[*it_edges]]);
		it_edges++;
	}*/

	//modelling objective function
	IloExpr exp(env);
	int n_vertices = num_vertices(g);
	int s = n_vertices - 1;//super source
	int f_colors = n_colors - n_vertices + 1;
	for (int i = f_colors; i < n_colors; ++i) {
		exp += Z[i];
	}
	mod.add(IloMinimize(env, exp));
	//mod.add(exp >= lower_bound);
	//mod.add(exp>=40);
	//mod.add(exp == 1);
	//exp.end();
	//modelling f_{ij} temporario ajeitar para deixar mais compactor depois um para cada aresta

	for (int i = 0; i < n_vertices; ++i) {
		F[i] = IloNumVarArray(env, n_vertices, 0, n_vertices-1, ILOFLOAT);
	}
	for (int i = 0; i < n_vertices; ++i) {
		for (int j = 0; j < n_vertices; ++j) {
			F[i][j].setName(("f_" + std::to_string(i) + "_" + std::to_string(j)).c_str());
		}
	}
	//setting names to labels variables.
	for (int i = 0; i<n_colors; ++i) {
		Z[i].setName(("z" + std::to_string(i)).c_str());
	}
	// first constraint
	typedef typename graph_traits<Graph>::vertex_iterator vertex_it;
	typedef typename graph_traits<Graph>::in_edge_iterator in_edge_it;
	IloExpr lhs(env);
	IloExpr lhs1(env);
	vertex_it vit, vend;
	std::tie(vit, vend) = vertices(g);
	in_edge_it eit, eend;
	for (auto it = vit; it != vend && *it<s; ++it) {
		std::tie(eit, eend) = in_edges(*it, g);
		for (auto tit = eit; tit != eend; ++tit) {
			lhs += F[source(*tit, g)][target(*tit, g)];
			//new constraint 1
			//if (source(*tit, g) != s) {
				lhs -= F[target(*tit, g)][source(*tit, g)];
				lhs1 += F[target(*tit, g)][source(*tit, g)];
			//}
		}
		mod.add(lhs == 0);
		mod.add(lhs1 >= 1);
		lhs.clear();
		lhs1.clear();
	}
	lhs.end();
	lhs1.end();

	edge_desc my_edge;
	bool result = false;
	//IloExpr expression1(env);
	for (int i = 0; i < n_vertices-1; ++i) {
		for (int j = 0; j < n_vertices-1; ++j) {
			std::tie(my_edge, result) = edge(i, j, g);
			if (result) {
				mod.add(F[i][j]<= (n_vertices - 1) * Z[colors[my_edge]]);
			}
		}

	}
	//expression1.end();
	//third big-mconstraint
	for (int i = f_colors; i < n_colors; ++i) {
		mod.add(F[s][i - f_colors] == IloInt(1));
		mod.add(F[i - f_colors][s] <= IloInt(n_vertices-1)*Z[i]);
		mod.add(F[i - f_colors][s] >= Z[i]);
		//mod.add(F[s][i - f_colors] <= IloInt(n_vertices - 1)*Z[i]);
	}
	exp.end();

	//constraint every vertex has a colored edge incident
	auto [first_vertex, last_vertex] = vertices(g);
	while (first_vertex != last_vertex) {
		db used_colors(n_colors);
		auto[first_edge, last_edge] = in_edges(*first_vertex,g);
		IloExpr expInEdges(env);
		while (first_edge != last_edge) {
			volatile int idx = colors[*first_edge];
			if (!used_colors.test_set(idx,1)) {
				expInEdges += Z[colors[*first_edge]];
			}
			first_edge++;
		}
		mod.add(expInEdges >= 1);
		expInEdges.end();
		first_vertex++;
	}

	IloExpr texp(env);
	for (int i = 0; i < f_colors; ++i) {
		texp += Z[i];
	}
	mod.add(texp == k);
	texp.end();

}

template<class Graph>
void solveModel(int n_vertices, int n_colors, int k, Graph &g) {

	//starting cplex code part
	IloEnv   env; //environment
	try {
		IloModel model(env);
		IloBoolVarArray Z(env, n_colors);
		IloNumArray pri(env, n_colors);
		IloVarMatrix    F(env, n_vertices); //each edge has at least a edge to the supersource
		int opt = kLSFMVCA(g, k, n_colors) - 1;
		buildFlowModel(model, Z, F, k, g, opt, 1);
		IloCplex cplex(model);
		int f_colors = n_colors - num_vertices(g) + 1;
		cplex.exportModel("kSLF_fluxo.lp"); // good to see if the model is correct
											//cross your fingers
		{//set priorities number edges by color.
			auto it = boost::edges(g).first;
			auto end = boost::edges(g).second;
			auto colormap = get(edge_color, g);
			while (it != end) {
				pri[colormap[*it]]++;
				++it;
			}
		}

		//trying to disable automatic cuts
		//cplex.setParam(IloCplex::Param::MIP::Limits::CutPasses,-1);
		cplex.setParam(IloCplex::Param::MIP::Tolerances::UpperCutoff, opt);
		cplex.setParam(IloCplex::Param::Threads, 4);
		//cplex.setParam(IloCplex::Param::Parallel, -1);
		cplex.setParam(IloCplex::Param::Emphasis::MIP, 1);
		cplex.setParam(IloCplex::Param::Benders::Strategy, 3);
		cplex.setParam(IloCplex::TiLim, 7300);
		cplex.setPriorities(Z, pri);
		cplex.solve();
		//cplex.exportModel("kSLF_fluxo_after_presolve.lp");
		cplex.out() << "solution status = " << cplex.getStatus() << endl;

		cplex.out() << endl;
		cplex.out() << "Number of components   = " << cplex.getObjValue() << endl;
		db temp(n_colors);
		cplex.out() << "color(s) solution:";
		for (int i = 0; i < f_colors; i++) {
			if (std::abs(cplex.getValue(Z[i]) - 1.0f) <= 1e-3)cplex.out() << " " << i;
		}
		cplex.out() << endl;
		cplex.out() << "root(s) solution:";
		//int f_colors = Z.getSize() - n_vertices + 1;
		for (int i = f_colors; i < n_colors; i++) {
			if (std::abs(cplex.getValue(Z[i]) - 1.0f) <= 1e-3)cplex.out() << " " << i - f_colors;
		}
		cplex.out() << endl;
	}
	catch (IloException& e) {
		cerr << "Concert exception caught: " << e << endl;
	}
	catch (...) {
		cerr << "Unknown exception caught" << endl;
	}
	//memory cleaning
	env.end();
}

int main(int argc, const char *argv[])
{
	typedef adjacency_list<vecS, vecS, undirectedS, no_property, property<edge_color_t, int>> Graph;
	typedef std::pair<int, int> Edge;
	typedef boost::graph_traits<Graph>::vertex_descriptor vertex_t;
	Graph::edge_iterator it, end;
	Graph g;
	int n_vertices, n_colors;
	//command-line processor

	try {
		std::ifstream ifn;
		po::options_description desc{ "Options" };
		desc.add_options()("help,h", "produce help message")
			("input-file,i", po::value< string >(), "input file")
			("include-path,I", po::value< string >(), "include path")
			("setup-file", po::value< string >(), "setup file");
		po::positional_options_description p;
		p.add("input-file", -1);

		po::variables_map vm;
		po::store(po::command_line_parser(argc, argv).
			options(desc).positional(p).run(), vm);
		po::notify(vm);

		if (vm.count("help")) {
			cout << desc << "\n";
			return 1;
		}
		else if (vm.count("input-file"))
		{
			std::cout << "Input files are: " << vm["input-file"].as<string>() << "\n";
			if (vm.count("include-path"))ifn.open((vm["include-path"].as<string>() + vm["input-file"].as<string>()).c_str(), ifstream::in);
			else ifn.open(vm["input-file"].as<string>().c_str(), ifstream::in);
			if (!ifn.is_open()) {
				std::cout << "error opening file" << std::endl;
				exit(EXIT_FAILURE);
			}
			dynamic_properties dp;
			dp.property("color", get(edge_color, g));
			read_graphml(ifn, g, dp);

			vector<string> vecI;
			split(vecI, vm["input-file"].as<string>(), is_any_of("-."), token_compress_off);
			if (vecI.size() == 6) {
				std::cout << vecI[0] << std::endl;
				n_vertices = stoi(vecI[0]);
				std::cout << vecI[2] << std::endl;
				n_colors = stoi(vecI[2]);
				std::cout << vecI[3] << std::endl;
				int k = stoi(vecI[3]);
				//add edges to super source vertex. remember!!!
				vertex_t u = add_vertex(g);
				n_vertices++;
				for (int i = 0; i < n_vertices - 1; ++i) boost::add_edge(u, i, property<edge_color_t, int>(n_colors++), g);
				//std::tie(it, end) = boost::edges(g);
				//print_edges(it, end, g);
				MCR(g, n_colors);
				//treefy(g, n_colors);
				//completefy(g, n_colors);
				//auto colors = get(edge_color, g);
				//dynamic_properties dp;
				//dp.property("Color", colors);
				//std::ofstream tmp("out.graphml");
				//write_graphml(tmp, g, dp, true);
				solveModel(n_vertices, n_colors, k, g);
			}
			else {
				std::cout << "file wrong name format." << std::endl;
			}

		}
		else if (vm.count("setup-file")) {
			std::cout << "Not implemented yet" << std::endl;
		}
		else {
			std::cout << "see options(-h)." << std::endl;
		}

	}
	catch (const po::error &ex) {
		std::cout << ex.what();
		exit(EXIT_FAILURE);
	}
	catch (boost::exception &ex) {
		std::cout << boost::diagnostic_information(ex) << std::endl;
	}
	catch (std::exception &ex) {
		std::cout << ex.what();
		exit(EXIT_FAILURE);
	}

	return 0;
}


#ifndef ESP_HPP
#define ESP_HPP

#include <armadillo>
#include <libint2.hpp>
#include <vector>

namespace willow {
namespace qcmol {

arma::vec esp_atomic_charges(const std::vector<libint2::Atom> &atoms,
                             const libint2::BasisSet &bs, const arma::mat &Dm,
                             const bool &l_resp);

}
} // namespace willow

#endif


#pragma once

#include <algorithm>
#include <cstdint>
#include <cstring>
#include <iterator>
#include <memory>
#include <tuple>
#include <type_traits>
#include <utility>

#include <boost/iterator/iterator_facade.hpp>
#include <stuff/assert.hpp>
#include <stuff/container/detail/hash_table_growth_policy.hpp>
#include <stuff/container/detail/raw_storage.hpp>
#include <stuff/container/selectors.hpp>
#include <stuff/meta/named_template_params.hpp>
#include <stuff/meta/nth.hpp>

namespace stuff::container::detail {

DECLARE_VALUE_SELECTOR(max_size, std::size_t);

constexpr size_t upper_power_of_2(std::size_t n) {
    if (n == 0 || n == 1) {
        return 1;
    } else {
        return 2 * upper_power_of_2((n + 1) / 2);
    }
}

template <std::size_t max_size>
struct bucket_metadata {
    enum class deleted : uint64_t { YES = 1, NO = 0 };

    deleted deleted : 1;
    std::uint64_t size : max_size;
    std::uint64_t distance : 64 - 1 - max_size;

    bucket_metadata(std::size_t distance, std::size_t size = 0,
                    enum deleted deleted = deleted::NO)
        : deleted(deleted), size(size), distance(distance) {}

    static bucket_metadata vacant() { return {0, 0, deleted::NO}; }
};

template <typename Key, typename T, typename Hash, typename Equal, typename Allocator,
          typename... Params>
class robin_hood_storage {
    using growth_policy = weird_powers_of_two_growth_policy;
    static constexpr std::size_t max_size_ =
        upper_power_of_2(meta::get_v<max_size<1ull << 37>, Params...>);
    static constexpr bool store_hash_ =
        meta::get_v<container::store_hash<true>, Params...>;

    using hash_type = std::uint32_t;
    using metadata = bucket_metadata<37>;
    using raw_storage_type =
        std::conditional_t<store_hash_,
                           raw_storage<Allocator, zeroable<metadata>, T, hash_type>,
                           raw_storage<Allocator, zeroable<metadata>, T>>;

    raw_storage_type raw_storage_;

    typename growth_policy::reducer to_position_;
    Hash hash_;
    Equal equal_;
    std::size_t size_ = 0;

    struct adjust_iterator_position {};

    template <bool constant>
    struct base_iterator
        : public boost::iterator_facade<base_iterator<constant>,
                                        std::conditional_t<constant, const T, T>,
                                        boost::forward_traversal_tag> {
        base_iterator(const base_iterator&) = default;
        base_iterator(base_iterator&&) = default;

        base_iterator& operator=(const base_iterator&) = default;
        base_iterator& operator=(base_iterator&&) noexcept = default;

    private:
        using parent_type =
            std::conditional_t<constant, const robin_hood_storage*, robin_hood_storage*>;
        std::size_t pos_ = 0;
        parent_type parent_ = nullptr;

        friend class robin_hood_storage;
        friend class boost::iterator_core_access;

        base_iterator(std::size_t pos, parent_type parent) : pos_(pos), parent_(parent) {}

        base_iterator(std::size_t pos, parent_type parent, adjust_iterator_position)
            : pos_(pos), parent_(parent) {
            if (pos_ != parent->capacity() &&
                (parent_->vacant_at(pos) || parent_->deleted_at(pos))) {
                increment();
            }
        }

        std::conditional_t<constant, const T, T>& dereference() const noexcept {
            return parent_->value_at(pos_);
        }

        bool equal(const base_iterator& other) const noexcept {
            return pos_ == other.pos_ && parent_ == other.parent_;
        }

        void increment() noexcept {
            while (true) {
                ++pos_;
                if (pos_ == parent_->capacity() ||
                    (!parent_->deleted_at(pos_) && !parent_->vacant_at(pos_))) {
                    break;
                }
            }
        }
    };

public:
    using iterator = base_iterator<false>;

    class const_iterator : public base_iterator<true> {
    public:
        const_iterator(const const_iterator&) = default;
        const_iterator(const_iterator&&) = default;
        const_iterator(const iterator& other)
            : const_iterator(other.pos_, other.parent_) {}

        const_iterator& operator=(const const_iterator&) noexcept = default;
        const_iterator& operator=(const_iterator&&) noexcept = default;
        const_iterator& operator=(const iterator& other) noexcept {
            return *this = const_iterator{other.pos_, other.parent_};
        }

        friend bool operator==(const const_iterator& l, const iterator& r) {
            return l.pos_ == r.pos_ && l.parent_ == r.parent_;
        }
        friend bool operator==(const iterator& l, const const_iterator& r) {
            return r == l;
        }
        friend bool operator!=(const const_iterator& l, const iterator& r) {
            return !(l == r);
        }
        friend bool operator!=(const iterator& l, const const_iterator& r) {
            return !(r == l);
        }

    private:
        friend class robin_hood_storage;

        const_iterator(std::size_t pos, const robin_hood_storage* parent)
            : base_iterator<true>(pos, parent) {}

        const_iterator(std::size_t pos, const robin_hood_storage* parent,
                       adjust_iterator_position)
            : base_iterator<true>(pos, parent, adjust_iterator_position{}) {}
    };

private:
    iterator iterator_for(std::size_t pos) noexcept { return iterator{pos, this}; }
    iterator to_iterator(const const_iterator& it) noexcept {
        return iterator{it.pos_, this};
    }

    const_iterator iterator_for(std::size_t pos) const noexcept {
        return const_iterator{pos, this};
    }

    metadata& metadata_at(std::size_t pos) noexcept {
        return raw_storage_.template get<0>(pos);
    }
    const metadata& metadata_at(std::size_t pos) const noexcept {
        return raw_storage_.template get<0>(pos);
    }

    T& value_at(std::size_t pos) noexcept { return raw_storage_.template get<1>(pos); }
    const T& value_at(std::size_t pos) const noexcept {
        return raw_storage_.template get<1>(pos);
    }

    hash_type& hash_at(std::size_t pos) noexcept {
        return raw_storage_.template get<2>(pos);
    }
    hash_type hash_at(std::size_t pos) const noexcept {
        return raw_storage_.template get<2>(pos);
    }

    bool deleted_at(std::size_t pos) const noexcept {
        return metadata_at(pos).deleted == metadata::deleted::YES;
    }

    std::uint64_t distance_at(std::size_t pos) const noexcept {
        return metadata_at(pos).distance;
    }

    void erase_at(std::size_t pos) {
        raw_storage_.template destroy<1>(pos);
        metadata_at(pos).deleted = metadata::deleted::YES;
        --size_;
    }

    template <typename V>
    hash_type folded_hash(const V& value) const noexcept(noexcept(hash_(value))) {
        const auto h = hash_(value);
        // return (h & 0x00000000FFFFFFFF) ^ (h >> 32);
        return static_cast<hash_type>(h);
    }

    // clang-format off
    void grow() {
        const uint64_t growth_factor = 2;
        const auto old_capacity = raw_storage_.capacity();
        std::size_t new_capacity;
        std::tie(new_capacity, to_position_) =
            growth_policy::get(std::max(capacity() * growth_factor, 1llu));
        raw_storage_type old_raw_storage(new_capacity, raw_storage_.allocator());
        swap(old_raw_storage, raw_storage_);

        size_ = 0;
        for (size_t pos = 0; pos < old_capacity; ++pos) {
            const auto& old_metadata = old_raw_storage.template get<0>(pos);
            if (*reinterpret_cast<const std::uint64_t*>(&old_metadata) != 0 &&
                old_metadata.deleted == metadata::deleted::NO) {
                auto& value = old_raw_storage.template get<1>(pos);
                if constexpr(store_hash_) {
                    const auto hash = old_raw_storage.template get<2>(pos);
                    insert_with_hash(hash, std::move(value));
                } else {
                    insert_impl(std::move(value));
                }
            }
        }
    }

    template <typename... Args>
    void put(std::size_t pos, hash_type hash, std::uint64_t distance, Args&&... args) {
        if (vacant_at(pos) || deleted_at(pos)) {
            raw_storage_.template construct<1>(pos, std::forward<Args>(args)...);
        } else {
            if constexpr(sizeof...(Args) == 1 && std::is_same<meta::nth_t<0, Args...>, T>::value &&
                    std::is_move_assignable<T>::value) {
                value_at(pos) = std::move(args...);
            } else {
                raw_storage_.template destroy<1>(pos);
                raw_storage_.template construct<1>(pos, std::forward<Args>(args)...);
            }
        }
        metadata_at(pos).distance = distance;
        metadata_at(pos).deleted = metadata::deleted::NO;
        if constexpr(store_hash_) { hash_at(pos) = hash; }
    }
    // clang-format on

    template <typename... Args>
    std::pair<iterator, bool> insert_with_key(const Key& key, Args&&... args) {
        const auto hash = folded_hash(key);
        // Creating copy of value only if necessary
        auto pos_for_hash = position_for_hash(hash);
        const auto[actual_pos, distance, should_insert] =
            find_insertion_position(pos_for_hash, key);
        if (should_insert) {
            put(actual_pos, hash, distance, std::forward<Args>(args)...);
            ++size_;
            ++metadata_at(pos_for_hash).size;
        }
        return {iterator_for(actual_pos), should_insert};
    }

    template <typename Value>
    std::pair<iterator, bool> insert_impl(Value&& value) {
        const auto hash = folded_hash(value);
        // Creating copy of value only if necessary
        // return insert_with_hash(hash, std::forward<Value>(value));
        auto pos_for_hash = position_for_hash(hash);
        const auto[actual_pos, distance, should_insert] =
            find_insertion_position(pos_for_hash, value);
        if (should_insert) {
            put(actual_pos, hash, distance, std::forward<Value>(value));
            ++size_;
            ++metadata_at(pos_for_hash).size;
        }
        return {iterator_for(actual_pos), should_insert};
    }

    template <typename K>
    std::tuple<std::size_t, std::uint64_t, bool> find_insertion_position(std::size_t pos,
                                                                         const K& key) {
        std::uint64_t distance = 0;
        while (true) {
            if (vacant_at(pos)) {
                return {pos, distance, true};
            }

            const bool deleted_at_pos = deleted_at(pos);
            if (!deleted_at_pos && equal_(key, value_at(pos))) {
                return {pos, distance, false};
            }

            const auto distance_at_pos = distance_at(pos);
            if (distance_at_pos < distance) {
                if (!deleted_at_pos) {
                    if
                        constexpr(store_hash_) {
                            continue_insertion(std::move(value_at(pos)), hash_at(pos),
                                               next_position(pos), distance_at_pos + 1);
                        }
                    else {
                        continue_insertion(std::move(value_at(pos)), hash_type{},
                                           next_position(pos), distance_at_pos + 1);
                    }
                }
                return {pos, distance, true};
            }

            ++distance;
            pos = next_position(pos);
        }
    }

    template <typename U>
    std::pair<iterator, bool> insert_with_hash(hash_type hash, U&& u) {
        auto pos = position_for_hash(hash);
        auto& bkt_metadata = metadata_at(pos);
        std::uint64_t distance = 0;

        while (true) {
            if (vacant_at(pos)) {
                put(pos, hash, distance, std::forward<U>(u));
                ++size_;
                ++bkt_metadata.size;
                return {iterator_for(pos), true};
            }

            const bool deleted_at_pos = deleted_at(pos);
            if (!deleted_at_pos && equal_(value_at(pos), u)) {
                return {iterator_for(pos), false};
            }

            const auto distance_at_pos = distance_at(pos);
            if (distance_at_pos < distance) {
                if (!deleted_at_pos) {
                    if
                        constexpr(store_hash_) {
                            T old_value{std::move(value_at(pos))};
                            hash_type old_hash{hash_at(pos)};
                            put(pos, hash, distance, std::forward<U>(u));
                            continue_insertion(std::move(old_value), old_hash,
                                               next_position(pos), distance_at_pos + 1);
                        }
                    else {
                        T old_value{std::move(value_at(pos))};
                        put(pos, hash, distance, std::forward<U>(u));
                        continue_insertion(std::move(old_value), hash, next_position(pos),
                                           distance_at_pos + 1);
                    }
                } else {
                    put(pos, hash, distance, std::forward<U>(u));
                }
                ++size_;
                ++bkt_metadata.size;
                return {iterator_for(pos), true};
            }
            ++distance;
            pos = next_position(pos);
        }
    }

    void continue_insertion(T&& value, hash_type hash, std::size_t pos,
                            std::uint64_t distance) {
        while (true) {
            if (vacant_at(pos)) {
                put(pos, hash, distance, std::move(value));
                return;
            }

            const auto distance_at_pos = distance_at(pos);
            if (distance_at_pos < distance) {
                if (deleted_at(pos)) {
                    put(pos, hash, distance, std::move(value));
                    return;
                }
                if
                    constexpr(store_hash_) {
                        T old_value{value_at(pos)};
                        hash_type old_hash{hash_at(pos)};
                        put(pos, hash, distance, std::move(value));
                        hash = old_hash;
                        if
                            constexpr(std::is_move_assignable<T>::value) {
                                value = std::move(old_value);
                            }
                        else {
                            std::allocator_traits<Allocator>::destroy(
                                raw_storage_.allocator(), &value);
                            std::allocator_traits<Allocator>::construct(
                                raw_storage_.allocator(), &value, std::move(old_value));
                        }
                    }
                else {
                    T old_value{std::move(value_at(pos))};
                    put(pos, hash, distance, std::move(value));
                    value = std::move(old_value);
                }
                distance = distance_at_pos;
            }
            ++distance;
            pos = next_position(pos);
        }
    }

    std::size_t next_position(std::uint64_t pos) const noexcept {
        ++pos;
        return to_position_(pos);
    }

    std::size_t position_for_hash(hash_type h) const noexcept { return to_position_(h); }

    template <typename V>
    std::size_t search(const V& value) const
        noexcept(noexcept(folded_hash(value) && equal_(value, value_at(0)))) {
        constexpr bool use_hash_for_comparison = sizeof(T) > 8;

        if (empty()) {
            return capacity();
        }
        const auto hash = folded_hash(value);
        auto pos = position_for_hash(hash);
        std::uint64_t distance = 0;
        while (true) {
            while (pos < capacity()) {
                if (vacant_at(pos) || distance > distance_at(pos)) {
                    return capacity();
                }

                if
                    constexpr(store_hash_ && use_hash_for_comparison) {
                        if (/*distance_at(pos) == distance &&*/ !deleted_at(pos) &&
                            hash == hash_at(pos) && equal_(value, value_at(pos))) {
                            return pos;
                        }
                    }
                else {
                    if (/*distance_at(pos) == distance &&*/ !deleted_at(pos) &&
                        equal_(value, value_at(pos))) {
                        return pos;
                    }
                }

                ++distance;
                ++pos;
            }
            pos = 0;
        }
    }

    bool vacant_at(std::size_t pos) const noexcept {
        return *reinterpret_cast<const std::uint64_t*>(&metadata_at(pos)) == 0;
    }

public:
    robin_hood_storage() = default;
    robin_hood_storage(const robin_hood_storage& other)
        : robin_hood_storage(other.begin(), other.end()) {}
    robin_hood_storage(robin_hood_storage&&) = default;

    explicit robin_hood_storage(std::size_t capacity,
                                const Allocator& alloc = Allocator{})
        : raw_storage_(capacity, alloc) {}

    template <
        typename InputIter,
        typename std::enable_if_t<
            std::is_same<typename std::iterator_traits<InputIter>::iterator_category,
                         std::input_iterator_tag>::value,
            int> = 0>
    robin_hood_storage(InputIter first, InputIter last, const Hash& hash,
                       const Equal& equal, const Allocator& alloc)
        : raw_storage_(alloc), hash_(hash), equal_(equal) {
        for (; first != last; ++first) {
            insert(*first);
        }
    }

    template <
        typename InputIter,
        typename std::enable_if_t<std::is_convertible<typename std::iterator_traits<
                                                          InputIter>::iterator_category,
                                                      std::forward_iterator_tag>::value,
                                  unsigned> = 0>
    robin_hood_storage(InputIter first, InputIter last, const Hash& hash,
                       const Equal& equal, const Allocator& alloc)
        : raw_storage_(0, alloc), hash_(hash), equal_(equal) {
        std::size_t capacity;
        std::tie(capacity, to_position_) =
            growth_policy::get((std::distance(first, last) * 10) / 8 + 1);
        raw_storage_ = raw_storage_type{capacity, alloc};
        for (; first != last; ++first) {
            insert_impl(*first);
        }
    }

    ~robin_hood_storage() { clear(); }

    robin_hood_storage& operator=(const robin_hood_storage& other) {
        robin_hood_storage other_copy{other.begin(), other.end(), hash_, equal_,
                                      raw_storage_.allocator()};
        *this = std::move(other_copy);
        return *this;
    }
    robin_hood_storage& operator=(robin_hood_storage&&) = default;

    iterator begin() noexcept { return iterator{0, this, adjust_iterator_position{}}; }
    const_iterator begin() const noexcept {
        return const_iterator{0, this, adjust_iterator_position{}};
    }

    iterator end() noexcept { return iterator_for(capacity()); }
    const_iterator end() const noexcept { return iterator_for(capacity()); }

    std::size_t capacity() const noexcept { return raw_storage_.capacity(); }

    bool empty() const noexcept { return size_ == 0; }

    iterator erase(const_iterator it) {
        STUFF_ASSERT(it.parent_ == this);
        STUFF_ASSERT(it.pos_ <= capacity());
        STUFF_ASSERT(!vacant_at(it.pos_) && !deleted_at(it.pos_));

        const auto result = std::next(it);
        erase_at(it.pos_);
        return to_iterator(result);
    }

    void quick_erase(const_iterator it) {
        STUFF_ASSERT(it.parent_ == this);
        STUFF_ASSERT(it.pos_ <= capacity());
        STUFF_ASSERT(!vacant_at(it.pos_) && !deleted_at(it.pos_));

        erase_at(it.pos_);
    }

    iterator erase(const_iterator first, const_iterator last) {
        STUFF_ASSERT(first.parent_ == this);
        STUFF_ASSERT(last.parent_ == this);
        STUFF_ASSERT(first.pos_ <= last.pos_);
        STUFF_ASSERT(last.pos_ <= capacity());
        STUFF_ASSERT(!vacant_at(first.pos_) && !deleted_at(first.pos_));
        STUFF_ASSERT(last == end() || (!vacant_at(last.pos_) && !deleted_at(last.pos_)));

        for (; first != last; ++first) {
            erase_at(first.pos_);
        }
        return to_iterator(last);
    }

    template <typename V>
    std::size_t erase(const V& value) {
        const auto pos = search(value);
        if (pos != capacity()) {
            erase_at(pos);
            return 1;
        }
        return 0;
    }

    std::size_t size() const noexcept { return size_; }

    std::size_t max_size() const noexcept { return max_size_; }

    void clear() noexcept {
        for (std::size_t pos = 0; pos < capacity(); ++pos) {
            if (vacant_at(pos)) {
                continue;
            }
            if (!deleted_at(pos)) {
                raw_storage_.template destroy<1>(pos);
            }
            metadata_at(pos) = metadata::vacant();
        }
        size_ = 0;
    }

    template <typename Value>
    std::pair<iterator, bool> insert(Value&& value) {
        if (10 * size() >= 8 * capacity()) {
            grow();
        }
        return insert_impl(std::forward<Value>(value));
    }

    template <typename InputIter>
    void insert(InputIter first, InputIter last) {
        for (; first != last; ++first) {
            insert(*first);
        }
    }

    // support for map
    template <typename... Args>
    std::pair<iterator, bool> emplace(const Key& key, Args&&... args) {
        if (10 * size() >= 8 * capacity()) {
            grow();
        }
        // Constructing mapped object from args only if necessary
        return insert_with_key(key, std::piecewise_construct, std::forward_as_tuple(key),
                               std::forward_as_tuple(std::forward<Args>(args)...));
    }

    template <typename K>
    std::size_t count(const K& key) const noexcept(noexcept(search(key))) {
        return search(key) == capacity() ? 0 : 1;
    }

    template <typename K>
    const_iterator find(const K& key) const noexcept(noexcept(search(key))) {
        return iterator_for(search(key));
    }

    template <typename K>
    iterator find(const K& key) noexcept(noexcept(search(key))) {
        return iterator_for(search(key));
    }
};

}  // namespace stuff::container::detail


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

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

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

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

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
------------------ Author:       Marnix Volckaert               -------------------
------------------ Affiliation:  TUDelft                        -------------------
-----------------------------------------------------------------------------------
 Modified by Tiziana Sabatini on May-July 2009
*/

#include <QDir>
#include <QTextStream>

#include "EntryTrajectory.h"
#include "Astro-Core/EODE/eode.h"
#include <Eigen/Core>
#include <QProcess>
#include <QDebug>

/*Function containing the equations of motion*/
//=================================================================================================
void derivstate (VectorXd state, double time, VectorXd parameters, VectorXd& derivative)
{
        derivative[0] = state[3];
        derivative[1] = state[4];
        derivative[2] = state[5];
        derivative[3] = parameters[3] + parameters[0] + pow(parameters[6],2)*state[0] + 2.0*parameters[6]*state[4];
        derivative[4] = parameters[4] + parameters[1] + pow(parameters[6],2)*state[1] - 2.0*parameters[6]*state[3];
        derivative[5] = parameters[5] + parameters[2];
        derivative[6] = parameters[7];
        derivative[7] = parameters[8];
}

class EntryDerivativeCalculator : public DerivativeCalculator<8>
{
public:
    EntryDerivativeCalculator(EntryTrajectory *trajectory):
            m_trajectory(trajectory)
            //m_perturbationList(perturbationList)
    {
    }

    virtual void compute(const State& state, double t, State& derivatives) const
    {
        Vector3d position = state.segment<3>(0);
        Vector3d velocity = state.segment<3>(3);
        sta::StateVector stateVector=StateVector(position,velocity);
        Vector3d gravity;                   //Components of gravitational acceleration in appropriate elements
        Vector3d aero;                     	//Components of aerodynamic acceleration in appropriate elements
        VectorXd parameters(9);             //List of parameters to send to integration: 0,1,2 = gravity, 3,4,5 = aero, 6 = omega, 7 = Qdot
        double Cdc;
        double tau = m_trajectory->settings.stepsize;

        m_trajectory->body.updateGravity(stateVector, gravity);
        m_trajectory->body.updateAero(stateVector, m_trajectory->state2, m_trajectory->atmosphere, m_trajectory->capsule, aero, Cdc);
        /*
        Vector3d perturbingAcceleration(0, 0, 0);
        foreach (Perturbations* perturbation, perturbationList)
        {
            perturbingAcceleration += perturbation->calculateAcceleration(stateVector, time, tau);
        }
        gravity += perturbingAcceleration;
        */
        m_trajectory->calculateState2(aero, Cdc);
        m_trajectory->updateEndstate();

        //Test for status
        m_trajectory->checkStatus();
        if ( m_trajectory->status != OK) {
            m_trajectory->result.status = m_trajectory->status;
        }
        parameters << gravity, aero, m_trajectory->body.getOmega(), m_trajectory->state2(3), m_trajectory->state2(4);
        //TO DO : re-insert the heat load integration (now heat flux is always set to zero)
        VectorXd state3(8);
                state3 << stateVector.position, stateVector.velocity, 0, 0;

                derivatives[0] = state3[3];
                derivatives[1] = state3[4];
                derivatives[2] = state3[5];
                derivatives[3] = parameters[3] + parameters[0] + pow(parameters[6],2)*state3[0] + 2.0*parameters[6]*state3[4];
                derivatives[4] = parameters[4] + parameters[1] + pow(parameters[6],2)*state3[1] - 2.0*parameters[6]*state3[3];
                derivatives[5] = parameters[5] + parameters[2];
                derivatives[6] = parameters[7];
                derivatives[7] = parameters[8];
    }

private:
    EntryTrajectory* m_trajectory;

    //const QList<Perturbations*>& m_perturbationList;

};

EntryTrajectory::EntryTrajectory(EntrySettings _settings)
{
    //Set the entry trajectory's settings to the input settings
    //---------------------------------------------------------
    settings = _settings;
    //---------------------------------------------------------

    //Assign body, atmosphere and heat rate, and capsule Cd profile
    //---------------------------------------------------------
    atmosphere.selectModel(settings.modelname);
    body.selectBody(settings.bodyname);
    heatrate.selectBody(settings.bodyname);
    capsule.flag = 0;
    capsule.selectCdCprofile(settings.CdCprofilename);
    capsule.selectClCprofile(settings.ClCprofilename);
    capsule.selectCsCprofile(settings.CsCprofilename);

    //capsule.selectCdPprofile(settings.CdPprofilename);
    //capsule.Sp = settings.parachuteArea;

    state.resize(8);
    state2.resize(10);
}

sta::StateVector EntryTrajectory::initialise(EntryParameters _parameters,sta::StateVector initialState, double startTime)
{

    parameters = _parameters;           //Save the parameters as a member of the trajectory, to possibly use later in plotting

    capsule.Sc = parameters.Sref;
    capsule.m = parameters.m;
    capsule.Rn = parameters.Rn;

    double theta = getGreenwichHourAngle(startTime);
    int celestialbody;
    if (settings.bodyname == "Earth")
         celestialbody = 0;
    else if (settings.bodyname == "Mars")
         celestialbody = 3;
    //inertialTOfixed(celestialbody, theta, initialState.position.x(), initialState.position.y(), initialState.position.z(), initialState.velocity.x(), initialState.velocity.y(), initialState.velocity.z(),
      //          state(0), state(1), state(2), state(3), state(4), state(5));

    //const Eigen::Vector3d position(initialState.position.x(),initialState.position.y(),initialState.position.z());
    //const Eigen::Vector3d velocity(state(3), state(4), state(5));
    //initialState = sta::StateVector(position,velocity);
    double omega=7.29211585494e-5;
    initialState.velocity.x()+=initialState.position.y()*omega;
    initialState.velocity.y()+=-initialState.position.x()*omega;

    //----- Initialise the heat loads to zero
    state(6) = 0.0;
    state(7) = 0.0;
    //---------------------------------------------------------

    //Initialise the rest
    //---------------------------------------------------------
    time = sta::daysToSecs(sta::JdToMjd(startTime));
    for (int i=0; i<10; i++) {
        state2(i) = 0.0;
    }
    state2(5) = parameters.inputstate[0];          //Initial altitude
    status = OK;                                   //Status = OK
    result.Machone_transition_altitude = 0.0;      //Needs to be zero at first so that it only gets set once
    result.parachutedeploy_altitude = 0.0;         //Needs to be zero at first so that it only gets set once
    result.maxconvheatrate.value = 0.0;
    result.maxradheatrate.value = 0.0;
    result.maxtotalheatrate.value = 0.0;
    result.maxloadfactor.value = 0.0;

    trajectory.open("data/REMTrajectory.stae");

    return initialState;
}

Endstate EntryTrajectory::getEndstate()
{
    save = false;
    QList<double> sampleTimesTemp;
    QList<sta::StateVector> samplesTemp;
    //integrate(sampleTimesTemp, samplesTemp);
    return (result);
}

void EntryTrajectory::saveTrajectory (QList<double>& sampleTimes, QList<sta::StateVector>& samples)
{
    save = true;
    trajectory.open("data/REMTrajectory.stae");
    misc.open("data/REMMiscellaneous.stam");
    //integrate(sampleTimes, samples);
    misc.close();
    trajectory.close();
    printReport();
}

sta::StateVector EntryTrajectory::integrate(sta::StateVector stateVector, QList<Perturbations*> perturbationsList)//Modified by Dominic to improve integration stability
{
    double tau = settings.stepsize;

    /*
    Vector3d gravity;                   //Components of gravitational acceleration in appropriate elements
    Vector3d aero;                     	//Components of aerodynamic acceleration in appropriate elements
    VectorXd parameters(9);             //List of parameters to send to integration: 0,1,2 = gravity, 3,4,5 = aero, 6 = omega, 7 = Qdot
    double Cdc;

    //Update gravity, perturbations and aerodynamic accelerations
    body.updateGravity(stateVector, gravity);
    body.updateAero(stateVector, state2, atmosphere, capsule, aero, Cdc);

    //Vector3d perturbingAcceleration(0, 0, 0);
    //foreach (Perturbations* perturbation, perturbationsList)
    //{
    //    perturbingAcceleration += perturbation->calculateAcceleration(stateVector, time, tau);
    //}
    //gravity += perturbingAcceleration;

    calculateState2(aero, Cdc);
    updateEndstate();

    //Test for status
    checkStatus();
    if ( status != OK) {
        result.status = status;
    }

    //Build the parameters vector to send to equations of motion
    parameters << gravity, aero, body.getOmega(), state2(3), state2(4);
    //TO DO : re-insert the heat load integration (now heat flux is always set to zero)
    VectorXd state(8);
            state << stateVector.position, stateVector.velocity, 0, 0;

    //if (settings.integrator == "RK4")
        rk4 (state, 8, time, tau, derivstate, parameters);
    */
    EntryDerivativeCalculator entryCalculator(this);// entryCalculator(this,QList<Perturbations*> perturbationsList);
    Matrix<double, 8, 1> state;
    state<<stateVector.position, stateVector.velocity, 0, 0;
    rk4(state, time, tau, &entryCalculator);

    const Vector3d vec1(state(0), state(1), state(2));
    const Vector3d vec2(state(3), state(4), state(5));
    sta::StateVector vector = sta::StateVector(vec1, vec2);

    stateVector = vector;

    time += tau;

    //------ Calculate the rest of the endstate structure ------
    calculateEndstate();

    return stateVector;
}

void EntryTrajectory::calculateState2(Vector3d& aero, double Cdc)
{
    state2(2) = aero.norm() / 9.81; //n = magnitude of resultant vector of accelerations / g
    //------ calculate heat rate at stagnation point ------
    state2(3) = heatrate.ConvectiveHeatRate(state2(6), state2(0), capsule.Rn);
    state2(4) = heatrate.RadiativeHeatRate(state2(6), state2(0), capsule.Rn);
    state2(8) = state2(3) + state2(4);
    state2(9) = state(6) + state(7);
}

inline void EntryTrajectory::checkStatus()
{
    if (state2(5) < 0.0) {status = landed;};
    if (state2(5) > settings.maxaltitude) {status = gone;};     //If higher than x times initial altitude -> gone
    if (state2(8) > settings.maxheatrate) {status = burnt;};
    if (result.maxloadfactor.value > settings.maxloadfactor) {status = crushed;};
}

Endstate EntryTrajectory::updateEndstate()
{
    if (state2(1) < 1.0 && result.Machone_transition_altitude == 0.0) {
        result.Machone_transition_time = time;
        result.Machone_transition_altitude = state2(5);
    }//-------------------------------------
    if (state2(1) < settings.parachuteDeployMach && result.parachutedeploy_altitude == 0.0) {
        result.parachutedeploy_time = time;
        result.parachutedeploy_altitude = state2(5);
        state2(7) = 1.0;
    }//-------------------------------------
    if (state2(2) > result.maxloadfactor.value && state2(7) != 1.0) {         //find the maximum load factor
        result.maxloadfactor.value = state2(2);
        result.maxloadfactor.time = time;
        result.maxloadfactor.altitude = state2(5);
    }//-------------------------------------
    if (state2(3) > result.maxconvheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxconvheatrate.value = state2(3);
        result.maxconvheatrate.time = time;
        result.maxconvheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(4) > result.maxradheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxradheatrate.value = state2(4);
        result.maxradheatrate.time = time;
        result.maxradheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(8) > result.maxtotalheatrate.value) {         //find the maximum total heat rate
        result.maxtotalheatrate.value = state2(8);
        result.maxtotalheatrate.time = time;
        result.maxtotalheatrate.altitude = state2(5);
    }
    return(result);
}

Endstate EntryTrajectory::calculateEndstate() {

    //------ Update the maximum values one more time ------
    if (state2(2) > result.maxloadfactor.value) {         //find the maximum heat rate
        result.maxloadfactor.value = state2(2);
        result.maxloadfactor.time = time;
        result.maxloadfactor.altitude = state2(5);
    }//-------------------------------------
    if (state2(3) > result.maxconvheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxconvheatrate.value = state2(3);
        result.maxconvheatrate.time = time;
        result.maxconvheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(4) > result.maxradheatrate.value) {           //find the maximum heat rate at the stagnation point
        result.maxradheatrate.value = state2(3);
        result.maxradheatrate.time = time;
        result.maxradheatrate.altitude = state2(5);
    }//-------------------------------------
    if (state2(8) > result.maxtotalheatrate.value) {         //find the maximum total heat rate

        result.maxtotalheatrate.value = state2(4);
        result.maxtotalheatrate.time = time;
        result.maxtotalheatrate.altitude = state2(5);
    }
    //------ Compute time of flight, total heat load and impact velocity ------
    result.timeofflight = time;
    result.totalheatload = state2(9);
    result.tpsmass = (0.0009*pow(state(6),0.5306)) * parameters.m;
    result.impactvelocity = state2(6);

    //The following is only applicable for a successful landing
    //---------------------------------------------------------
    if (status == 0) {
        //------ Compute the landing longitude ------
        result.longitude = atan2(state(1), state(0)) / acos(-1.0) * 180;
        //------ Compute the landing latitude ------
        //lambda = asin( state(2) / sqrt(pow(state(0),2) + pow(state(1),2) + pow(state(2),2)) );
        //result.latitude = body.geocentric2geodetic_latitude(lambda) / acos(-1) * 180;
        result.latitude = asin( state(2) / sqrt(pow(state(0),2) + pow(state(1),2) + pow(state(2),2)) ) / acos(-1.0) * 180;
    }
    //---------------------------------------------------------
    return(result);
}

inline void EntryTrajectory::saveOutput (double time)
{

    double stateSpherical[6];
    cartesianTOspherical(state(0), state(1), state(2), state(3), state(4), state(5),
                         stateSpherical[0], stateSpherical[1], stateSpherical[2], stateSpherical[3], stateSpherical[4], stateSpherical[5]);
    stateSpherical[2] = body.altitude(stateSpherical[2] , stateSpherical[1] );
    double theta = getGreenwichHourAngle(sta::MjdToJd(sta::secsToDays(time)));
    double a,b,c,d,e,f;
    int celestialbody;
    if (settings.bodyname == "Earth")
         celestialbody = 0;
    else if (settings.bodyname == "Mars")
         celestialbody = 3;
    fixedTOinertial(celestialbody, theta, state(0), state(1), state(2), state(3), state(4), state(5),
                    a,b,c,d,e,f);
    double v = sqrt(d*d + e*e + f*f);
    double g,h,i;
    cartesianTOspherical(a, b, c, d, e, f,
                         g, h, i, stateSpherical[3], stateSpherical[4], stateSpherical[5]);

    trajectory << setfill('0');
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << state(0) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << state(1) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << state(2) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(8) << state(3) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(8) << state(4) << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(8) << state(5) << "     ";
        trajectory << stateSpherical[0]*1000 << "     ";
        trajectory << stateSpherical[1]*1000 << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << stateSpherical[2]*1000 << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << v << "     ";
        trajectory << stateSpherical[4]*1000 << "     ";
        trajectory << setiosflags(ios::fixed) << setprecision(2) << setw(10) << stateSpherical[5]*1000 << "     ";
        trajectory << state2[0]*1000 << "     ";//density
        trajectory << state2[1] << "     ";//Ma
        trajectory << state2[2]*1000 << "     ";//load factor
        trajectory << state2[3] << "     ";//qdot_c
        trajectory << state2[4] << "     ";//qdot_r
        trajectory << state[7] << "     ";//parachute
        trajectory << state2[8]*1000 << "     ";//totalheatrate
        trajectory << time << endl;
}

inline void EntryTrajectory::printReport ()
{

    angle longitude_angle;
    angle latitude_angle;
    angle entrylong_angle;
    angle entrylat_angle;
    AngleConversion angleconversion;
    //angleconversion.decimalToDegMinSec(result.longitude, longitude_angle.degrees, longitude_angle.minutes, longitude_angle.seconds);
    //angleconversion.decimalToDegMinSec(result.latitude, latitude_angle.degrees, latitude_angle.minutes, latitude_angle.seconds);
    angleconversion.decimalToDegMinSec(parameters.inputstate[1], entrylong_angle.degrees, entrylong_angle.minutes, entrylong_angle.seconds);
    angleconversion.decimalToDegMinSec(parameters.inputstate[2], entrylat_angle.degrees, entrylat_angle.minutes, entrylat_angle.seconds);

//    angle longitude_angle = double2angle (result.longitude);
//    angle latitude_angle = double2angle (result.latitude);
//    angle entrylong_angle = double2angle (parameters.inputstate[1]);
//    angle entrylat_angle = double2angle (parameters.inputstate[2]);
    //ofstream report ("C:/Users/Tizy/Desktop/Code/trunk/sta-src/Entry/REMReport.txt");
    ofstream report("data/REMReport.txt");

    //ofstream summary ("data/REMSummary.txt");

    switch (status)
       {
       case landed:
            cout << "Landed" << endl;
            report << "Status report: succesful landing" << endl;
            report << "================================" << endl;
            report << " " << endl;
            report << "Capsule data and initial conditions" << endl;
            report << "--------------------------------" << endl;
            //report << "Capsule base radius:\t" << parameters.R << " m" << endl; Make consistent with enw schema
            report << "Capsule mass:\t\t" << parameters.m << " kg" << endl;
            report << "Capsule nose radius:\t" << parameters.Rn << " m" << endl;
            report << "Entry altitude:\t\t" << parameters.inputstate[0] / 1000.0 << " km" << endl;
            report << "Entry longitude:\t" << entrylong_angle.degrees << "° " << entrylong_angle.minutes << "' " << entrylong_angle.seconds << "'' " << "(" << parameters.inputstate[1] << "°)" << endl;
            report << "Entry latitude:\t\t" << entrylat_angle.degrees << "° " << entrylat_angle.minutes << "' " << entrylat_angle.seconds << "'' " << "(" << parameters.inputstate[2] << "°)"<< endl;
            report << "Velocity:\t\t" << parameters.inputstate[3] << " m/s" << endl;
            report << "Flight path angle:\t" << parameters.inputstate[4] << " deg" << endl;
            report << "Heading:\t\t" << parameters.inputstate[5] << " deg" << endl;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Trajectory data" << endl;
            report << "--------------------------------" << endl;
            //report << "Impact longitude:\t" << longitude_angle.degrees << "° " << longitude_angle.minutes << "' " << longitude_angle.seconds << "'' " << "(" << result.longitude << "°)" << endl;
            //report << "Impact latitude:\t" << latitude_angle.degrees << "° " << latitude_angle.minutes << "' " << latitude_angle.seconds << "'' " << "(" << result.latitude << "°)" << endl;
            report << setprecision (3);
            report << "Impact velocity:\t" << result.impactvelocity << " m/s" << endl;
            report << fixed;
            report << "Total time of flight:\t" << result.timeofflight << " s" << endl;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Load data" << endl;
            report << "--------------------------------" << endl;
            report << "Maximum G load:\t\t\t" << result.maxloadfactor.value << " G (limit: " << settings.maxloadfactor << " G)" << endl;
            report << "Maximum convective heat rate:\t" << result.maxconvheatrate.value << " W/cm²" << endl;
            report << "Maximum radiative heat rate:\t" << result.maxradheatrate.value << " W/cm²" << endl;
            report << "Maximum total heat rate:\t" << result.maxtotalheatrate.value << " W/cm² (limit: " << settings.maxheatrate << " W/cm²)" << endl;
            report << "Total heat load:\t\t" << result.totalheatload << " J/cm²" << endl;
            report << setprecision (3);
            report << "TPS mass estimate:\t\t" << result.tpsmass << " kg (" << (0.0009 * pow(result.totalheatload,0.5306))*100.0 << " % of capsule mass)" << endl;
            report << fixed;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Entry profile: altitude" << endl;
            report << "--------------------------------" << endl;
            report << "Maximum radiative heat rate:\t" << result.maxradheatrate.altitude / 1000.0 << " km" << endl;
            report << "Maximum total heat rate:\t" << result.maxtotalheatrate.altitude / 1000.0 << " km" << endl;
            report << "Maximum convective heat rate:\t" << result.maxconvheatrate.altitude / 1000.0 << " km" << endl;
            report << "Maximum G:\t\t\t" << result.maxloadfactor.altitude / 1000.0 << " km" << endl;
            report << "Mach 1 transition:\t\t" << result.Machone_transition_altitude / 1000.0 << " km" << endl;
            report << "Parachute deployment:\t\t" << result.parachutedeploy_altitude / 1000.0 << " km (M " << settings.parachuteDeployMach << ")" << endl;
            report << "--------------------------------" << endl;
            report << " " << endl;
            report << "Entry profile: time (TE = time of entry, TI = time of impact)" << endl;
            report << "--------------------------------" << endl;
            report << "Maximum radiative heat rate:\tTE + " << result.maxradheatrate.time << " s, TI - " << (result.timeofflight - result.maxradheatrate.time) << " s" << endl;
            report << "Maximum total heat rate:\tTE + " << result.maxtotalheatrate.time << " s, TI - " << (result.timeofflight - result.maxtotalheatrate.time) << " s" << endl;
            report << "Maximum convective heat rate:\tTE + " << result.maxconvheatrate.time << " s, TI - " << (result.timeofflight - result.maxconvheatrate.time) << " s" << endl;
            report << "Maximum G:\t\t\tTE + " << result.maxloadfactor.time << " s, TI - " << (result.timeofflight - result.maxloadfactor.time) << " s" << endl;
            report << "Mach 1 transition:\t\tTE + " << result.Machone_transition_time << " s, TI - " << (result.timeofflight - result.Machone_transition_time) << " s" << endl;
            report << "Parachute deployment:\t\tTE + " << result.parachutedeploy_time << " s, TI - " << (result.timeofflight - result.parachutedeploy_time) << " s (M " << settings.parachuteDeployMach << ")" << endl;
            report << "================================";
            //    summary << longitude_angle.degrees << endl; summary << longitude_angle.minutes << endl; summary << longitude_angle.seconds << endl;
            //    summary << latitude_angle.degrees << endl; summary << latitude_angle.minutes << endl; summary << latitude_angle.seconds << endl;
            //    summary << result.maxconvheatrate.value << endl; summary << result.maxconvheatrate.altitude << endl; summary << result.maxconvheatrate.time << endl;
            //    summary << result.maxradheatrate.value << endl; summary << result.maxradheatrate.altitude << endl; summary << result.maxradheatrate.time << endl;
            //    summary << result.maxtotalheatrate.value << endl; summary << result.maxtotalheatrate.altitude << endl; summary << result.maxtotalheatrate.time << endl;
            //    summary << result.maxloadfactor.value << endl; summary << result.maxloadfactor.altitude << endl; summary << result.maxloadfactor.time << endl;
            //    summary << result.Machone_transition_altitude << endl; summary << result.Machone_transition_time << endl;
            //    summary << result.parachutedeploy_altitude << endl;  summary << result.parachutedeploy_time << endl;
            //    summary << result.totalheatload << endl; summary << result.timeofflight << endl; summary << result.impactvelocity << endl;
            break;
        case gone:
            cout << "Gone" << endl;
            report << "Status report: unsuccesful landing" << endl;
            report << "==================================" << endl;
            report << "The capsule reached maximum altitude after " << result.timeofflight << " s" << endl;
            break;
        case crushed:
            cout << "Crushed" << endl;
            break;
        case burnt:
            cout << "Burnt" << endl;
            break;
        case OK:
            cout << "In air" << endl;
            break;

        }

    report.close();
    //summary.close();
}


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include <NTL/lzz_pXFactoring.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include "FHE_operation.h"

#include <cassert>
#include <cstdio>
#include <iostream>
using namespace std;

class Transaction
{
public:

	Transaction(int l) : length(l) {t = new int[l];}
	Transaction(const Transaction &tr);
	~Transaction(){delete t;}

	int length;
	int *t;
};

class Data
{
 public:

	Data(char *filename);
	~Data();

	Transaction *getNext();

 private:

	FILE *in;
	char *fn;
};

//*********************************item.h***************************************
class Item
{
 public:

  Item(unsigned i, unsigned s = 0) : id(i), support(s) {}

  bool operator< (const Item &i) const {return support < i.support;}

  mutable vector<unsigned> transactions;
  unsigned id;
  unsigned support;
};
Transaction::Transaction(const Transaction &tr)
{
  length = tr.length;
  t = new int[tr.length];
  for(int i=0; i< length; i++)
    t[i] = tr.t[i];
}

Data::Data(char *filename)
{
  fn = filename;
  in = fopen(fn,"rt");
}

Data::~Data()
{
  if(in) fclose(in);
}

Transaction *Data::getNext()
{
  vector<int> list;
  char c;

  // read list of items
  do {
    int item=0, pos=0;
    c = getc(in);
    while((c >= '0') && (c <= '9')) {
      item *=10;
      item += int(c)-int('0');
      c = getc(in);
      pos++;
    }
    if(pos) list.push_back(item);
  }while(c != '\n' && !feof(in));

  if(feof(in)) return 0;
  // Note, also last transaction must end with newline,
  // else, it will be ignored

  // put items in Transaction structure
  Transaction *t = new Transaction(list.size());
  for(int i=0; i<int(list.size()); i++)
    t->t[i] = list[i];

  return t;
}

int main()
{
	long m = 0;          // 确定系数
	long p = 2147483647; // 模量(素数)，定义超过p/2的数为负数，负数的真值x=D[E[x]]-p
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	m = FindM(k, L, c, p, d, s, 0);

	FHEcontext context(m, p, r);
	buildModChain(context, L, c);
	ZZX G = context.alMod.getFactorsOverZZ()[0];

	// 生成公钥
	FHESecKey secretKey(context);
	const FHEPubKey& publicKey = secretKey;
	secretKey.GenSecKey(w);

	// 初始化密文
	Ctxt Ea(publicKey);
	Ctxt Eb(publicKey);
	Data *dt1 = new Data("./small_data.txt");

	unsigned tnr = 0;
	set<Item> root;
  	multiset<Item> *allitems = new multiset<Item>;
  	set<Item>::iterator it;

  	// read all transactions
  	while(Transaction *t = data->getNext()) {
    	for(int i=0; i<t->length;i++) {
      		it = root.find(Item(t->t[i],t->t[i]));
      		if(it == root.end())
      			it = root.insert(Item(t->t[i],t->t[i])).first;
      		it->transactions.push_back(tnr);
    	}
    	tnr++;
    	delete t;
  	}
  	for (int i = 0; i < it->transactions.size(); ++i)
  	{
  		cout << transactions[i] << " ";
  	}
  	cout << endl;

	return 0;
}


#include <iostream>
#include <valarray> // No nice indexing.
// We load Eigen just to have nice indexing & printing!
// Download Eigen & place Eigen/ -> ./Eigen/
//                        unsupported/ -> Eigen_unsupported/
#include <Eigen/Dense>
#include <Eigen_unsupported/Eigen/CXX11/Tensor>

using namespace std;
using namespace Eigen;

typedef TensorMap<Tensor<int64_t, 3>> TensorMap3d;
typedef Matrix<int64_t, 3, 1> Vector3i64;
typedef Matrix<int64_t, Dynamic, 1> VectorXi64;
typedef Matrix<int64_t, Dynamic, Dynamic> MatrixXi64;

extern "C" {
  int density_gradient(
                       double* point_ptr,
                       const int64_t nmo,
                       const int64_t natm,
                       const int64_t nprims,
                       const int64_t mgrp,
                       const int64_t ngto_h,
                       int64_t* ngroup_ptr,
                       int64_t* ityp_ptr,
                       int64_t* nzexp_ptr,
                       int64_t* nlm_ptr,
                       int64_t* nuexp_ptr,
                       double* occ_ptr,
                       double* oexp_ptr,
                       double* xyz_ptr,
                       double* rcutte_ptr,
                       double* coef_ptr,
                       double* grad_ptr
                      ) ;
}

int density_gradient(
                     double* point_ptr,
                     const int64_t nmo,
                     const int64_t natm,
                     const int64_t nprims,
                     const int64_t mgrp,
                     const int64_t ngto_h,
                     int64_t* ngroup_ptr,
                     int64_t* ityp_ptr,
                     int64_t* nzexp_ptr,
                     int64_t* nlm_ptr,
                     int64_t* nuexp_ptr,
                     double* occ_ptr,
                     double* oexp_ptr,
                     double* xyz_ptr,
                     double* rcutte_ptr,
                     double* coef_ptr,
                     double* grad_ptr
										) {

  // input & output variables
  Map<Vector3d>   point(point_ptr, 3);
  Map<VectorXi64> ngroup(ngroup_ptr, natm);
  Map<VectorXi64> ityp(ityp_ptr, nprims);
  Map<MatrixXi64> nzexp(nzexp_ptr, natm, mgrp);
  Map<MatrixXi64> nlm(nlm_ptr, 56, 3); // could be static ... => () -> []
  TensorMap3d     nuexp(nuexp_ptr, natm, mgrp, ngto_h);
  Map<VectorXd>   occ(occ_ptr, nmo);
  Map<VectorXd>   oexp(oexp_ptr, nprims);
  Map<MatrixXd>   xyz(xyz_ptr, natm, 3);
  Map<MatrixXd>   rcutte(rcutte_ptr, natm, mgrp);
  Map<MatrixXd>   coef(coef_ptr, 2*nmo, nprims);
  Map<Vector3d>   grad(grad_ptr, 3);
  // local variables
  Vector3d fun   = Vector3d::Zero(3);
  Vector3d fun1  = Vector3d::Zero(3);
  Vector3d xcoor = Vector3d::Zero(3);
  VectorXd gun   = VectorXd::Zero(nmo);
  MatrixXd gun1  = MatrixXd::Zero(nmo,3);
  int k, i, itip, n;
  double dis2, ori, dp2, aexp, x2, x;
  double f12, f123, fa, fb, fc, cfj;
  double fac, facgun;

  grad.setZero();

  // Run over centers
  for ( int ic=0; ic<natm; ic++) {
    // Atomic coordinates of this center
    xcoor(0) = point(0) - xyz(ic,0);
    xcoor(1) = point(1) - xyz(ic,1);
    xcoor(2) = point(2) - xyz(ic,2);
    dis2 = xcoor.cwiseAbs2().sum();
    // Loop over different shell in this atom
    for ( int m=0; m<ngroup(ic); m++) {
      k = nuexp(ic, m, 0)-1;
      // Skip to compute this primitive if distance is too big.
      if (dis2 > rcutte(ic, m) * rcutte(ic, m) ) {
        continue;
      }
      ori = -oexp(k);
      dp2 = 2.0*ori;
      // All primitives in a shell share the same exponent.
      aexp = exp( ori * dis2 );
      // Loop over the different primitives in this shell.
      for ( int jj=0; jj<nzexp(ic,m); jj++) {
        // "i" is the original index of the primitive in the WFN.
        i = nuexp(ic, m, jj)-1;
        itip = ityp(i)-1;
        // Integer coefficients.
        Vector3i64 it = Vector3i64::Zero();
        it(0) = nlm(itip,0);
        it(1) = nlm(itip,1);
        it(2) = nlm(itip,2);

        for ( int j=0; j<3; j++) {
          n = it(j);
          x = xcoor(j);
          if (n == 0) {
                  fun1(j) = dp2 * x;
                  fun(j) = 1.0;
          } else if (n == 1) {
                  fun1(j) = 1.0 + dp2 * x * x;
                  fun(j) = x;
          } else if (n == 2) {
                  x2 = x * x;
                  fun1(j) = x * ( 2.0 + dp2 * x2 );
                  fun(j) = x2;
          } else if (n == 3) {
                  x2 = x * x;
                  fun1(j) = x2 * ( 3.0 + dp2 * x2 );
                  fun(j) = x * x2;
          } else if (n == 4) {
                  x2 = x * x;
                  fun1(j) = x2 * x * ( 4.0 + dp2 * x2 );
                  fun(j) = x2 * x2;
          } else if (n == 5) {
                  x2 = x * x;
                  fun1(j) = x2 * x2 * ( 5.0 + dp2 * x2 );
                  fun(j) = x2 * x2 * x;
          }
        } // endfor j
        f12 = fun(0) * fun(1) * aexp;
        f123 = f12 * fun(2);
        fa = fun1(0) * fun(1) * fun(2) * aexp;
        fb = fun1(1) * fun(0) * fun(2) * aexp;
        fc = fun1(2) * f12;

        // run over orbitals
        for ( int j=0; j<nmo; j++) {
          cfj = coef(j,i);
          gun(j) = gun(j) + cfj * f123;
          gun1(j,0) += cfj * fa;
          gun1(j,1) += cfj * fb;
          gun1(j,2) += cfj * fc;
        }

      } // endfor jj
    } // endfor m
  } // endfor ic

  // Run again over orbitals
  for ( int i=0; i<nmo; i++) {
    fac = occ(i);
    facgun = fac * gun(i);
    for ( int j=0; j<3; j++) {
      grad(j) += facgun * gun1(i,j);
    }
  }
  grad *= 2.0;

  return EXIT_SUCCESS;
}