/*
  This ROS node creates an instance of the libdeepdive driver, which it uses
  to pull data from all available trackers, as well as lighthouse/tracker info.
*/

// ROS includes
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>

// General messages
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Imu.h>
#include <deepdive_ros/Trackers.h>
#include <deepdive_ros/Light.h>
#include <deepdive_ros/Lighthouses.h>

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

// UKF includes
#include <UKF/Types.h>
#include <UKF/Integrator.h>
#include <UKF/StateVector.h>
#include <UKF/MeasurementVector.h>
#include <UKF/Core.h>

// C++ includes
#include <vector>
#include <functional>

// Deepdive internal
#include "deepdive.hh"

// FILTER KEYS

// State indexes
enum Keys : uint8_t {
  // STATE
  Position,             // Position (world frame, m)
  Attitude,             // Attitude quaternion (rotates vec from body to world)
  Velocity,             // Velocity (body frame, m/s)
  Omega,                // Angular velocity (body frame, rads/s)
  Acceleration,         // Acceleration (body frame, m/s^2)
  Alpha,                // Angular acceleration (body frame, rads/s^2)
  // ERRORS
  GyroscopeBias,        // Gyroscope bias offset (body frame, rad/s)
  GyroscopeScale,       // Gyroscope scale factor (body frame, multiplier)
  AccelerometerBias,    // Accelerometer bias offset (body frame, m/s^2)
  AccelerometerScale,   // Accelerometer scale factor (body frame, mutliplier)
  // MEASUREMENTS
  Accelerometer,        // Acceleration (body frame, m/s^2)
  Gyroscope,            // Gyroscope (body frame, rads/s)
  Angle
};

// OBSERVATION

// Observation vector
using Observation = UKF::DynamicMeasurementVector<
  UKF::Field<Accelerometer, UKF::Vector<3>>,
  UKF::Field<Gyroscope, UKF::Vector<3>>,
  UKF::Field<Angle, real_t>
>;

// TRACKING FILTER

// State vector
using State = UKF::StateVector<
  UKF::Field<Position, UKF::Vector<3>>,
  UKF::Field<Attitude, UKF::Quaternion>,
  UKF::Field<Velocity, UKF::Vector<3>>,
  UKF::Field<Omega, UKF::Vector<3>>,
  UKF::Field<Acceleration, UKF::Vector<3>>,
  UKF::Field<Alpha, UKF::Vector<3>>
>;

// For tracking
using TrackingFilter = UKF::Core<
  State, Observation, UKF::IntegratorRK4
>;

// ERROR FILTER

// Parameters
using Error = UKF::StateVector<
  UKF::Field<AccelerometerBias, UKF::Vector<3>>,
  UKF::Field<AccelerometerScale, UKF::Vector<3>>,
  UKF::Field<GyroscopeBias, UKF::Vector<3>>,
  UKF::Field<GyroscopeScale, UKF::Vector<3>>
>;

// For parameter estimation
using ErrorFilter = UKF::Core<
  Error, Observation, UKF::IntegratorEuler
>;

// For IMU parameter estimation

typedef std::map<std::string, Eigen::Affine3d> TransformMap;
typedef std::map<std::string, ErrorFilter> ErrorMap;

// Default IMU errors
Eigen::Vector3d imu_cov_ab_;         // Initial covariance: Accel bias
Eigen::Vector3d imu_cov_as_;         // Initial covariance: Accel scale
Eigen::Vector3d imu_cov_gb_;         // Initial covariance: Gyro bias
Eigen::Vector3d imu_cov_gs_;         // Initial covariance: Gyro scale
Eigen::Vector3d imu_proc_ab_;        // Process noise: Accel bias
Eigen::Vector3d imu_proc_as_;        // Process noise: Accel scale
Eigen::Vector3d imu_proc_gb_;        // Process noise: Gyro bias
Eigen::Vector3d imu_proc_gs_;        // Process noise: Gyro scale

// GLOBAL DATA STRUCTURES

std::string calfile_ = "deepdive.tf2";
std::string frame_world_ = "world";     // World frame
std::string frame_vive_ = "vive";       // Vive frame
std::string frame_body_ = "body";       // EKF / external solution
std::string frame_truth_ = "truth";     // Vive solution

LighthouseMap lighthouses_;          // List of lighthouses
TrackerMap trackers_;                // List of trackers
ErrorMap errors_;                    // List of error filters
TrackingFilter filter_;              // Tracking filter
std::string frame_parent_;           // Parent frame, eg "world"
std::string frame_child_;            // Child frame, eg "truth"
double rate_ = 10.0;                 // Desired tracking rate in Hz
int thresh_count_ = 4;               // Min num measurements required per bundle
double thresh_angle_ = 60.0;         // Angle threshold in degrees
double thresh_duration_ = 1.0;       // Duration threshold in micorseconds
bool use_gyroscope_ = true;          // Input measurements from gyroscope
bool use_accelerometer_ = true;      // Input measurements from accelerometer
bool use_light_ = true;              // Input measurements from light
double registration_[6];             // World -> vive

// ROS publishers
ros::Publisher pub_pose_;
ros::Publisher pub_twist_;

// Default measurement errors
bool correct_ = false;               // Whether to correct light parameters
Eigen::Vector3d gravity_;            // Gravity
Eigen::Vector3d obs_cov_acc_;        // Measurement covariance: Accelerometer
Eigen::Vector3d obs_cov_gyr_;        // Measurement covariance: Gyroscope
double obs_cov_ang_;                 // Measurement covariance: Angle

// Intermediary data
Eigen::Affine3d wTv_;                // ALL: world -> vive
TransformMap vTl_;                   // ALL: vive -> lighthouse
TransformMap bTh_;                   // ALL: head -> body
TransformMap tTh_;                   // ALL: head -> tracking
TransformMap tTi_;                   // ALL: imu -> tracking

// Context data
struct Context {
  std::string lighthouse;            // Active lighthouse
  std::string tracker;               // Active tracker
  Eigen::Vector3d sensor;            // Active sensor
  uint8_t axis;                      // Active axis
};

// Are we initialized and ready to track
bool initialized_ = false;

// TRACKING FILTER

namespace UKF {

  // MEASURMENT

  template <>
  Observation::CovarianceVector Observation::measurement_covariance(
    (Observation::CovarianceVector() <<
      1.0e-4, 1.0e-4, 1.0e-4,   // Accel
      1.0e-6, 1.0e-6, 1.0e-6,   // Gyro
      1.0e-8).finished());

  // TRACKING FILTER

  // Standard 6DoF kinematics with constant Acceleration assumption
  template <> template <> State
  State::derivative<>() const {
    UKF::Quaternion omega_q;
    omega_q.vec() = get_field<Omega>() * 0.5;
    omega_q.w() = 0;
    State output;
    output.set_field<Position>(get_field<Velocity>());
    output.set_field<Velocity>(get_field<Attitude>() * get_field<Acceleration>());
    output.set_field<Acceleration>(UKF::Vector<3>(0, 0, 0));
    output.set_field<Attitude>(omega_q * get_field<Attitude>());
    output.set_field<Omega>(get_field<Alpha>());
    output.set_field<Alpha>(UKF::Vector<3>(0, 0, 0));
    return output;
  }

  // See http://www.mdpi.com/1424-8220/11/7/6771/htm
  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<State, Accelerometer, Error, Context>(
    State const& state, Error const& error, Context const& context) {
    Eigen::Affine3d iTb = tTi_[context.tracker].inverse()    // light -> imu
                        * tTh_[context.tracker]              // head -> light
                        * bTh_[context.tracker].inverse();   // body -> head
    Eigen::Vector3d r = iTb.translation();
    Eigen::Vector3d w = state.get_field<Omega>();
    return error.get_field<AccelerometerScale>().cwiseInverse().cwiseProduct(
        iTb.linear() * state.get_field<Acceleration>()
      + iTb.linear() * w.cross(w.cross(r))
      + iTb.linear() * (state.get_field<Attitude>().conjugate() * gravity_)
      - error.get_field<AccelerometerBias>());
  }

  // See http://www.mdpi.com/1424-8220/11/7/6771/htm
  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<State, Gyroscope, Error, Context>(
    State const& state, Error const& error, Context const& context) {
    Eigen::Affine3d iTb = tTi_[context.tracker].inverse()    // light -> imu
                        * tTh_[context.tracker]              // head -> light
                        * bTh_[context.tracker].inverse();   // body -> head
    return error.get_field<GyroscopeScale>().cwiseInverse().cwiseProduct(
        iTb.linear() * state.get_field<Omega>()
      - error.get_field<GyroscopeBias>());
  }

  // Lighthouse angle prediction
  template <> template <> real_t
  Observation::expected_measurement<State, Angle, Error, Context>(
    State const& state, Error const& error, Context const& context) {
    Eigen::Affine3d wTb;
    wTb.translation() = state.get_field<Position>();
    wTb.linear() = state.get_field<Attitude>().toRotationMatrix();
    UKF::Vector<3> x = wTv_.inverse()                      // world -> vive
                     * vTl_[context.lighthouse].inverse()  // vive -> lh
                     * wTb                                 // body -> world
                     * bTh_[context.tracker]               // head -> body
                     * tTh_[context.tracker].inverse()     // tracker -> head
                     * context.sensor;
    double xyz[3], ang[2];
    xyz[0] = x[0];
    xyz[1] = x[1];
    xyz[2] = x[2];
    Predict(lighthouses_[context.lighthouse].params, xyz, ang, correct_);
    return ang[context.axis];
  }

  // ERROR FILTER

  template <> template <> Error
  Error::derivative<>() const {
    return Error::Zero();
  }

  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<Error, Accelerometer, State, Context>(
    Error const& errors, State const& state, Context const& context) {
    return expected_measurement<State, Accelerometer, Error, Context>(
      state, errors, context);
  }

  template <> template <> UKF::Vector<3>
  Observation::expected_measurement<Error, Gyroscope, State, Context>(
    Error const& errors, State const& state, Context const& context) {
    return expected_measurement<State, Gyroscope, Error, Context>(
      state, errors, context);
  }

  template <> template <> real_t
  Observation::expected_measurement<Error, Angle, State, Context>(
    Error const& errors, State const& state, Context const& context) {
    return expected_measurement<State, Angle, Error, Context>(
      state, errors, context);
  }
}

// UTILITY FUNCTIONS

bool Delta(double & dt) {
  static ros::Time last = ros::Time::now(), now;
  now = ros::Time::now();
  dt = (now - last).toSec();
  if (dt > 0)
    last = now;
  return (dt > 0 && dt < 1.0);
}

// CALLBACKS

// This will be called at approximately 120Hz
// - Single lighthouse in 'A' mode : 120Hz (60Hz per axis)
// - Dual lighthouses in b/A or b/c modes : 120Hz (30Hz per axis)
void LightCallback(deepdive_ros::Light::ConstPtr const& msg) {
  static double dt;
  if (!use_light_ || !initialized_ || !Delta(dt))
    return;

  // Check that we are recording and that the tracker/lighthouse is ready
  TrackerMap::iterator tracker = trackers_.find(msg->header.frame_id);
  if (tracker == trackers_.end() || !tracker->second.ready) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker not found or ready");
    return;
  }

  // Check that we are recording and that the tracker/lighthouse is ready
  LighthouseMap::iterator lighthouse = lighthouses_.find(msg->lighthouse);
  if (lighthouse == lighthouses_.end() || !lighthouse->second.ready) {
    ROS_INFO_STREAM_THROTTLE(1, "Lighthouse not found or ready");
    return;
  }

  // Make sure we have a filter setup for this
  ErrorMap::iterator error = errors_.find(msg->header.frame_id);
  if (error == errors_.end()) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker error filter not initialized");
    return;
  }

  // Clean up the measurments
  std::vector<deepdive_ros::Pulse> data;
  for (size_t i = 0; i < msg->pulses.size(); i++) {
    // Basic sanity checks on the data
    if (fabs(msg->pulses[i].angle) > thresh_angle_ / 57.2958) {
      ROS_INFO_STREAM_THROTTLE(1.0, "Rejected based on angle");
      continue;
    }
    if (fabs(msg->pulses[i].duration) < thresh_duration_ / 1e6) {
      ROS_INFO_STREAM_THROTTLE(1.0, "Rejected based on duration");
      continue;
    }
    if (msg->pulses[i].sensor >= NUM_SENSORS) {
      ROS_INFO_STREAM_THROTTLE(1.0, "Rejected based on invalid sensor id");
      continue;
    }
    data.push_back(msg->pulses[i]);
  }
  if (thresh_count_ > 0 && data.size() < thresh_count_) {
    ROS_INFO_STREAM_THROTTLE(1, "Not enough data so skipping bundle.");
    return;
  }

  // Set the context correctly
  Context context;
  context.tracker = msg->header.frame_id;
  context.lighthouse = msg->lighthouse;
  context.axis = msg->axis;

  // Correct the error filter
  error->second.a_priori_step(dt);
  for (size_t i = 0; i < data.size(); i++) {
    // Set the context correctly
    context.sensor[0] = tracker->second.sensors[6 * data[i].sensor + 0];
    context.sensor[1] = tracker->second.sensors[6 * data[i].sensor + 1];
    context.sensor[2] = tracker->second.sensors[6 * data[i].sensor + 2];
    // Create the observation
    Observation obs;
    obs.set_field<Angle>(data[i].angle);
    error->second.innovation_step(obs, filter_.state, context);
  }
  error->second.a_posteriori_step();

  // Correct the tracking filter
  filter_.a_priori_step(dt);
  for (size_t i = 0; i < data.size(); i++) {
    // Set the context correctly
    context.sensor[0] = tracker->second.sensors[6 * data[i].sensor + 0];
    context.sensor[1] = tracker->second.sensors[6 * data[i].sensor + 1];
    context.sensor[2] = tracker->second.sensors[6 * data[i].sensor + 2];
    // Create the observation
    Observation obs;
    obs.set_field<Angle>(data[i].angle);
    filter_.innovation_step(obs, error->second.state, context);
  }
  filter_.a_posteriori_step();
}
// This will be called at approximately 250Hz
void ImuCallback(sensor_msgs::Imu::ConstPtr const& msg) {
  static double dt;
  if ((!use_accelerometer_ && !use_gyroscope_) || !initialized_ || !Delta(dt))
    return;

  // Check that we are recording and that the tracker/lighthouse is ready
  TrackerMap::iterator tracker = trackers_.find(msg->header.frame_id);
  if (tracker == trackers_.end() || !tracker->second.ready) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker not found or ready");
    return;
  }

  // Make sure we have a filter setup for this
  ErrorMap::iterator error = errors_.find(msg->header.frame_id);
  if (error == errors_.end()) {
    ROS_INFO_STREAM_THROTTLE(1, "Tracker error filter not initialized");
    return;
  }

  // Get the measurements
  Eigen::Vector3d acc(
    msg->linear_acceleration.x,
    msg->linear_acceleration.y,
    msg->linear_acceleration.z);
  Eigen::Vector3d gyr(
    msg->angular_velocity.x,
    msg->angular_velocity.y,
    msg->angular_velocity.z);

  // Set the context correctly
  Context context;
  context.tracker = msg->header.frame_id;

  // Create a measurement
  Observation obs;
  if (use_accelerometer_)
    obs.set_field<Accelerometer>(acc);
  if (use_gyroscope_)
    obs.set_field<Gyroscope>(gyr);

  // Step the parameter filter
  error->second.a_priori_step(dt);
  error->second.innovation_step(obs, filter_.state, context);
  error->second.a_posteriori_step();

  // Propagate the filter
  filter_.a_priori_step(dt);
  filter_.innovation_step(obs, error->second.state, context);
  filter_.a_posteriori_step();
}

// This will be called back at the desired tracking rate
void TimerCallback(ros::TimerEvent const& info) {
  static double dt;
  if (!initialized_ || !Delta(dt))
    return;

  // Propagate the filter forward
  filter_.a_priori_step(dt);

  // Debug
  /*
  ErrorMap::iterator it;
  for (it = errors_.begin(); it != errors_.end(); it++) {
    ROS_INFO_STREAM(it->first << ":");
    ROS_INFO_STREAM(it->second.state);
  }
  ROS_INFO_STREAM(filter_.state);
  ROS_INFO_STREAM("Filter:");
  ROS_INFO_STREAM(filter_.state);
  */

  // The filter relates WORLD and IMU frames
  ros::Time now = ros::Time::now();

  // Broadcast the tracker pose on TF2
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped tfs;
  tfs.header.stamp = now;
  tfs.header.frame_id = frame_world_;
  tfs.child_frame_id = frame_truth_;
  tfs.transform.translation.x = filter_.state.get_field<Position>()[0];
  tfs.transform.translation.y = filter_.state.get_field<Position>()[1];
  tfs.transform.translation.z = filter_.state.get_field<Position>()[2];
  tfs.transform.rotation.w = filter_.state.get_field<Attitude>().w();
  tfs.transform.rotation.x = filter_.state.get_field<Attitude>().x();
  tfs.transform.rotation.y = filter_.state.get_field<Attitude>().y();
  tfs.transform.rotation.z = filter_.state.get_field<Attitude>().z();
  SendDynamicTransform(tfs);

  // Broadcast the pose with covariance
  geometry_msgs::PoseWithCovarianceStamped pwcs;
  pwcs.header.stamp = now;
  pwcs.header.frame_id = frame_world_;
  pwcs.pose.pose.position.x = filter_.state.get_field<Position>()[0];
  pwcs.pose.pose.position.y = filter_.state.get_field<Position>()[1];
  pwcs.pose.pose.position.z = filter_.state.get_field<Position>()[2];
  pwcs.pose.pose.orientation.w = filter_.state.get_field<Attitude>().w();
  pwcs.pose.pose.orientation.x = filter_.state.get_field<Attitude>().x();
  pwcs.pose.pose.orientation.y = filter_.state.get_field<Attitude>().y();
  pwcs.pose.pose.orientation.z = filter_.state.get_field<Attitude>().z();
  for (size_t i = 0; i < 6; i++)
    for (size_t j = 0; j < 6; j++)
      pwcs.pose.covariance[i*6 + j] = filter_.covariance(i, j);
  pub_pose_.publish(pwcs);

  // Broadcast the twist with covariance
  geometry_msgs::TwistWithCovarianceStamped twcs;
  twcs.header.stamp = now;
  twcs.header.frame_id = frame_world_;
  twcs.twist.twist.linear.x = filter_.state.get_field<Velocity>()[0];
  twcs.twist.twist.linear.y = filter_.state.get_field<Velocity>()[1];
  twcs.twist.twist.linear.z = filter_.state.get_field<Velocity>()[2];
  twcs.twist.twist.angular.x = filter_.state.get_field<Omega>()[0];
  twcs.twist.twist.angular.y = filter_.state.get_field<Omega>()[1];
  twcs.twist.twist.angular.z = filter_.state.get_field<Omega>()[2];
  for (size_t i = 0; i < 6; i++)
    for (size_t j = 0; j < 6; j++)
      twcs.twist.covariance[i*6 + j] = filter_.covariance(6+i, 6+j);
  pub_twist_.publish(twcs);
}

void CheckIfReadyToTrack() {
  if (!initialized_) {
    TrackerMap::const_iterator it;
    for (it = trackers_.begin(); it != trackers_.end(); it++)
      if (!it->second.ready) return;
    LighthouseMap::const_iterator jt;
    for (jt = lighthouses_.begin(); jt != lighthouses_.end(); jt++)
      if (!jt->second.ready) return;
    ROS_INFO_STREAM("All trackers and lighthouses found. Tracking started.");
    initialized_ = true;
  }
}

// Convert an angle axis to an eigen transform
Eigen::Affine3d AngleAxisToTransform(double data[6]) {
  Eigen::Affine3d tf = Eigen::Affine3d::Identity();
  tf.translation() = Eigen::Vector3d(data[0], data[1], data[2]);
  Eigen::Vector3d v(data[3], data[4], data[5]);
  if (v.norm() > 0) {
    Eigen::AngleAxisd aa = Eigen::AngleAxisd::Identity();
    aa.angle() = v.norm();
    aa.axis() = v.normalized();
    tf.linear() = aa.toRotationMatrix();
  }
  return tf;
}

// Called when a new lighthouse appears
void NewLighthouseCallback(LighthouseMap::iterator lighthouse) {
  ROS_INFO_STREAM("Found lighthouse " << lighthouse->first);
  // Initialize
  vTl_[lighthouse->first] = AngleAxisToTransform(lighthouse->second.vTl);
  // Check if we have got all info from lighthouses and trackers
  CheckIfReadyToTrack();
}

// Called when a new tracker appears
void NewTrackerCallback(TrackerMap::iterator tracker) {
  ROS_INFO_STREAM("Found tracker " << tracker->first);
  // Initialize the error filter
  ErrorFilter & error = errors_[tracker->first];
  error.state.set_field<AccelerometerBias>(UKF::Vector<3>(
    tracker->second.errors[ERROR_ACC_BIAS][0],
    tracker->second.errors[ERROR_ACC_BIAS][1],
    tracker->second.errors[ERROR_ACC_BIAS][2]));
  error.state.set_field<AccelerometerScale>(UKF::Vector<3>(
    tracker->second.errors[ERROR_ACC_SCALE][0],
    tracker->second.errors[ERROR_ACC_SCALE][1],
    tracker->second.errors[ERROR_ACC_SCALE][2]));
  error.state.set_field<GyroscopeBias>(UKF::Vector<3>(
    tracker->second.errors[ERROR_GYR_BIAS][0],
    tracker->second.errors[ERROR_GYR_BIAS][1],
    tracker->second.errors[ERROR_GYR_BIAS][2]));
  error.state.set_field<GyroscopeScale>(UKF::Vector<3>(
    tracker->second.errors[ERROR_GYR_SCALE][0],
    tracker->second.errors[ERROR_GYR_SCALE][1],
    tracker->second.errors[ERROR_GYR_SCALE][2]));
  error.covariance = Error::CovarianceMatrix::Zero();
  error.covariance.diagonal() <<
    imu_cov_ab_, imu_cov_as_, imu_cov_gb_, imu_cov_gs_;
  error.process_noise_covariance = Error::CovarianceMatrix::Zero();
  error.process_noise_covariance.diagonal() <<
    imu_proc_ab_, imu_proc_as_, imu_proc_gb_, imu_proc_gs_;
  // Global cache
  bTh_[tracker->first] = AngleAxisToTransform(tracker->second.bTh);
  tTh_[tracker->first] = AngleAxisToTransform(tracker->second.tTh);
  tTi_[tracker->first] = AngleAxisToTransform(tracker->second.tTi);
  // Check if we have got all info from lighthouses and trackers
  CheckIfReadyToTrack();
}

// MAIN ENTRY POINT OF APPLICATION

bool GetPairParam(ros::NodeHandle &nh,
  std::string const& name, UKF::Vector<2> & data) {
  std::vector<double> tmp;
  if (!nh.getParam(name, tmp) || tmp.size() != 2)
    return false;
  data[0] = tmp[0];
  data[1] = tmp[1];
  return true;
}

bool GetVectorParam(ros::NodeHandle &nh,
  std::string const& name, UKF::Vector<3> & data) {
  std::vector<double> tmp;
  if (!nh.getParam(name, tmp) || tmp.size() != 3)
    return false;
  data[0] = tmp[0];
  data[1] = tmp[1];
  data[2] = tmp[2];
  return true;
}

bool GetQuaternionParam(ros::NodeHandle &nh,
  std::string const& name, UKF::Quaternion & data) {
  std::vector<double> tmp;
  if (!nh.getParam(name, tmp) || tmp.size() != 4)
    return false;
  data.x() = tmp[0];
  data.y() = tmp[1];
  data.z() = tmp[2];
  data.w() = tmp[3];
  return true;
}

int main(int argc, char **argv) {
  // Initialize ROS and create node handle
  ros::init(argc, argv, "deepdive_tracker");
  ros::NodeHandle nh("~");

  // Get the parent information
  if (!nh.getParam("calfile", calfile_))
    ROS_FATAL("Failed to get the calfile file.");

  // Get some global information
  if (!nh.getParam("frames/world", frame_world_))
    ROS_FATAL("Failed to get frames/world parameter.");
  if (!nh.getParam("frames/vive", frame_vive_))
    ROS_FATAL("Failed to get frames/vive parameter.");
  if (!nh.getParam("frames/body", frame_body_))
    ROS_FATAL("Failed to get frames/body parameter.");
  if (!nh.getParam("frames/truth", frame_truth_))
    ROS_FATAL("Failed to get frames/truth parameter.");

  // Get the parent information
  std::vector<std::string> lighthouses;
  if (!nh.getParam("lighthouses", lighthouses))
    ROS_FATAL("Failed to get the lighthouse list.");
  std::vector<std::string>::iterator it;
  for (it = lighthouses.begin(); it != lighthouses.end(); it++) {
    std::string serial;
    if (!nh.getParam(*it + "/serial", serial))
      ROS_FATAL("Failed to get the lighthouse serial.");
    std::vector<double> transform;
    if (!nh.getParam(*it + "/transform", transform))
      ROS_FATAL("Failed to get the lighthouse transform.");
    if (transform.size() != 7) {
      ROS_FATAL("Failed to parse lighthouse transform.");
      continue;
    }
    Eigen::Quaterniond q(
      transform[6],  // qw
      transform[3],  // qx
      transform[4],  // qy
      transform[5]); // qz
    Eigen::AngleAxisd aa(q);
    lighthouses_[serial].vTl[0] = transform[0];
    lighthouses_[serial].vTl[1] = transform[1];
    lighthouses_[serial].vTl[2] = transform[2];
    lighthouses_[serial].vTl[3] = aa.angle() * aa.axis()[0];
    lighthouses_[serial].vTl[4] = aa.angle() * aa.axis()[1];
    lighthouses_[serial].vTl[5] = aa.angle() * aa.axis()[2];
    lighthouses_[serial].ready = false;
  }

  // Get the parent information
  std::vector<std::string> trackers;
  if (!nh.getParam("trackers", trackers))
    ROS_FATAL("Failed to get the tracker list.");
  std::vector<std::string>::iterator jt;
  for (jt = trackers.begin(); jt != trackers.end(); jt++) {
    std::string serial;
    if (!nh.getParam(*jt + "/serial", serial))
      ROS_FATAL("Failed to get the tracker serial.");
    std::vector<double> extrinsics;
    if (!nh.getParam(*jt + "/extrinsics", extrinsics))
      ROS_FATAL("Failed to get the tracker extrinsics.");
    if (extrinsics.size() != 7) {
      ROS_FATAL("Failed to parse tracker extrinsics.");
      continue;
    }
    Eigen::Quaterniond q(
      extrinsics[6],  // qw
      extrinsics[3],  // qx
      extrinsics[4],  // qy
      extrinsics[5]); // qz
    Eigen::AngleAxisd aa(q);
    trackers_[serial].bTh[0] = extrinsics[0];
    trackers_[serial].bTh[1] = extrinsics[1];
    trackers_[serial].bTh[2] = extrinsics[2];
    trackers_[serial].bTh[3] = aa.angle() * aa.axis()[0];
    trackers_[serial].bTh[4] = aa.angle() * aa.axis()[1];
    trackers_[serial].bTh[5] = aa.angle() * aa.axis()[2];
    trackers_[serial].ready = false;
  }

  // Get the topics for data topics
  std::string topic_pose, topic_twist;
  if (!nh.getParam("topics/pose", topic_pose))
    ROS_FATAL("Failed to get topics/pose parameter.");
  if (!nh.getParam("topics/twist", topic_twist))
    ROS_FATAL("Failed to get topics/twist parameter.");

  // Get the thresholds
  if (!nh.getParam("thresholds/angle", thresh_angle_))
    ROS_FATAL("Failed to get thresholds/angle parameter.");
  if (!nh.getParam("thresholds/duration", thresh_duration_))
    ROS_FATAL("Failed to get thresholds/duration parameter.");
  if (!nh.getParam("thresholds/count", thresh_count_))
    ROS_FATAL("Failed to get thresholds/count parameter.");

  // Whether to apply light corrections
  if (!nh.getParam("correct", correct_))
    ROS_FATAL("Failed to get correct parameter.");

  // Get the tracker update rate.
  if (!nh.getParam("rate", rate_))
    ROS_FATAL("Failed to get rate parameter.");

  // Get the tracker update rate.
  if (!nh.getParam("use/gyroscope", use_gyroscope_))
    ROS_FATAL("Failed to get use/gyroscope  parameter.");
  if (!nh.getParam("use/accelerometer", use_accelerometer_))
    ROS_FATAL("Failed to get use/accelerometer  parameter.");
  if (!nh.getParam("use/light", use_light_))
    ROS_FATAL("Failed to get use/light parameter.");

  // Get gravity
  if (!GetVectorParam(nh, "gravity", gravity_))
    ROS_FATAL("Failed to get gravity parameter.");

  // Tracking filter: Initial estimates
  UKF::Vector<3> est_position(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/position", est_position))
    ROS_FATAL("Failed to get position parameter.");
  UKF::Quaternion est_attitude(1, 0, 0, 0);
  if (!GetQuaternionParam(nh, "initial_estimate/attitude", est_attitude))
    ROS_FATAL("Failed to get attitude parameter.");
  UKF::Vector<3> est_velocity(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/velocity", est_velocity))
    ROS_FATAL("Failed to get velocity parameter.");
  UKF::Vector<3> est_omega(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/omega", est_omega))
    ROS_FATAL("Failed to get angular_velocity parameter.");
  UKF::Vector<3> est_acceleration(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/acceleration", est_acceleration))
    ROS_FATAL("Failed to get acceleration parameter.");
  UKF::Vector<3> est_alpha(0, 0, 0);
  if (!GetVectorParam(nh, "initial_estimate/alpha", est_alpha))
    ROS_FATAL("Failed to get alpha parameter.");

  // Tracking filter:  Initial covariances
  UKF::Vector<3> cov_position(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/position", cov_position))
    ROS_FATAL("Failed to get position parameter.");
  UKF::Vector<3> cov_attitude(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/attitude", cov_attitude))
    ROS_FATAL("Failed to get attitude parameter.");
  UKF::Vector<3> cov_velocity(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/velocity", cov_velocity))
    ROS_FATAL("Failed to get velocity parameter.");
  UKF::Vector<3> cov_omega(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/omega", cov_omega))
    ROS_FATAL("Failed to get omega parameter.");
  UKF::Vector<3> cov_accel(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/acceleration", cov_accel))
    ROS_FATAL("Failed to get acceleration parameter.");
  UKF::Vector<3> cov_alpha(0, 0, 0);
  if (!GetVectorParam(nh, "initial_cov/alpha", cov_alpha))
    ROS_FATAL("Failed to get acceleration parameter.");

  // Tracking filter:  Noise
  UKF::Vector<3> noise_position(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/position", noise_position))
    ROS_FATAL("Failed to get position parameter.");
  UKF::Vector<3> noise_attitude(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/attitude", noise_attitude))
    ROS_FATAL("Failed to get attitude parameter.");
  UKF::Vector<3> noise_velocity(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/velocity", noise_velocity))
    ROS_FATAL("Failed to get velocity parameter.");
  UKF::Vector<3> noise_omega(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/omega", noise_omega))
    ROS_FATAL("Failed to get omega parameter.");
  UKF::Vector<3> noise_accel(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/acceleration", noise_accel))
    ROS_FATAL("Failed to get acceleration parameter.");
  UKF::Vector<3> noise_alpha(0, 0, 0);
  if (!GetVectorParam(nh, "process_noise_cov/alpha", noise_alpha))
    ROS_FATAL("Failed to get acceleration parameter.");

  // Setup the filter
  filter_.state.set_field<Position>(est_position);
  filter_.state.set_field<Attitude>(est_attitude);
  filter_.state.set_field<Velocity>(est_velocity);
  filter_.state.set_field<Omega>(est_omega);
  filter_.state.set_field<Acceleration>(est_acceleration);
  filter_.state.set_field<Alpha>(est_alpha);
  filter_.covariance = State::CovarianceMatrix::Zero();
  filter_.covariance.diagonal() <<
    cov_position, cov_attitude,
    cov_velocity, cov_omega,
    cov_accel, cov_alpha;
  filter_.process_noise_covariance = State::CovarianceMatrix::Zero();
  filter_.process_noise_covariance.diagonal() <<
    noise_position, noise_attitude,
    noise_velocity, noise_omega,
    noise_accel, noise_alpha;

  // IMU error : initial estimate
  if (!GetVectorParam(nh, "measurement_cov/accelerometer", obs_cov_acc_))
    ROS_FATAL("Failed to get accelerometer parameter.");
  if (!GetVectorParam(nh, "measurement_cov/gyroscope", obs_cov_gyr_))
    ROS_FATAL("Failed to get gyroscope parameter.");
  if (!nh.getParam("measurement_cov/angle", obs_cov_ang_))
    ROS_FATAL("Failed to get angle parameter.");

  // IMU error : initial estimate
  if (!GetVectorParam(nh, "imu_initial_cov/acc_bias", imu_cov_ab_))
    ROS_FATAL("Failed to get acc_bias parameter.");
  if (!GetVectorParam(nh, "imu_initial_cov/acc_scale", imu_cov_as_))
    ROS_FATAL("Failed to get acc_scale parameter.");
  if (!GetVectorParam(nh, "imu_initial_cov/gyr_bias", imu_cov_gb_))
    ROS_FATAL("Failed to get gyr_bias parameter.");
  if (!GetVectorParam(nh, "imu_initial_cov/gyr_scale", imu_cov_gs_))
    ROS_FATAL("Failed to get gyr_scale parameter.");

  // IMU error : initial estimate
  if (!GetVectorParam(nh, "imu_process_noise_cov/acc_bias", imu_proc_ab_))
    ROS_FATAL("Failed to get acc_bias parameter.");
  if (!GetVectorParam(nh, "imu_process_noise_cov/acc_scale", imu_proc_as_))
    ROS_FATAL("Failed to get acc_scale parameter.");
  if (!GetVectorParam(nh, "imu_process_noise_cov/gyr_bias", imu_proc_gb_))
    ROS_FATAL("Failed to get gyr_bias parameter.");
  if (!GetVectorParam(nh, "imu_process_noise_cov/gyr_scale", imu_proc_gs_))
    ROS_FATAL("Failed to get gyr_scale parameter.");

  // If reading the configuration file results in inserting the correct
  // number of static transforms into the problem, then we can publish
  // the solution for use by other entities in the system.
  if (ReadConfig(calfile_, frame_world_, frame_vive_, frame_body_,
    registration_, lighthouses_, trackers_)) {
    ROS_INFO("Read transforms from calibration");
  } else {
    ROS_INFO("Could not read calibration file");
  }
  SendTransforms(frame_world_, frame_vive_, frame_body_,
    registration_, lighthouses_, trackers_);

  // Convert the registration info to a world transform
  wTv_ = AngleAxisToTransform(registration_);

  // Markers showing sensor positions
  pub_pose_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>
    (topic_pose, 0);
  pub_twist_ = nh.advertise<geometry_msgs::TwistWithCovarianceStamped>
    (topic_twist, 0);

  // Subscribe to the motion and light callbacks
  std::vector<ros::Subscriber> subs;
  subs.push_back(nh.subscribe<deepdive_ros::Trackers>("/trackers", 1000,
    std::bind(TrackerCallback, std::placeholders::_1,
      std::ref(trackers_), NewTrackerCallback)));
  subs.push_back(nh.subscribe<deepdive_ros::Lighthouses>("/lighthouses", 1000,
    std::bind(LighthouseCallback, std::placeholders::_1,
      std::ref(lighthouses_), NewLighthouseCallback)));
  subs.push_back(nh.subscribe("/light", 1000, LightCallback));
  subs.push_back(nh.subscribe("/imu", 1000, ImuCallback));

  // Start a timer to callback
  ros::Timer timer = nh.createTimer(
    ros::Duration(ros::Rate(rate_)), TimerCallback, false, true);

  // Block until safe shutdown
  ros::spin();

  // Success!
  return 0;
}


#pragma once

#include "traffic/speed_groups.hpp"

#include "platform/location.hpp"

#include "geometry/latlon.hpp"

#include "coding/point_coding.hpp"
#include "coding/reader.hpp"
#include "coding/varint.hpp"
#include "coding/writer.hpp"
#include "coding/zlib.hpp"

#include "base/checked_cast.hpp"
#include "base/logging.hpp"

#include <cstdint>
#include <exception>
#include <iterator>
#include <type_traits>
#include <utility>
#include <vector>

#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdeprecated-copy"
#endif

#include <boost/circular_buffer.hpp>

#if defined(__clang__)
#pragma clang diagnostic pop
#endif

namespace
{
size_t constexpr kCoordBits = 32;
}  // namespace

namespace tracking
{
namespace helpers
{
struct Restrictions
{
  static double const kMinDeltaLat;
  static double const kMaxDeltaLat;
  static double const kMinDeltaLon;
  static double const kMaxDeltaLon;
};

struct Limits
{
  Limits() = delete;

  double m_minLat;
  double m_maxLat;
  double m_minLon;
  double m_maxLon;
};

Limits GetLimits(bool isDelta);
} // namespace helpers

struct Packet
{
  Packet();
  explicit Packet(location::GpsInfo const & info);
  Packet(double lat, double lon, uint32_t timestamp);

  double m_lat;
  double m_lon;
  uint32_t m_timestamp;
};

struct PacketCar : Packet
{
  PacketCar();
  PacketCar(location::GpsInfo const & info, traffic::SpeedGroup const & speedGroup);
  PacketCar(double lat, double lon, uint32_t timestamp, traffic::SpeedGroup speed);

  traffic::SpeedGroup m_speedGroup;
};

template <typename Pack>
class BasicArchive
{
public:
  BasicArchive(size_t maxSize, double minDelaySeconds);

  template <typename Writer>
  bool Write(Writer & dst);

  template <typename Reader>
  bool Read(Reader & src);

  template <typename... PackParameters>
  bool Add(PackParameters &&... params);

  size_t Size() const;
  bool ReadyToDump() const;
  std::vector<Pack> Extract() const;

private:
  boost::circular_buffer<Pack> m_buffer;
  double const m_minDelaySeconds;
};

// Abstract packet traits.
template <typename Pack>
struct TraitsPacket
{
};

template <>
struct TraitsPacket<Packet>
{
  template <typename Writer>
  static void Write(Writer & writer, Packet const & packet, bool isDelta)
  {
    auto const lim = helpers::GetLimits(isDelta);

    WriteVarUint(writer, DoubleToUint32(packet.m_lat, lim.m_minLat, lim.m_maxLat, kCoordBits));
    WriteVarUint(writer, DoubleToUint32(packet.m_lon, lim.m_minLon, lim.m_maxLon, kCoordBits));
    WriteVarUint(writer, packet.m_timestamp);
  }

  template <typename Reader>
  static Packet Read(Reader & src, bool isDelta)
  {
    auto const lim = helpers::GetLimits(isDelta);

    double const lat =
        Uint32ToDouble(ReadVarUint<uint32_t>(src), lim.m_minLat, lim.m_maxLat, kCoordBits);
    double const lon =
        Uint32ToDouble(ReadVarUint<uint32_t>(src), lim.m_minLon, lim.m_maxLon, kCoordBits);
    uint32_t const timestamp = ReadVarUint<uint32_t>(src);
    return Packet(lat, lon, timestamp);
  }

  static Packet GetDelta(Packet const & current, Packet const & previous)
  {
    uint32_t const deltaTimestamp = current.m_timestamp - previous.m_timestamp;
    double const deltaLat = current.m_lat - previous.m_lat;
    double const deltaLon = current.m_lon - previous.m_lon;
    return Packet(deltaLat, deltaLon, deltaTimestamp);
  }

  static Packet Combine(Packet const & previous, Packet const & delta)
  {
    double const lat = previous.m_lat + delta.m_lat;
    double const lon = previous.m_lon + delta.m_lon;
    uint32_t const timestamp = previous.m_timestamp + delta.m_timestamp;
    return Packet(lat, lon, timestamp);
  }

  static traffic::SpeedGroup GetSpeedGroup(Packet const & packet)
  {
    return traffic::SpeedGroup::Unknown;
  }
};

template <>
struct TraitsPacket<PacketCar>
{
  template <typename Writer>
  static void Write(Writer & writer, PacketCar const & packet, bool isDelta)
  {
    TraitsPacket<Packet>::Write(writer, packet, isDelta);
    static_assert(
        std::is_same<uint8_t, std::underlying_type_t<decltype(packet.m_speedGroup)>>::value, "");
    WriteVarUint(writer, static_cast<uint8_t>(packet.m_speedGroup));
  }

  template <typename Reader>
  static PacketCar Read(Reader & src, bool isDelta)
  {
    Packet const base = TraitsPacket<Packet>::Read(src, isDelta);
    auto const speedGroup = static_cast<traffic::SpeedGroup>(ReadPrimitiveFromSource<uint8_t>(src));
    return PacketCar(base.m_lat, base.m_lon, base.m_timestamp, speedGroup);
  }

  static PacketCar GetDelta(PacketCar const & current, PacketCar const & previous)
  {
    Packet const delta = TraitsPacket<Packet>::GetDelta(current, previous);
    return PacketCar(delta.m_lat, delta.m_lon, delta.m_timestamp, current.m_speedGroup);
  }

  static PacketCar Combine(PacketCar const & previous, PacketCar const & delta)
  {
    Packet const base = TraitsPacket<Packet>::Combine(previous, delta);
    return PacketCar(base.m_lat, base.m_lon, base.m_timestamp, delta.m_speedGroup);
  }

  static traffic::SpeedGroup GetSpeedGroup(PacketCar const & packet)
  {
    return packet.m_speedGroup;
  }
};

template <typename Reader>
std::vector<uint8_t> InflateToBuffer(Reader & src)
{
  std::vector<uint8_t> deflatedBuf(base::checked_cast<size_t>(src.Size()));
  src.Read(deflatedBuf.data(), deflatedBuf.size());

  coding::ZLib::Inflate inflate(coding::ZLib::Inflate::Format::ZLib);
  std::vector<uint8_t> buffer;
  inflate(deflatedBuf.data(), deflatedBuf.size(), std::back_inserter(buffer));
  return buffer;
}

template <typename Writer>
void DeflateToDst(Writer & dst, std::vector<uint8_t> const & buf)
{
  coding::ZLib::Deflate deflate(coding::ZLib::Deflate::Format::ZLib,
                                coding::ZLib::Deflate::Level::BestCompression);
  std::vector<uint8_t> deflatedBuf;
  deflate(buf.data(), buf.size(), std::back_inserter(deflatedBuf));
  dst.Write(deflatedBuf.data(), deflatedBuf.size());
}

template <typename Pack>
BasicArchive<Pack>::BasicArchive(size_t maxSize, double minDelaySeconds)
  : m_buffer(maxSize), m_minDelaySeconds(minDelaySeconds)
{
}

template <typename Pack>
template <typename... PackParameters>
bool BasicArchive<Pack>::Add(PackParameters &&... params)
{
  Pack newPacket = Pack(std::forward<PackParameters>(params)...);
  if (!m_buffer.empty() && newPacket.m_timestamp < m_buffer.back().m_timestamp + m_minDelaySeconds)
    return false;

  m_buffer.push_back(std::move(newPacket));
  return true;
}

template <typename Pack>
size_t BasicArchive<Pack>::Size() const
{
  return m_buffer.size();
}

template <typename Pack>
bool BasicArchive<Pack>::ReadyToDump() const
{
  return m_buffer.full();
}

template <typename Pack>
std::vector<Pack> BasicArchive<Pack>::Extract() const
{
  std::vector<Pack> res;
  res.reserve(m_buffer.size());
  res.insert(res.end(), m_buffer.begin(), m_buffer.end());
  return res;
}

template <typename Pack>
template <typename Writer>
bool BasicArchive<Pack>::Write(Writer & dst)
{
  if (m_buffer.empty())
    return false;

  try
  {
    std::vector<uint8_t> buf;
    MemWriter<std::vector<uint8_t>> writer(buf);
    // For aggregating tracks we use two-step approach: at first we delta-code the coordinates and
    // timestamps; then we use coding::ZLib::Inflate to compress data.
    TraitsPacket<Pack>::Write(writer, m_buffer[0], false /* isDelta */);

    for (size_t i = 1; i < m_buffer.size(); ++i)
    {
      Pack const delta = TraitsPacket<Pack>::GetDelta(m_buffer[i], m_buffer[i - 1]);
      TraitsPacket<Pack>::Write(writer, delta, true /* isDelta */);
    }

    DeflateToDst(dst, buf);

    LOG(LDEBUG, ("Dumped to disk", m_buffer.size(), "items"));

    m_buffer.clear();
    return true;
  }
  catch (std::exception const & e)
  {
    LOG(LWARNING, ("Error writing to file", e.what()));
  }
  return false;
}

template <typename Pack>
template <typename Reader>
bool BasicArchive<Pack>::Read(Reader & src)
{
  try
  {
    auto const buffer = InflateToBuffer(src);
    ReaderSource<MemReaderWithExceptions> reader(MemReaderWithExceptions(buffer.data(), buffer.size()));
    if (reader.Size() == 0)
      return false;

    m_buffer.clear();

    // Read first point.
    m_buffer.push_back(TraitsPacket<Pack>::Read(reader, false /* isDelta */));

    // Read with delta.
    while (reader.Size() > 0)
    {
      Pack const delta = TraitsPacket<Pack>::Read(reader, true  /* isDelta */);
      m_buffer.push_back(TraitsPacket<Pack>::Combine(m_buffer.back(), delta));
    }
    return true;
  }
  catch (std::exception const & e)
  {
    LOG(LWARNING, ("Error reading file", e.what()));
  }
  return false;
}
}  // namespace tracking


// Copyright (c) 2014-2015 The Pebblecoin developers
// Copyright (c) 2012-2013 The Cryptonote developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <algorithm>
#include <cassert>
#include <cstddef>
#include <cstdint>
#include <vector>

#include "common/int-util.h"
#include "crypto/hash.h"
#include "cryptonote_config.h"
#include "difficulty.h"

#include <boost/algorithm/clamp.hpp>

namespace cryptonote {

  using std::size_t;
  using std::uint64_t;
  using std::vector;

#if defined(_MSC_VER)
#include <windows.h>
#include <winnt.h>

  static inline void mul(uint64_t a, uint64_t b, uint64_t &low, uint64_t &high) {
    low = mul128(a, b, &high);
  }

#else

  static inline void mul(uint64_t a, uint64_t b, uint64_t &low, uint64_t &high) {
    typedef unsigned __int128 uint128_t;
    uint128_t res = (uint128_t) a * (uint128_t) b;
    low = (uint64_t) res;
    high = (uint64_t) (res >> 64);
  }

#endif

  static inline bool cadd(uint64_t a, uint64_t b) {
    return a + b < a;
  }

  static inline bool cadc(uint64_t a, uint64_t b, bool c) {
    return a + b < a || (c && a + b == (uint64_t) -1);
  }

  bool check_hash(const crypto::hash &hash, difficulty_type difficulty) {
    uint64_t low, high, top, cur;
    // First check the highest word, this will most likely fail for a random hash.
    mul(swap64le(((const uint64_t *) &hash)[3]), difficulty, top, high);
    if (high != 0) {
      return false;
    }
    mul(swap64le(((const uint64_t *) &hash)[0]), difficulty, low, cur);
    mul(swap64le(((const uint64_t *) &hash)[1]), difficulty, low, high);
    bool carry = cadd(cur, low);
    cur = high;
    mul(swap64le(((const uint64_t *) &hash)[2]), difficulty, low, high);
    carry = cadc(cur, low, carry);
    carry = cadc(high, top, carry);
    return !carry;
  }

  difficulty_type next_difficulty(uint64_t height, vector<uint64_t> timestamps, vector<difficulty_type> cumulative_difficulties, size_t target_seconds) {
    //cutoff DIFFICULTY_LAG
    if(timestamps.size() > DIFFICULTY_WINDOW)
    {
      timestamps.resize(DIFFICULTY_WINDOW);
      cumulative_difficulties.resize(DIFFICULTY_WINDOW);
    }

    size_t length = timestamps.size();
    assert(length == cumulative_difficulties.size());
    if (length <= 1) {
      return 1;
    }
    static_assert(DIFFICULTY_WINDOW >= 2, "Window is too small");
    assert(length <= DIFFICULTY_WINDOW);

    vector<std::pair<int64_t, size_t> > timespans_indices(timestamps.size() - 1);
    vector<difficulty_type> difficulties(timestamps.size() - 1);
    for (size_t i=0; i < timestamps.size() - 1; i++)
    {
      int64_t timespan_diff;
      if (height >= BOULDERHASH_2_SWITCH_BLOCK)
      {
        if (timestamps[i] > timestamps[i + 1])
          timespan_diff = 0;
        else
          timespan_diff = timestamps[i + 1] - timestamps[i];
      }
      else
      {
        timespan_diff = timestamps[i + 1] - timestamps[i];
      }
      timespans_indices[i] = std::make_pair(timespan_diff, i);
      difficulties[i] = cumulative_difficulties[i + 1] - cumulative_difficulties[i];
    }

    sort(timespans_indices.begin(), timespans_indices.end());

    size_t cut_begin, cut_end;
    static_assert(2 * DIFFICULTY_CUT <= DIFFICULTY_WINDOW - 2, "Cut length is too large");
    if (length - 1 <= DIFFICULTY_WINDOW - 2 * DIFFICULTY_CUT) {
      cut_begin = 0;
      cut_end = length - 1;
    } else {
      cut_begin = (length - 1 - (DIFFICULTY_WINDOW - 2 * DIFFICULTY_CUT) + 1) / 2;
      cut_end = cut_begin + (DIFFICULTY_WINDOW - 2 * DIFFICULTY_CUT);
    }
    assert(cut_begin < cut_end && cut_end <= length);

    uint64_t time_span = 0;
    difficulty_type total_work = 0;
    for (size_t i=cut_begin; i < cut_end; i++)
    {
      time_span += timespans_indices[i].first;
      total_work += difficulties[timespans_indices[i].second];
    }
    if (time_span == 0) {
      time_span = 1;
    }
    assert(total_work > 0);

    uint64_t low, high;
    mul(total_work, target_seconds, low, high);

    difficulty_type raw_res;
    if (high != 0 || low + time_span - 1 < low) {
      raw_res = 0;
    }
    else
    {
      raw_res = (low + time_span - 1) / time_span;
    }

    difficulty_type res = boost::algorithm::clamp(raw_res,
                                                  total_work / 4 / difficulties.size(),
                                                  total_work * 4 / difficulties.size());

    if (height >= BOULDERHASH_2_SWITCH_BLOCK && height < BOULDERHASH_2_SWITCH_BLOCK + BOULDERHASH_2_SWITCH_EASY_WINDOW)
    {
      //make difficulty easier for start of new algorithm
      const uint32_t window_size = BOULDERHASH_2_SWITCH_EASY_WINDOW;
      const uint32_t window_end = BOULDERHASH_2_SWITCH_BLOCK + BOULDERHASH_2_SWITCH_EASY_WINDOW;
      difficulty_type old = res;
      res = res * window_size / (window_size + 14 * (window_end - height));
      LOG_PRINT_L1("Difficulty for height " << height << " would be " << old << ", but switched to " << res);
    }

    if (res < 1) res = 1;

    LOG_PRINT_L3("time_span for " << (cut_end - cut_begin) << " blocks was " << time_span << "s, total work was " << total_work << ", low=" << low << ", high=" << high << ", raw_res=" << raw_res << ", res=" << res);

    return res;
  }

  difficulty_type next_difficulty(uint64_t height, vector<uint64_t> timestamps, vector<difficulty_type> cumulative_difficulties)
  {
    return next_difficulty(height, std::move(timestamps), std::move(cumulative_difficulties),
                           cryptonote::config::difficulty_target());
  }
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#include <boost/simd/function/scalar/asinpi.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/generic/function/asinpi.hpp>

#endif


#include <boost/multi_index_container.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/composite_key.hpp>
#include <boost/multi_index/random_access_index.hpp>
#include <boost/multi_index/sequenced_index.hpp>

#include <algorithm>
#include <iostream>
#include <random>
#include <chrono>
#include <set>
#include <unordered_set>
#include <memory>
#include <deque>
#include "Timer.hpp"

class Matrix {
 public:
  /* ///////////////////////////////////////////////////////////////////
  basic types
  */ ///////////////////////////////////////////////////////////////////
  using Index = uint32_t;
  using Value = std::complex<double>;
  /* ///////////////////////////////////////////////////////////////////
  type T to store in container
  */ ///////////////////////////////////////////////////////////////////
  class T {
   friend class Matrix;
   public:
    Index x_; Index y_; mutable Value v_;
    T() {}
    T(const Index& x, const Index& y, const Value& v ) : x_(x), y_(y), v_(v) {}
    T(const Index& x, const Index& y, const double& r, const double& i ) {
      x_ = x; y_ = y; v_ = std::complex<double>(r,i);
    }
    std::string to_string() const {
      std::ostringstream oss;
      oss.precision(2);
      oss << std::fixed ;
      oss << "(" << x_ << "," << y_ << ")" << " ";
      oss << "(" << v_.real() << "," << v_.imag() << ")";
      return oss.str();
    }
    //std::vector<double> res = std::vector<double>(1<<5);
  };
  /* ///////////////////////////////////////////////////////////////////
  Tags
  */ ///////////////////////////////////////////////////////////////////
  struct order_xy {};
  struct order_yx {};
  struct random_access {};
  /* ///////////////////////////////////////////////////////////////////
  Type for Boost's Multi-Index Container
  */ ///////////////////////////////////////////////////////////////////
  using Container = boost::multi_index_container<
    T, // the data type stored
    boost::multi_index::indexed_by<
      boost::multi_index::random_access<
        boost::multi_index::tag<random_access>
      >
      ,
      boost::multi_index::ordered_unique<
        boost::multi_index::tag<order_xy>,
        boost::multi_index::composite_key<
          T,
          boost::multi_index::member<T,Index,&T::x_>,
          boost::multi_index::member<T,Index,&T::y_>
        >,
        boost::multi_index::composite_key_compare<
          std::less<Index>,
          std::less<Index>
        >
      >
      ,
      boost::multi_index::ordered_unique<
        boost::multi_index::tag<order_yx>,
        boost::multi_index::composite_key<
          T,
          boost::multi_index::member<T,Index,&T::y_>,
          boost::multi_index::member<T,Index,&T::x_>
        >,
        boost::multi_index::composite_key_compare<
          std::less<Index>,
          std::less<Index>
        >
      >
      /*
      */
    >
  >;
  /* ///////////////////////////////////////////////////////////////////
  variables
  */ ///////////////////////////////////////////////////////////////////
  Container container_;
  Index index_max_ = UINT32_MAX;
  /* ///////////////////////////////////////////////////////////////////
  implicit methods
  */ ///////////////////////////////////////////////////////////////////
  void insert(const Matrix::Index& x, const Matrix::Index& y,
    const Matrix::Value& v);
  std::string to_string() const;
  Container::index<Matrix::random_access>::type::iterator
    random_access_begin();
  Container::index<Matrix::random_access>::type::iterator
    random_access_end();
  void clear();
  void reserve(Index m);
};

/* ///////////////////////////////////////////////////////////////////
explicit methods
*/ ///////////////////////////////////////////////////////////////////
void Matrix::reserve(Index m) {
  container_.get<random_access>().reserve(m);
}

void Matrix::clear() {
  container_.clear();
}

Matrix::Container::index<Matrix::random_access>::type::iterator
Matrix::random_access_begin() {
  return container_.get<random_access>().begin();
}

Matrix::Container::index<Matrix::random_access>::type::iterator
Matrix::random_access_end() {
  return container_.get<random_access>().end();
}

std::string Matrix::to_string() const {
  auto it_random_access = container_.get<random_access>().cbegin();
  std::string tmp = "";
  tmp += "random_access\n";
  while(it_random_access != container_.get<random_access>().cend()) {
    tmp += it_random_access->to_string() + "\n";
    it_random_access++;
  }
  return tmp;
}

void Matrix::insert(const Matrix::Index& x, const Matrix::Index& y,
  const Matrix::Value& v) {
  container_.insert(random_access_end(),T(x,y,v));
}
//////////////////////////////////////////////////////////////////////

void printMEM() {
  auto sys = system("ps -eo cmd,%mem,%cpu --sort=-%mem | head -n 2 | tail -n 1");
}

int main() {
  Timer mytimer = Timer({"shuffle","init","iter","vec iter","deque init","deque iter"});
  Matrix mat;
  int index;
  int min_qubits = 1;
  int max_qubits = 11;
  mat.reserve(1<<(2*max_qubits));
  struct T {
    int x_; int y_; std::complex<double> v_;
    T() {}
    T(const int& x, const int& y, const double& r, const double& i) {
      x_ = x; y_ = y; v_ = std::complex<double>(r,i);
    }
  };
  //std::vector<std::shared_ptr<Matrix::T>> input;
  std::vector<Matrix::T> input;
  input.reserve(1<<(2*max_qubits));
  std::complex<double> res=0;
  std::deque<Matrix::T> input2;
  for(int q = min_qubits; q <= max_qubits; q++) {

    mytimer.start();
    input.resize(1<<(2*q));
    index = 0;
    std::cout << "a" << std::endl;
    for(int x = 0; x < 1<<q; x++) {
      for(int y = 0; y < 1<<q; y++) {
        //input[index] = (std::make_shared<Matrix::T>(Matrix::T(x,y,Matrix::Value(-x,-y))));
        input[index] = Matrix::T(x,y,Matrix::Value(-x,-y));
        index++;
      }
    }
    std::cout << "b" << std::endl;
    std::random_shuffle(input.begin(),input.end());
    mytimer.set_time("shuffle");

    mytimer.start();
    input2.clear();
    for(int i = 0; i< input.size(); i++) {
      input2.push_back(input[i]);
    }
    mytimer.set_time("deque init");

    mytimer.start();
    auto it2 = input2.begin();
    res = 0;
    index=0;
    for(int i = 0; i< input2.size(); i++) {
      //res += it2->v_;
      res+=input2[index].v_;
      it2++;
      index++;
    }
    mytimer.set_time("deque iter");
    std::cout << res << " " << log2(index) << std::endl;

    auto itv = input.begin();
    index = 0;
    res=0;
    while(itv != input.end()) {
      //res+=(*itv).v_;
      res+=input[index].v_;
      itv++;
      index++;
    }
    mytimer.set_time("vec iter");
    std::cout << log2(index) << " " << res << std::endl;

    mytimer.start();
    mat.clear();
    for(int i = 0; i < input.size(); i++) {
      mat.insert(input[i].x_,input[i].y_,input[i].v_);
      /*
      if(i%(1<<q)==0){
        printMEM();
      }
      */
    }
    mytimer.set_time("init");

    mytimer.start();
    auto it = mat.random_access_begin();
    index = 0;
    res = 0;
    while(it != mat.random_access_end()) {
      res+=it->v_;
      it++;
      index++;
    }
    mytimer.set_time("iter");
    std::cout << log2(index) << " " << res << std::endl;
    /*
    if(q==3) {
       std::cout << mat.to_string() << std::endl;
    }
    */
  }
  std::cout << mytimer.get_data_as_table_string() << std::endl;
  return 0;
}

/*

*/


#ifndef EXPR_PARSER_H_
#define EXPR_PARSER_H_

// C/C++ headers
#include <fstream>
#include <string>

// Boost headers
#include <boost/config/warning_disable.hpp>
#include <boost/fusion/include/at_c.hpp>
#include <boost/fusion/sequence/intrinsic/at_c.hpp>
#include <boost/phoenix/function/adapt_function.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/phoenix_container.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_statement.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>
#include <boost/spirit/include/qi.hpp>

// Engine headers
#include "../operand.hpp"

#include "node.hpp"
#include "node_list.hpp"
#include "operation_holder.hpp"
#include "variable_cluster.hpp"
#include "special_tokens.hpp"

namespace zhetapi {

// Parser
struct parser : boost::spirit::qi::grammar <
		std::string::const_iterator,
		node (),
		boost::spirit::qi::space_type> {
public:
	using Siter = std::string::const_iterator;

	// Qi rule (no skip)
	template <class A, class B>
	using NSRule = boost::spirit::qi::rule <A, B>;

	// Qi rule
	template <class A, class B, class C>
	using Rule = boost::spirit::qi::rule <A, B, C>;

	// Space type
	using Space = boost::spirit::qi::space_type;

	// Vector aliases
	template <class T>
	using V1 = std::vector <T>;

	template <class T>
	using V2 = std::vector <std::vector <T>>;
private:
	boost::spirit::qi::symbols
		<char const, char const>	_esc;

	// Node grammars (structural, from bottom to top)
	Rule <Siter, node (), Space>		_operand;
	Rule <Siter, node (), Space>		_closed_factor;
	Rule <Siter, node (), Space>		_full_factor;
	Rule <Siter, node (), Space>		_factor;
	Rule <Siter, node (), Space>		_term;
	Rule <Siter, node (), Space>		_simple_expression;
	Rule <Siter, node (), Space>		_start;

	// Type parsers
	Rule <Siter, Z (), Space>		_integer;
	Rule <Siter, R (), Space>		_pure_real;
	Rule <Siter, R (), Space>		_real;
	Rule <Siter, VecZ (), Space>		_vector_integer;
	Rule <Siter, VecR (), Space>		_vector_real;
	Rule <Siter, MatZ (), Space>		_matrix_integer;
	Rule <Siter, MatR (), Space>		_matrix_real;
	Rule <Siter, V1 <node> (), Space>	_vector_expr;
	Rule <Siter, node (), Space>		_partial_matrix_expr;
	Rule <Siter, V1 <node> (), Space>	_matrix_expr;
	Rule <Siter, Token *(), Space>		_collection;
	NSRule <Siter, std::string ()>		_string;
	NSRule <Siter, std::string ()>		_identifier;

	// Categories of operations
	Rule <Siter, std::string(), Space>	_term_operation;
	Rule <Siter, std::string(), Space>	_start_operation;
	Rule <Siter, std::string(), Space>	_expression_operation;
	Rule <Siter, std::string(), Space>	_post_operations;
	Rule <Siter, std::string(), Space>	_pre_operations;

	// Operations
	Rule <Siter, std::string(), Space>	_and;
	Rule <Siter, std::string(), Space>	_attribute;
	Rule <Siter, std::string(), Space>	_divide;
	Rule <Siter, std::string(), Space>	_dot;
	Rule <Siter, std::string(), Space>	_eq;
	Rule <Siter, std::string(), Space>	_exponent;
	Rule <Siter, std::string(), Space>	_factorial;
	Rule <Siter, std::string(), Space>	_ge;
	Rule <Siter, std::string(), Space>	_geq;
	Rule <Siter, std::string(), Space>	_le;
	Rule <Siter, std::string(), Space>	_leq;
	Rule <Siter, std::string(), Space>	_minus;
	Rule <Siter, std::string(), Space>	_mod;
	Rule <Siter, std::string(), Space>	_neq;
	Rule <Siter, std::string(), Space>	_or;
	Rule <Siter, std::string(), Space>	_plus;
	Rule <Siter, std::string(), Space>	_transpose;
	Rule <Siter, std::string(), Space>	_post_decr;
	Rule <Siter, std::string(), Space>	_post_incr;
	Rule <Siter, std::string(), Space>	_pre_decr;
	Rule <Siter, std::string(), Space>	_pre_incr;
	Rule <Siter, std::string(), Space>	_times;
public:
	parser(Engine *ctx);
};

}

#endif


// Copyright Aleksey Gurtovoy 2000-2004
//
// Distributed under the Boost Software License,Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef int_<0> _0;
    typedef int_<1> _1;
    typedef int_<2> _2;

    MPL_ASSERT(( is_same< next<_0>::type, _1 > ));
    MPL_ASSERT(( is_same< next<_1>::type, _2 > ));
    MPL_ASSERT(( is_same< prior<_1>::type, _0 > ));
    MPL_ASSERT(( is_same< prior<_2>::type, _1 > ));
}


#define BOOST_TEST_MODULE FactorizationTest

#include "Factorization.hpp"
#include <vector>
#include <cstdint>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string.hpp>

BOOST_AUTO_TEST_SUITE(FactorizationSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(ShouldReturnCorrectFactors)
{
  // Arrange
  int number = 14;
  vector<int> expected_factors = { 1, 2, 7 };

  Factorization<int> factorization(number);

  // Act
  auto actual_factors = factorization.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_factors.begin(),
    expected_factors.end(),
    actual_factors.begin(),
    actual_factors.end()
  );
}

BOOST_AUTO_TEST_CASE(ShouldReturnCorrectFactors_2)
{
  // Arrange
  int number = 8;
  vector<int> expected_factors = { 1, 2, 2, 2 };

  Factorization<int> factorization(number);

  // Act
  auto actual_factors = factorization.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_factors.begin(),
    expected_factors.end(),
    actual_factors.begin(),
    actual_factors.end()
  );
}

BOOST_AUTO_TEST_CASE(ShouldReturnCorrectFactors_3)
{
  // Arrange
  int number = 42;
  vector<int> expected_factors = { 1, 2, 3, 7 };

  Factorization<int> factorization(number);

  // Act
  auto actual_factors = factorization.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_factors.begin(),
    expected_factors.end(),
    actual_factors.begin(),
    actual_factors.end()
  );
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Qbit_.h"

#include "ParsQbit.h"

#include <boost/bind.hpp>

using namespace mathutils; using std::make_shared;

namespace qbit {

///////////
//
// Averaged
//
///////////

Averaged::Averaged()
  : Base(keyTitle,{"rho00","rho11","real(rho10=<sigma>)","imag(\")"})
{
}

const Averaged::Averages Averaged::average_v(NoTime, const LazyDensityOperator& matrix) const
{
  auto averages(initializedAverages());
  averages=matrix(0),matrix(1),real(matrix(1)(0)),imag(matrix(1)(0));
  return averages;
}

namespace {

void sigmaJump(qbit::StateVectorLow& psi, double gamma_perpendicular)
{
  psi(0)=sqrt(2.*gamma_perpendicular)*psi(1);
  psi(1)=0;
}

void sigma_zJump(qbit::StateVectorLow& psi, double gamma_parallel)
{
  double fact=sqrt(2.*gamma_parallel);
  psi(0)*= fact; psi(1)*=-fact;
}

double dummyProba(const qbit::LazyDensityOperator&)
{
  return -1;
}

}

LiouvilleanPhaseNoise::LiouvilleanPhaseNoise(double gamma_perpendicular, double gamma_parallel)
  : structure::ElementLiouvilleanStrategies<1,2>(JumpStrategies(bind(sigmaJump  ,_1,gamma_perpendicular),
                                                                bind(sigma_zJump,_1,gamma_parallel)),
                                                 JumpRateStrategies(dummyProba),
                                                 "LossyQbitWithPhaseNoise",{"excitation loss","phase noise"})
{}

} // qbit

////////////////
//
// Highest level
//
////////////////

QbitBase::QbitBase(const RealFreqs& realFreqs, const ComplexFreqs& complexFreqs)
  : ModeBase(2,realFreqs,complexFreqs,qbit::keyTitle), Averaged()
{}

#define TUPLE_delta(ISIP) RF{"delta",-imag(BOOST_PP_IF(ISIP,get_zI,get_zSch)()),1}
#define TUPLE_eta CF{"eta",get_eta(),1}
#define TUPLE_gammadelta(ISIP) CF{"(gamma,delta)",conj(BOOST_PP_IF(ISIP,get_zI,get_zSch)()),1}
#define TUPLE_gamma RF{"gamma",real(get_zSch()),1}

Qbit::Qbit(const qbit::Pars& p)
  : Exact(dcomp(0,-p.delta)),
    QbitBase{TUPLE_delta(1)}
{}

QbitSch::QbitSch(const qbit::Pars& p)
  : qbit::Hamiltonian<false>(dcomp(0,-p.delta),0),
    QbitBase{TUPLE_delta(0)}
{
  getParsStream()<<"Schroedinger picture.\n";
}

PumpedQbit::PumpedQbit(const qbit::ParsPumped& p)
  : qbit::Hamiltonian<true>(0,dcomp(0,-p.delta),p.eta),
    QbitBase(TUPLE_delta(1),TUPLE_eta)
{
  getParsStream()<<"Pumped.\n";
}

PumpedQbitSch::PumpedQbitSch(const qbit::ParsPumped& p)
  : qbit::Hamiltonian<false>(dcomp(0,-p.delta),p.eta),
    QbitBase(TUPLE_delta(0),TUPLE_eta)
{
  getParsStream()<<"Pumped, Schroedinger picture.\n";
}

LossyQbit::LossyQbit(double delta, double gamma)
  : qbit::Liouvillean(gamma),
    Exact(dcomp(gamma,-delta)),
    QbitBase{TUPLE_gammadelta(1)}
{
  getParsStream()<<"Lossy.\n";
}

LossyQbitSch::LossyQbitSch(double delta, double gamma)
  : qbit::Liouvillean(gamma),
    qbit::Hamiltonian<false>(dcomp(gamma,-delta),0),
    QbitBase{TUPLE_gamma,TUPLE_delta(0)}
{
  getParsStream()<<"Lossy, Schroedinger picture.\n";
}

LossyQbitUIP::LossyQbitUIP(double delta, double gamma)
  : qbit::Liouvillean(gamma),
    qbit::Hamiltonian<true>(dcomp(gamma,0),dcomp(0,-delta),0),
    QbitBase{TUPLE_gamma,TUPLE_delta(1)}
{
  getParsStream()<<"Lossy, Unitary interaction picture.\n";
}

PumpedLossyQbit::PumpedLossyQbit(const qbit::ParsPumpedLossy& p)
  : qbit::Liouvillean(p.gamma),
    qbit::Hamiltonian<true>(0,dcomp(p.gamma,-p.delta),p.eta),
    QbitBase{TUPLE_gammadelta(1),TUPLE_eta}
{
  getParsStream()<<"PumpedLossy.\n";
}

PumpedLossyQbitUIP::PumpedLossyQbitUIP(const qbit::ParsPumpedLossy& p)
  : qbit::Liouvillean(p.gamma),
    qbit::Hamiltonian<true>(dcomp(p.gamma,0),dcomp(0,-p.delta),p.eta),
    QbitBase({TUPLE_gamma,TUPLE_delta(1)},TUPLE_eta)
{
  getParsStream()<<"PumpedLossy, Unitary interaction picture.\n";
}

PumpedLossyQbitSch::PumpedLossyQbitSch(const qbit::ParsPumpedLossy& p)
  : qbit::Liouvillean(p.gamma),
    qbit::Hamiltonian<false>(dcomp(p.gamma,-p.delta),p.eta),
    QbitBase{TUPLE_gammadelta(0),TUPLE_eta}
{
  getParsStream()<<"PumpedLossy, Schroedinger picture.\n";
}

LossyQbitWithPhaseNoise::LossyQbitWithPhaseNoise(double delta, double gamma, double gamma_parallel)
  : Exact(dcomp(gamma,-delta)), // gamma_parallel does not contribute to the Hamiltonian
    qbit::LiouvilleanPhaseNoise(gamma,gamma_parallel),
    QbitBase(RF{"gamma_parallel",gamma_parallel,1},TUPLE_gammadelta(1))
{
  getParsStream()<<"LossyWithPhaseNoise.\n";
}

LossyQbitWithPhaseNoiseUIP::LossyQbitWithPhaseNoiseUIP(double delta, double gamma, double gamma_parallel)
  : qbit::Hamiltonian<true>(dcomp(gamma,0),dcomp(0,-delta),0),
    qbit::LiouvilleanPhaseNoise(gamma,gamma_parallel),
    QbitBase{RF{"gamma_parallel",gamma_parallel,1},TUPLE_gamma,TUPLE_delta(1)}
{
  getParsStream()<<"LossyWithPhaseNoise, Unitary interaction picture.\n";
}

PumpedLossyQbitWithPhaseNoise::PumpedLossyQbitWithPhaseNoise(const qbit::ParsPumpedLossyPhaseNoise& p)
  : qbit::Hamiltonian<true>(0,dcomp(p.gamma,-p.delta),p.eta),
    qbit::LiouvilleanPhaseNoise(p.gamma,p.gamma_parallel),
    QbitBase{RF{"gamma_parallel",p.gamma_parallel,1},TUPLE_gammadelta(1),TUPLE_eta}
{
  getParsStream()<<"PumpedLossyWithPhaseNoise.\n";
}

PumpedLossyQbitWithPhaseNoiseUIP::PumpedLossyQbitWithPhaseNoiseUIP(const qbit::ParsPumpedLossyPhaseNoise& p)
  : qbit::Hamiltonian<true>(dcomp(p.gamma,0),dcomp(0,-p.delta),p.eta),
    qbit::LiouvilleanPhaseNoise(p.gamma,p.gamma_parallel),
    QbitBase{RF{"gamma_parallel",p.gamma_parallel,1},TUPLE_gamma,TUPLE_delta(1)}
{
  getParsStream()<<"PumpedLossyWithPhaseNoise, Unitary interaction picture.\n";
}

#undef  TUPLE_gamma
#undef  TUPLE_gammadelta
#undef  TUPLE_eta
#undef  TUPLE_delta

//////////
//
// Helpers
//
//////////

namespace qbit {

Ptr make(const ParsPumpedLossy& p, QM_Picture qmp)
{
  switch (qmp) {
  case QMP_IP  :
    if (p.gamma==0 && std::abs(p.eta)==0)
      return std::make_shared<Qbit            >(p);
    if (p.gamma==0)
      return std::make_shared<PumpedQbit      >(p);
    if (std::abs(p.eta)==0)
      return std::make_shared<LossyQbit       >(p);
    return std::make_shared<PumpedLossyQbit   >(p);
  case QMP_UIP :
    if (p.gamma==0 && std::abs(p.eta)==0)
      return std::make_shared<QbitUIP         >(p);
    if (p.gamma==0)
      return std::make_shared<PumpedQbitUIP   >(p);
    if (std::abs(p.eta)==0)
      return std::make_shared<LossyQbitUIP    >(p);
    return std::make_shared<PumpedLossyQbitUIP>(p);
  case QMP_SCH :
    ;
  }
  if (p.gamma==0 && std::abs(p.eta)==0)
    return std::make_shared<QbitSch         >(p);
  if (p.gamma==0)
    return std::make_shared<PumpedQbitSch   >(p);
  if (std::abs(p.eta)==0)
    return std::make_shared<LossyQbitSch    >(p);
  return std::make_shared<PumpedLossyQbitSch>(p);
}

Ptr make(const ParsPumpedLossyPhaseNoise& p, QM_Picture qmp)
{
  if (p.gamma_parallel) {
    if (qmp==QMP_UIP) {
      if (std::abs(p.eta))
        return std::make_shared<PumpedLossyQbitWithPhaseNoiseUIP>(p);
      else
        return std::make_shared<      LossyQbitWithPhaseNoiseUIP>(p);
    }
    else {
      if (std::abs(p.eta))
        return std::make_shared<PumpedLossyQbitWithPhaseNoise>(p);
      else
        return std::make_shared<      LossyQbitWithPhaseNoise>(p);
    }
  }
  return make(static_cast<const ParsPumpedLossy&>(p),qmp);
}

const Tridiagonal sigmadagsigmaop()
{
  return mode::nop(make_shared<QbitBase>());
}

StateVector init(dcomp psi1)
{
  StateVector res(2);
  res(0)=sqrt(1-sqrAbs(psi1)); res(1)=psi1;
  return res;
}

} // qbit


//  phoenix_style_control_structures.cpp  -- The Boost Lambda Library ------
//
// Copyright (C) 2000-2003 Jaakko Jarvi (jaakko.jarvi@cs.utu.fi)
// Copyright (C) 2000-2003 Gary Powell (powellg@amazon.com)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// For more information, see www.boost.org

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

#include <boost/core/lightweight_test.hpp>
#define BOOST_CHECK BOOST_TEST

#include "boost/lambda/lambda.hpp"
#include "boost/lambda/if.hpp"
#include "boost/lambda/loops.hpp"

#include <iostream>
#include <vector>
#include <list>
#include <algorithm>
#include <cmath>
#include <cassert>
#include <functional>

using namespace boost::lambda;
using namespace std;

//  If-else, while, do-while, for statements

int main() {

    vector<int> v;
    v.clear();
    v.push_back(1);
    v.push_back(2);
    v.push_back(3);
    v.push_back(4);
    v.push_back(5);
    v.push_back(6);
    v.push_back(7);
    v.push_back(8);
    v.push_back(9);
    v.push_back(10);

    int sum = 0;
    //////////////////////////////////
    for_each(v.begin(), v.end(),
        if_(_1 > 3 && _1 <= 8)
        [
            sum += _1
        ]
    );

    BOOST_CHECK(sum == 4+5+6+7+8);

    int gt = 0, eq = 0, lt = 0;
    //////////////////////////////////
    for_each(v.begin(), v.end(),
        if_(_1 > 5)
        [
            ++var(gt)
        ]
        .else_
        [
            if_(_1 == 5)
            [
                ++var(eq)
            ]
            .else_
            [
                ++var(lt)
            ]
        ]
    );

    BOOST_CHECK(lt==4);
    BOOST_CHECK(eq==1);
    BOOST_CHECK(gt==5);

    vector<int> t = v;

    int counta = 0;
    int countb = 0;
    //////////////////////////////////
    for_each(v.begin(), v.end(),
        (
            while_(_1--)
            [
                ++var(counta)
            ],
            ++var(countb)
        )
    );

    BOOST_CHECK(counta == 55);
    BOOST_CHECK(countb == 10);

    v = t;

    counta = 0; countb = 0;
    //////////////////////////////////
    for_each(v.begin(), v.end(),
        (
            do_
            [
             ++var(counta)
            ]
            .while_(_1--),
            ++var(countb)
        )
    );

    BOOST_CHECK(counta == (2+11)*10/2);
    BOOST_CHECK(countb == 10);

    v = t;
    counta = 0; countb = 0;
    //////////////////////////////////
    int iii;
    for_each(v.begin(), v.end(),
        (
            for_(var(iii) = 0, var(iii) < _1, ++var(iii))
            [
              ++var(counta)
            ],
            ++var(countb)
        )
    );

    BOOST_CHECK(counta == (1+10)*10/2);
    BOOST_CHECK(countb == 10);

    v = t;

    return boost::report_errors();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014-2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_TEST_DISTANCE_COMMON_HPP
#define BOOST_GEOMETRY_TEST_DISTANCE_COMMON_HPP

#include <iostream>
#include <string>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/io/wkt/write.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <boost/geometry/algorithms/num_interior_rings.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>

#include <from_wkt.hpp>
#include <string_from_type.hpp>

#ifndef BOOST_GEOMETRY_TEST_DISTANCE_HPP

namespace bg = ::boost::geometry;

// function copied from BG's test_distance.hpp

template <typename Geometry1, typename Geometry2>
void test_empty_input(Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    try
    {
        bg::distance(geometry1, geometry2);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown" );
}
#endif // BOOST_GEOMETRY_TEST_DISTANCE_HPP

//========================================================================

#ifdef BOOST_GEOMETRY_TEST_DEBUG
// pretty print geometry -- START
template <typename Geometry, typename GeometryTag>
struct pretty_print_geometry_dispatch
{
    template <typename Stream>
    static inline Stream& apply(Geometry const& geometry, Stream& os)
    {
        os << bg::wkt(geometry);
        return os;
    }
};

template <typename Geometry>
struct pretty_print_geometry_dispatch<Geometry, bg::segment_tag>
{
    template <typename Stream>
    static inline Stream& apply(Geometry const& geometry, Stream& os)
    {
        os << "SEGMENT" << bg::dsv(geometry);
        return os;
    }
};

template <typename Geometry>
struct pretty_print_geometry_dispatch<Geometry, bg::box_tag>
{
    template <typename Stream>
    static inline Stream& apply(Geometry const& geometry, Stream& os)
    {
        os << "BOX" << bg::dsv(geometry);
        return os;
    }
};

template <typename Geometry>
struct pretty_print_geometry
{
    template <typename Stream>
    static inline Stream& apply(Geometry const& geometry, Stream& os)
    {
        return pretty_print_geometry_dispatch
            <
                Geometry, typename bg::tag<Geometry>::type
            >::apply(geometry, os);
    }
};
// pretty print geometry -- END
#endif // BOOST_GEOMETRY_TEST_DEBUG

//========================================================================

template <typename T>
struct check_equal
{
    static inline void apply(T const& detected, T const& expected,
                             bool is_finite)
    {
        if (is_finite)
        {
            BOOST_CHECK(detected == expected);
        }
        else
        {
            BOOST_CHECK(! boost::math::isfinite(detected));
        }
    }
};

template <>
struct check_equal<double>
{
    static inline void apply(double detected, double expected,
                             bool is_finite)
    {
        if (is_finite)
        {
            BOOST_CHECK_CLOSE(detected, expected, 0.0001);
        }
        else
        {
            BOOST_CHECK(! boost::math::isfinite(detected));
        }
    }
};

//========================================================================

template
<
    typename Geometry1, typename Geometry2,
    int id1 = bg::geometry_id<Geometry1>::value,
    int id2 = bg::geometry_id<Geometry2>::value
>
struct test_distance_of_geometries
    : public test_distance_of_geometries<Geometry1, Geometry2, 0, 0>
{};

template <typename Geometry1, typename Geometry2>
class test_distance_of_geometries<Geometry1, Geometry2, 0, 0>
{
private:
    template
    <
        typename G1,
        typename G2,
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void base_test(std::string const& header,
                   G1 const& g1, G2 const& g2,
                   DistanceType const& expected_distance,
                   ComparableDistanceType const& expected_comparable_distance,
                   Strategy const& strategy,
                   bool is_finite)
    {
        typedef typename bg::default_distance_result
            <
                G1, G2
            >::type default_distance_result;

        typedef typename bg::strategy::distance::services::return_type
            <
                Strategy, G1, G2
            >::type distance_result_from_strategy;

        static const bool same_regular = boost::is_same
            <
                default_distance_result,
                distance_result_from_strategy
            >::type::value;

        BOOST_CHECK( same_regular );

        typedef typename bg::default_comparable_distance_result
            <
                G1, G2
            >::type default_comparable_distance_result;

        typedef typename bg::strategy::distance::services::return_type
            <
                typename bg::strategy::distance::services::comparable_type
                    <
                        Strategy
                    >::type,
                G1,
                G2
            >::type comparable_distance_result_from_strategy;

        static const bool same_comparable = boost::is_same
            <
                default_comparable_distance_result,
                comparable_distance_result_from_strategy
            >::type::value;

        BOOST_CHECK( same_comparable );

        // check distance with default strategy
        default_distance_result dist_def = bg::distance(g1, g2);

        check_equal
            <
                default_distance_result
            >::apply(dist_def, expected_distance, is_finite);

        // check distance with passed strategy
        distance_result_from_strategy dist = bg::distance(g1, g2, strategy);

        check_equal
            <
                default_distance_result
            >::apply(dist, expected_distance, is_finite);

        // check comparable distance with default strategy
        default_comparable_distance_result cdist_def =
            bg::comparable_distance(g1, g2);

        check_equal
            <
                default_comparable_distance_result
            >::apply(cdist_def, expected_comparable_distance, is_finite);

        // check comparable distance with passed strategy
        comparable_distance_result_from_strategy cdist =
            bg::comparable_distance(g1, g2, strategy);

        check_equal
            <
                default_comparable_distance_result
            >::apply(cdist, expected_comparable_distance, is_finite);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << string_from_type<typename bg::coordinate_type<Geometry1>::type>::name()
                  << string_from_type<typename bg::coordinate_type<Geometry2>::type>::name()
                  << " -> "
                  << string_from_type<default_distance_result>::name()
                  << string_from_type<default_comparable_distance_result>::name()
                  << std::endl;

        std::cout << "distance" << header
                  << " (def. strategy) = " << dist_def << " ; "
                  << "distance" << header
                  <<" (passed strategy) = " << dist << " ; "
                  << "comp. distance" << header <<" (def. strategy) = "
                  << cdist_def << " ; "
                  << "comp. distance" << header <<" (passed strategy) = "
                  << cdist << std::endl;
#endif
    }

public:
    template
    <
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void apply(std::string const& wkt1,
               std::string const& wkt2,
               DistanceType const& expected_distance,
               ComparableDistanceType const& expected_comparable_distance,
               Strategy const& strategy,
               bool is_finite = true)
    {
        Geometry1 geometry1 = from_wkt<Geometry1>(wkt1);
        Geometry2 geometry2 = from_wkt<Geometry2>(wkt2);

        apply(geometry1, geometry2,
              expected_distance, expected_comparable_distance,
              strategy, is_finite);
    }

    template
    <
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void apply(Geometry1 const& geometry1,
               Geometry2 const& geometry2,
               DistanceType const& expected_distance,
               ComparableDistanceType const& expected_comparable_distance,
               Strategy const& strategy,
               bool is_finite = true)
    {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
        typedef pretty_print_geometry<Geometry1> PPG1;
        typedef pretty_print_geometry<Geometry2> PPG2;
        PPG1::apply(geometry1, std::cout);
        std::cout << " - ";
        PPG2::apply(geometry2, std::cout);
        std::cout << std::endl;
#endif

        base_test("", geometry1, geometry2,
                  expected_distance, expected_comparable_distance,
                  strategy, is_finite);

        base_test("[reversed args]", geometry2, geometry1,
                  expected_distance, expected_comparable_distance,
                  strategy, is_finite);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << std::endl;
#endif
    }
};

//========================================================================

template <typename Segment, typename Polygon>
struct test_distance_of_geometries
<
    Segment, Polygon,
    92 /* segment */, 3 /* polygon */
>
    : public test_distance_of_geometries<Segment, Polygon, 0, 0>
{
    typedef test_distance_of_geometries<Segment, Polygon, 0, 0> base;

    typedef typename bg::ring_type<Polygon>::type ring_type;

    template
    <
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void apply(std::string const& wkt_segment,
               std::string const& wkt_polygon,
               DistanceType const& expected_distance,
               ComparableDistanceType const& expected_comparable_distance,
               Strategy const& strategy,
               bool is_finite = true)
    {
        Segment segment = from_wkt<Segment>(wkt_segment);
        Polygon polygon = from_wkt<Polygon>(wkt_polygon);
        apply(segment,
              polygon,
              expected_distance,
              expected_comparable_distance,
              strategy,
              is_finite);
    }

    template
    <
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void apply(Segment const& segment,
               Polygon const& polygon,
               DistanceType const& expected_distance,
               ComparableDistanceType const& expected_comparable_distance,
               Strategy const& strategy,
               bool is_finite = true)
    {
        base::apply(segment, polygon, expected_distance,
                    expected_comparable_distance, strategy, is_finite);

        if ( bg::num_interior_rings(polygon) == 0 ) {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "... testing also exterior ring ..." << std::endl;
#endif
            test_distance_of_geometries
                <
                    Segment, ring_type
                >::apply(segment,
                         bg::exterior_ring(polygon),
                         expected_distance,
                         expected_comparable_distance,
                         strategy,
                         is_finite);
        }
    }
};

//========================================================================

template <typename Box, typename Segment>
struct test_distance_of_geometries
<
    Box, Segment,
    94 /* box */, 92 /* segment */
>
{
    template
    <
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void apply(std::string const& wkt_box,
               std::string const& wkt_segment,
               DistanceType const& expected_distance,
               ComparableDistanceType const& expected_comparable_distance,
               Strategy const& strategy,
               bool is_finite = true)
    {
        test_distance_of_geometries
            <
                Segment, Box, 92, 94
            >::apply(wkt_segment,
                     wkt_box,
                     expected_distance,
                     expected_comparable_distance,
                     strategy,
                     is_finite);
    }
};

template <typename Segment, typename Box>
struct test_distance_of_geometries
<
    Segment, Box,
    92 /* segment */, 94 /* box */
>
    : public test_distance_of_geometries<Segment, Box, 0, 0>
{
    typedef test_distance_of_geometries<Segment, Box, 0, 0> base;

    template
    <
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void apply(std::string const& wkt_segment,
               std::string const& wkt_box,
               DistanceType const& expected_distance,
               ComparableDistanceType const& expected_comparable_distance,
               Strategy const& strategy,
               bool is_finite = true)
    {
        Segment segment = from_wkt<Segment>(wkt_segment);
        Box box = from_wkt<Box>(wkt_box);
        apply(segment,
              box,
              expected_distance,
              expected_comparable_distance,
              strategy,
              is_finite);
    }

    template
    <
        typename DistanceType,
        typename ComparableDistanceType,
        typename Strategy
    >
    static inline
    void apply(Segment const& segment,
               Box const& box,
               DistanceType const& expected_distance,
               ComparableDistanceType const& expected_comparable_distance,
               Strategy const& strategy,
               bool is_finite = true)
    {
        typedef typename bg::strategy::distance::services::return_type
            <
                Strategy, Segment, Box
            >::type distance_result_type;

        typedef typename bg::strategy::distance::services::comparable_type
            <
                Strategy
            >::type comparable_strategy;

        typedef typename bg::strategy::distance::services::return_type
            <
                comparable_strategy, Segment, Box
            >::type comparable_distance_result_type;

        base::apply(segment, box, expected_distance,
                    expected_comparable_distance, strategy, is_finite);

        comparable_strategy cstrategy =
            bg::strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        distance_result_type distance_generic =
            bg::detail::distance::segment_to_box_2D_generic
                <
                    Segment, Box, Strategy
                >::apply(segment, box, strategy);

        comparable_distance_result_type comparable_distance_generic =
            bg::detail::distance::segment_to_box_2D_generic
                <
                    Segment, Box, comparable_strategy
                >::apply(segment, box, cstrategy);

        check_equal
            <
                distance_result_type
            >::apply(distance_generic, expected_distance, is_finite);

        check_equal
            <
                comparable_distance_result_type
            >::apply(comparable_distance_generic,
                     expected_comparable_distance,
                     is_finite);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "... testing with naive seg-box distance algorithm..."
                  << std::endl;
        std::cout << "distance (generic algorithm) = "
                  << distance_generic << " ; "
                  << "comp. distance (generic algorithm) = "
                  << comparable_distance_generic
                  << std::endl;
        std::cout << std::endl << std::endl;
#endif
    }
};

//========================================================================

template <typename Geometry1, typename Geometry2, typename Strategy>
void test_empty_input(Geometry1 const& geometry1,
                      Geometry2 const& geometry2,
                      Strategy const& strategy)
{
    try
    {
        bg::distance(geometry1, geometry2, strategy);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown" );

    try
    {
        bg::distance(geometry2, geometry1, strategy);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown" );
}

#endif // BOOST_GEOMETRY_TEST_DISTANCE_COMMON_HPP


#include "collision_tensor_zlastAM_storage.hpp"
#include <Eigen/Dense>
#include "aux/eigen2hdf.hpp"
#include "aux/exceptions.h"
#include "collision_tensor_dense_base.hpp"
#include "spectral/utility/mass_matrix.hpp"

namespace boltzmann {
namespace ct_dense {

CollisionTensorZLastAMstorage::CollisionTensorZLastAMstorage(const basis_t& basis, int bufsize)
    : CollisionTensorDenseBase(basis)
    , bufsize_(bufsize)
{
  static_assert(EIGEN_DEFAULT_ALIGN_BYTES == 32, "EIGEN_DEFAULT_ALIGN_BYTES is not 32");
  static_assert(EIGEN_MAX_ALIGN_BYTES == 32, "EIGEN_MAX_ALIGN_BYTES is not 32");
}

void CollisionTensorZLastAMstorage::resize_buffer(int bufsize)
{
  int padded_size = (*voffsets_.rbegin());
  if (buffer_.size() / padded_size != bufsize) {
    buffer_.resize(bufsize*padded_size);
    bufsize_ = bufsize;
    std::fill(buffer_.begin(), buffer_.end(), 0.0);
  }
}

int CollisionTensorZLastAMstorage::get_buffer_size() const
{
  int padded_size = (*voffsets_.rbegin());
  int bufsize = buffer_.size() / padded_size;
  return bufsize;
}

void
CollisionTensorZLastAMstorage::import_entries(
    const std::vector<std::shared_ptr<Eigen::SparseMatrix<double>>>& slices)
{
  auto aligned_size = [](size_t size) {
    return size + (VBCRSSparsity<>::align - (size % VBCRSSparsity<>::align));
  };

  static_assert(!mat_t::IsRowMajor, "the underlying sparsity pattern assumes col-major sub-blocks");

  /*   It makes no difference to use Eigen::Unaliged, but using Eigen::Aligned
   *   will trigger an assert, if the given pointer is not properly aligned
   */
  // typedef Eigen::Map<mat_t, Eigen::Aligned, Eigen::OuterStride<> > vmat_t;
  typedef Eigen::Map<mat_t, Eigen::Unaligned, Eigen::OuterStride<>> vmat_t;

  typedef VBCRSSparsity<>::mem_size_t memsize_t;

  double* data = entries_;
  BOOST_ASSERT(data != NULL);

  memsize_t memoffset = 0;
  unsigned int z_offset = 0;
  // iterate over (l,t) pairs (block slices)
  for (unsigned int lt = 0; lt < sparsity_patterns_.size(); ++lt) {
    const auto& vbcrs = sparsity_patterns_[lt];
    for (unsigned int z = 0; z < vbcrs.dimz(); ++z) {
      unsigned int slice_id = z_offset + z;
      // iterate over block rows
      for (unsigned int i = 0; i < vbcrs.nblock_rows(); ++i) {
        const size_t row_offset = vbcrs.get_row_info(i).offset;
        const size_t row_extent = vbcrs.get_row_info(i).extent;
        for (unsigned int blkidx = vbcrs.row_begin(i); blkidx < vbcrs.row_end(i); ++blkidx) {
          memsize_t padded_row_size = aligned_size(row_extent);
          auto& subblock = vbcrs.get_block(blkidx);
          const size_t col_offset = subblock.offset;
          const size_t col_extent = subblock.extent;
          memsize_t sblocksize = padded_row_size * col_extent;
          // copy blocks for all k-values to the given (l1,t1), (l2,t2) values.
          if (slices.size() > 0) {
            // only proc = 0 reads tensor entries
            vmat_t mat_dst(
                data + memoffset, row_extent, col_extent, Eigen::OuterStride<>(padded_row_size));
            mat_dst = (*slices[slice_id]).block(row_offset, col_offset, row_extent, col_extent);
          }
          // increment data pointer
          memoffset += sblocksize;
        }
      }
    }
    // update z-offset
    z_offset += vbcrs.dimz();
  }
  BOOST_ASSERT(z_offset == N_);

  // the vector *voffsets_* contains the offsets for the aligned (padded) input
  // vector offset_map_ is used to map unpadded offsets to the padded offsets
  // (the padded offsets are not stored in the sparsity structure)
  auto& vbcrs = sparsity_patterns_[0];
  voffsets_ = std::vector<int>(vbcrs.nblock_rows() + 1);
  // build voffsets_
  int voffset = 0;
  int unpadded_offset = 0;
  for (int i = 0; i < vbcrs.nblock_rows(); ++i) {
    int row_extent = vbcrs.get_row_info(i).extent;
    voffsets_[i] = voffset;
    // this is needed because vbcrs stores the unpadded col_offset
    offset_map_[unpadded_offset] = voffset;
    voffset += aligned_size(row_extent);
    unpadded_offset += row_extent;
  }
  voffsets_[vbcrs.nblock_rows()] = voffset;
  offset_map_[unpadded_offset] = voffset;

  this->resize_buffer(bufsize_);
  // buffer_.setZero();
}

}  // namespace ct_dense
}  // namespace boltzmann


#define BOOST_TEST_MODULE util_test

#include <iostream>
#include <vector>
#include <utility>

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include "pgmlink/util.h"

using namespace pgmlink;
using namespace std;
using namespace boost;

BOOST_AUTO_TEST_CASE( sort_indices_test ) {
  vector<int> v;
  v.push_back( 9 );
  v.push_back( 3 );
  v.push_back( 1 );
  v.push_back( 1 );
  v.push_back( 7 );
  vector<size_t> inds;

  indexsorter::sort_indices(v.begin(), v.end(), inds);

  BOOST_CHECK_EQUAL(v.size(), 5);
  BOOST_CHECK_EQUAL(v[0], 9);
  BOOST_CHECK_EQUAL(v[1], 3);
  BOOST_CHECK_EQUAL(v[2], 1);
  BOOST_CHECK_EQUAL(v[3], 1);
  BOOST_CHECK_EQUAL(v[4], 7);

  BOOST_CHECK_EQUAL(inds.size(), 5);
  BOOST_CHECK_EQUAL(inds[0], 2);
  BOOST_CHECK_EQUAL(inds[1], 3);
  BOOST_CHECK_EQUAL(inds[2], 1);
  BOOST_CHECK_EQUAL(inds[3], 4);
  BOOST_CHECK_EQUAL(inds[4], 0);
}

BOOST_AUTO_TEST_CASE( reorder_test ) {
  vector<int> v;
  v.push_back( 9 );
  v.push_back( 3 );
  v.push_back( 1 );
  v.push_back( 1 );
  v.push_back( 7 );

  vector<size_t> inds;
  inds.push_back(2);
  inds.push_back(3);
  inds.push_back(1);
  inds.push_back(4);
  inds.push_back(0);

  indexsorter::reorder( v, inds );
  BOOST_CHECK_EQUAL( v.size(), 5 );
  BOOST_CHECK_EQUAL(v[0], 1);
  BOOST_CHECK_EQUAL(v[1], 1);
  BOOST_CHECK_EQUAL(v[2], 3);
  BOOST_CHECK_EQUAL(v[3], 7);
  BOOST_CHECK_EQUAL(v[4], 9);
}