#ifndef ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP
#define ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP

#include <aslam/backend/JacobianContainer.hpp>
#include "RotationExpressionNode.hpp"
#include "ScalarExpressionNode.hpp"
#include "TransformationExpressionNode.hpp"
#include "MatrixExpressionNode.hpp"
#include <boost/shared_ptr.hpp>
#include <Eigen/Core>
#include <sm/kinematics/RotationalKinematics.hpp>

namespace aslam {
  namespace backend {
    template <int D> class VectorExpression;
    template <int D> class VectorExpressionNode;
  class HomogeneousExpressionNode;
    /**
     * \class EuclideanExpressionNode
     * \brief The superclass of all classes representing euclidean points.
     */
    class EuclideanExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      EuclideanExpressionNode();
      virtual ~EuclideanExpressionNode();

      /// \brief Evaluate the euclidean matrix.
      Eigen::Vector3d toEuclidean() const;

      /// \brief Evaluate the Jacobians
      void evaluateJacobians(JacobianContainer & outJacobians) const;

      /// \brief Evaluate the Jacobians and apply the chain rule.
      void evaluateJacobians(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
      void getDesignVariables(DesignVariable::set_t & designVariables) const;
    protected:
      // These functions must be implemented by child classes.
      virtual Eigen::Vector3d toEuclideanImplementation() const = 0;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const = 0;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const = 0;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const = 0;
    };

    /**
     * \class EuclideanExpressionNodeMultiply
     *
     * \brief A class representing the multiplication of two euclidean matrices.
     *
     */
    class EuclideanExpressionNodeMultiply : public EuclideanExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      EuclideanExpressionNodeMultiply(boost::shared_ptr<RotationExpressionNode> lhs,
				     boost::shared_ptr<EuclideanExpressionNode> rhs);
      virtual ~EuclideanExpressionNodeMultiply();

    private:
      virtual Eigen::Vector3d toEuclideanImplementation() const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

      boost::shared_ptr<RotationExpressionNode> _lhs;
      mutable Eigen::Matrix3d _C_lhs;
      boost::shared_ptr<EuclideanExpressionNode> _rhs;
      mutable Eigen::Vector3d _p_rhs;

    };

    // ## New Class for Multiplication with a MatrixExpression
    /**
      * \class EuclideanExpressionNodeMatrixMultiply
      *
      * \brief A class representing the multiplication of a Matrix with a euclidean Vector.
      *
      */
     class EuclideanExpressionNodeMatrixMultiply : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeMatrixMultiply(boost::shared_ptr<MatrixExpressionNode> lhs, boost::shared_ptr<EuclideanExpressionNode> rhs);
       virtual ~EuclideanExpressionNodeMatrixMultiply();

     private:
       virtual Eigen::Vector3d toEuclideanImplementation() const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
       virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

       boost::shared_ptr<MatrixExpressionNode> _lhs;
       mutable Eigen::Matrix3d _A_lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
       mutable Eigen::Vector3d _p_rhs;

     };

    /**
      * \class EuclideanExpressionNodeCrossEuclidean
      *
      * \brief A class representing the cross product of two euclidean expressions.
      *
      */
     class EuclideanExpressionNodeCrossEuclidean : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeCrossEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       virtual ~EuclideanExpressionNodeCrossEuclidean();

     private:
       virtual Eigen::Vector3d toEuclideanImplementation() const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
       virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

     /**
       * \class EuclideanExpressionNodeAddEuclidean
       *
       * \brief A class representing the addition of two euclidean expressions.
       *
       */
      class EuclideanExpressionNodeAddEuclidean : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeAddEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
            boost::shared_ptr<EuclideanExpressionNode> rhs);
        virtual ~EuclideanExpressionNodeAddEuclidean();

      private:
        virtual Eigen::Vector3d toEuclideanImplementation() const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
        virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

        boost::shared_ptr<EuclideanExpressionNode> _lhs;
        boost::shared_ptr<EuclideanExpressionNode> _rhs;
      };

      /**
      * \class EuclideanExpressionNodeSubtractEuclidean
      *
      * \brief A class representing the subtraction of two Euclidean expressions.
      *
      */
     class EuclideanExpressionNodeSubtractEuclidean : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeSubtractEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       virtual ~EuclideanExpressionNodeSubtractEuclidean();

     private:
       virtual Eigen::Vector3d toEuclideanImplementation() const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
       virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

     /**
     * \class EuclideanExpressionNodeConstant
     *
     * \brief A class representing a constant Euclidean expressions.
     *
     */
     class EuclideanExpressionNodeConstant : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeConstant(const Eigen::Vector3d & p);
       virtual ~EuclideanExpressionNodeConstant();

         void set(const Eigen::Vector3d & p){ _p = p; }
     private:
         virtual Eigen::Vector3d toEuclideanImplementation() const;
         virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
         virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
         virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

         Eigen::Vector3d _p;
     };

    /**
      * \class EuclideanExpressionNodeSubtractVector
      *
      * \brief A class representing the subtraction of a vector from an Euclidean expression.
      *
      */
     class EuclideanExpressionNodeSubtractVector : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeSubtractVector(boost::shared_ptr<EuclideanExpressionNode> lhs,
              const Eigen::Vector3d & rhs);
       virtual ~EuclideanExpressionNodeSubtractVector();

     private:
       virtual Eigen::Vector3d toEuclideanImplementation() const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
       virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
       virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       Eigen::Vector3d _rhs;
     };

     /**
       * \class EuclideanExpressionNodeSubtractVector
       *
       * \brief A class representing the negated Euclidean expression.
       *
       */
      class EuclideanExpressionNodeNegated : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeNegated(boost::shared_ptr<EuclideanExpressionNode> operand);
        virtual ~EuclideanExpressionNodeNegated();

      private:
        virtual Eigen::Vector3d toEuclideanImplementation() const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
        virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

        boost::shared_ptr<EuclideanExpressionNode> _operand;
      };

     /**
       * \class EuclideanExpressionNodeScalarMultiply
       *
       * \brief A class representing the multiplication of a ScalarExpression with an Euclidean expression.
       *
       */
      class EuclideanExpressionNodeScalarMultiply : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeScalarMultiply(boost::shared_ptr<EuclideanExpressionNode> p, boost::shared_ptr<ScalarExpressionNode> s);
        virtual ~EuclideanExpressionNodeScalarMultiply();

      private:
        virtual Eigen::Vector3d toEuclideanImplementation() const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
        virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

        boost::shared_ptr<EuclideanExpressionNode> _p;
        boost::shared_ptr<ScalarExpressionNode> _s;
      };

     /**
       * \class VectorExpression2EuclideanExpressionAdapter
       *
       * \brief A class representing an adapted VectorExpression<3>.
       *
       */
      class VectorExpression2EuclideanExpressionAdapter : public EuclideanExpressionNode
      {
      public:
        VectorExpression2EuclideanExpressionAdapter(boost::shared_ptr<VectorExpressionNode<3> > vectorExpressionNode);
        virtual ~VectorExpression2EuclideanExpressionAdapter();

      private:
        virtual Eigen::Vector3d toEuclideanImplementation() const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
        virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

        boost::shared_ptr<VectorExpressionNode<3> > _vectorExpressionNode;
      };

      class EuclideanExpressionNodeTranslation : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeTranslation(boost::shared_ptr<TransformationExpressionNode> operand);
        virtual ~EuclideanExpressionNodeTranslation();

      private:
        virtual Eigen::Vector3d toEuclideanImplementation() const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
        virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

        boost::shared_ptr<TransformationExpressionNode> _operand;
      };

  class EuclideanExpressionNodeRotationParameters : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeRotationParameters(boost::shared_ptr<RotationExpressionNode> operand, sm::kinematics::RotationalKinematics::Ptr rk);
        virtual ~EuclideanExpressionNodeRotationParameters();

      private:
        virtual Eigen::Vector3d toEuclideanImplementation() const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
        virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

        boost::shared_ptr<RotationExpressionNode> _operand;
        sm::kinematics::RotationalKinematics::Ptr _rk;
      };

  class EuclideanExpressionNodeFromHomogeneous : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeFromHomogeneous(boost::shared_ptr<HomogeneousExpressionNode> root);
        virtual ~EuclideanExpressionNodeFromHomogeneous();

      private:
        virtual Eigen::Vector3d toEuclideanImplementation() const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const;
        virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
        virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const;

        boost::shared_ptr<HomogeneousExpressionNode> _root;
      };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP */


#include <stdio.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include "message_filters/subscriber.h"
#include "tf/message_filter.h"

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/ndt.h>

#include <boost/thread/thread.hpp>

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

using namespace std;

double xmax, xmin, ymax, ymin;
string ref_frame_id;
ros::Publisher pub;

nav_msgs::Odometry odom_out;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud_XYZ;

void Callback(const sensor_msgs::PointCloud2ConstPtr& input) {
  ROS_DEBUG("get velodyne data");
  PointCloud_XYZ cloud;
  pcl::fromROSMsg(*input, cloud);

  static PointCloud_XYZ prev_cloud;
  if (0 == prev_cloud.size()) {
    ROS_INFO("got first PointClouds");
    prev_cloud = cloud;
    return;
  }
  ROS_INFO_STREAM("size  now:" << cloud.size() << " prev:" << prev_cloud.size());

  // Filtering input scan to roughly 10% of original size to increase speed of registration.
  PointCloud_XYZ filtered_cloud;
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  // approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
  approximate_voxel_filter.setLeafSize(1.0, 1.0, 1.0);
  approximate_voxel_filter.setInputCloud(cloud.makeShared());
  approximate_voxel_filter.filter(*filtered_cloud.makeShared());

  // Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
//   ndt.setTransformationEpsilon(0.01);
  // Setting maximum step size for More-Thuente line search.
//   ndt.setStepSize(0.1);
  // Setting Resolution of NDT grid structure (VoxelGridCovariance).
//   ndt.setResolution(1.0);

  // Setting max number of registration iterations.
//   ndt.setMaximumIterations(35);

  // Setting point cloud to be aligned.
  ndt.setInputSource(filtered_cloud.makeShared());
  // Setting point cloud to be aligned to.
  ndt.setInputTarget(prev_cloud.makeShared());
  Eigen::AngleAxisf init_rotation(0.0, Eigen::Vector3f::UnitZ());
  Eigen::Translation3f init_translation(0.0, 0.0, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
  PointCloud_XYZ::Ptr useless_cloud(new PointCloud_XYZ);
//   ndt.align(*useless_cloud);
  ndt.align(*useless_cloud, init_guess);
//   computeTransformation	(*useless_cloud);
  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
            << " score: " << ndt.getFitnessScore() << std::endl;

  // Transforming unfiltered, input cloud using found transform.
  std::cout << "translation is " << ndt.getFinalTransformation() << std::endl;
  //  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  prev_cloud = cloud;

  // tring fid = scan->header.frame_id;
  // try {
  //   // tf::MessageFilterで待ってるから待ち時間は発生しないはずだけど一応
  //   listener->waitForTransform(
  //       ref_frame_id, fid,
  //       scan->header.stamp + ros::Duration().fromSec(scan->ranges.size() * scan->time_increment),
  //       ros::Duration(1.0));
  // } catch (tf::TransformException& ex) {
  //   ROS_WARN("%s", ex.what());
  //   return;
  // }
  // sensor_msgs::PointCloud cloud;
  // static laser_geometry::LaserProjection projector_;
  // projector_.transformLaserScanToPointCloud(ref_frame_id, *scan, cloud, *listener);
  //
  // // pclに変換できた点のLaserScanでのインデックス配列が格納されている,channelsインデックス
  // int ch_i = -1;
  // for (int i = 0; i < cloud.channels.size(); i++) {
  //   if ("index" == cloud.channels[i].name) {
  //     ch_i = i;
  //     break;
  //   }
  // }
  // if (-1 == ch_i) {
  //   ROS_ERROR("There is not 'index' channel in PointCloud converted from LaserScan");
  // }
  //
  // // 四角形内のインデックスを記録
  // vector<int> rip_idx;
  // for (int i = 0; i < cloud.points.size(); i++) {
  //   float x = cloud.points.at(i).x;
  //   float y = cloud.points.at(i).y;
  //   if (xmin < x && x < xmax && ymin < y && y < ymax) {
  //     int tmp = (int)cloud.channels[ch_i].values[i];
  //     rip_idx.push_back(tmp);
  //   }
  // }
  //
  // // インデックスに登録された点を削除，無効な距離を設定
  // sensor_msgs::LaserScan scan_out = *scan;
  // for (vector<int>::iterator it = rip_idx.begin(); it < rip_idx.end(); ++it) {
  //   int i = (int)*it;
  //   scan_out.ranges.at(i) = scan_out.range_max + 1.0;
  // }
  //
  // pub.publish(scan_out);
}

int main(int argc, char** argv) {
  ROS_INFO("Hello World!");
  ros::init(argc, argv, "pc_scan_matcher");
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  // nh_private.param<double>("ymax", ymax, 1.0);
  nh_private.param<string>("frame_id", ref_frame_id, "velodyne");

  // tf::TransformListener tfl;
  // message_filters::Subscriber<sensor_msgs::LaserScan> scan_filter_sub_(nh, "velodyne_points", 5);
  // tf::MessageFilter<sensor_msgs::LaserScan> scan_filter_(scan_filter_sub_, tfl, ref_frame_id, 5);
  // scan_filter_.registerCallback(boost::bind(&laserCallback, _1, &tfl));

  pub = nh.advertise<sensor_msgs::LaserScan>("odom_out", 5);
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("velodyne_points", 10, Callback);

  ros::spin();
}


#include <test/test_bitcoin.h>
#include <tradelayer/consensushash.h>
#include <tradelayer/rpcrequirements.h>
#include <tradelayer/rules.h>
#include <tradelayer/sp.h>
#include <tradelayer/tally.h>
#include <tradelayer/tradelayer.h>
#include <tradelayer/tx.h>
#include <tradelayer/uint256_extensions.h>
#include <util/strencodings.h>

#include <univalue.h>

#include <assert.h>
#include <fstream>
#include <limits>
#include <map>
#include <set>
#include <stdint.h>
#include <string>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
#include <boost/test/unit_test.hpp>

using namespace mastercore;

BOOST_FIXTURE_TEST_SUITE(tradelayer_rpcrequirements, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(requirebalance)
{
    const uint32_t propertyId = 1;
    const int64_t amount = 99999;
    const std::string address = "moCYruRphhYgejzH75bxWD49qRFan8eGES";

    BOOST_CHECK(mastercore::update_tally_map(address,propertyId, amount, BALANCE));

    BOOST_CHECK_EQUAL(99999, getMPbalance(address, propertyId, BALANCE));

    const int64_t balanceUnconfirmed = getUserAvailableMPbalance(address, propertyId);

    BOOST_CHECK_EQUAL(99999,balanceUnconfirmed);
}

// no inverse quoted
BOOST_AUTO_TEST_CASE(collateral)
{
    const int64_t uPrice = 100000000; // 1
    const uint32_t propertyId = 4;
    const int64_t contract_amount = 2000;
    const int64_t amount = 400000000000; // 4000 units
    const uint64_t leverage = 2;
    const std::string address = "moCYruRphhYgejzH75bxWD49qRFan8eGES";
    const uint64_t margin_requirement = 100000000;  // 1

    BOOST_CHECK(mastercore::update_tally_map(address, propertyId, amount, BALANCE));

    const arith_uint256 amountTR = (ConvertTo256(COIN) * mastercore::ConvertTo256(contract_amount) * mastercore::ConvertTo256(margin_requirement)) / (mastercore::ConvertTo256(leverage) * mastercore::ConvertTo256(uPrice));
    const int64_t amountToReserve = mastercore::ConvertTo64(amountTR);

    const int64_t nBalance = getMPbalance(address, propertyId, BALANCE);

    BOOST_CHECK_EQUAL(400000000000,nBalance);

    BOOST_CHECK_EQUAL(100000000000,amountToReserve);

    BOOST_CHECK(nBalance >= amountToReserve && nBalance > 0);

    const int64_t balanceUnconfirmed = getUserAvailableMPbalance(address, propertyId);

    BOOST_CHECK_EQUAL(400000000000,balanceUnconfirmed);

}

// no inverse quoted
BOOST_AUTO_TEST_CASE(collateral_inverse_quoted)
{
    const uint32_t propertyId = 4;
    const int64_t contract_amount = 2000;
    const int64_t amount = 400000000000; // 4000 units
    const uint64_t leverage = 2;
    const std::string address = "moCYruRphhYgejzH75bxWD49qRFan8eGEx";
    // rational_t conv = rational_t(1,1);

    const uint64_t margin_requirement = 100000000;  // 1

    // BTC/dUSD , 1 BTC = $10000
    const int64_t uPrice = 1000000000000;

    BOOST_CHECK(mastercore::update_tally_map(address, propertyId, amount, BALANCE));

    const arith_uint256 amountTR = (ConvertTo256(COIN) * mastercore::ConvertTo256(contract_amount) * mastercore::ConvertTo256(margin_requirement)) / (mastercore::ConvertTo256(leverage) * mastercore::ConvertTo256(uPrice));
    const int64_t amountToReserve = mastercore::ConvertTo64(amountTR);

    const int64_t nBalance = getMPbalance(address, propertyId, BALANCE);

    BOOST_CHECK_EQUAL(400000000000,nBalance);

    BOOST_CHECK_EQUAL(10000000,amountToReserve);

    BOOST_CHECK(nBalance >= amountToReserve && nBalance > 0);

    const int64_t balanceUnconfirmed = getUserAvailableMPbalance(address, propertyId);

    BOOST_CHECK_EQUAL(400000000000,balanceUnconfirmed);

}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/geometry/arithmetic/cross_product.hpp>


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/table.hpp>
#include <nt2/include/functions/rows.hpp>
#include <nt2/include/functions/size.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/exceptions.hpp>
#include <boost/dispatch/meta/nth_hierarchy.hpp>

NT2_TEST_CASE_TPL( _0d, NT2_TYPES )
{
  using boost::mpl::_;
  using nt2::meta::value_type_;
 typedef std::complex<T> cT;

  nt2::table<cT> x1 = nt2::rows( cT(42) );
  NT2_TEST_EXPR_TYPE( ( nt2::rows( cT(42) )), (value_type_<_>), (cT) );
  NT2_TEST_EQUAL( nt2::extent(x1), nt2::of_size(1) );
  NT2_TEST_EQUAL( x1(1),cT(42) );
}

NT2_TEST_CASE_TPL( square, NT2_TYPES )
{
  using boost::mpl::_;
  using nt2::meta::value_type_;
  typedef std::complex<T> cT;

  cT s = cT(42);
  nt2::table<cT> ref( nt2::of_size(3,3) );
  for(int i=1;i<= 3;++i)
    for(int j=1;j<= 3;++j)
      ref(i,j) = cT(i-1)+s;

  nt2::table<cT> x0 = nt2::rows(3,s);

  NT2_TEST_EXPR_TYPE( ( nt2::rows( 3,s )), (value_type_<_>), (cT) );
  NT2_TEST_EQUAL( nt2::extent(x0), nt2::of_size(3,3) );
  NT2_TEST_EQUAL( x0,ref );
}

NT2_TEST_CASE_TPL( nd, NT2_TYPES )
{
  using boost::mpl::_;
  using nt2::meta::value_type_;
  typedef std::complex<T> cT;

  cT s = cT(42);

  nt2::table<cT> ref( nt2::of_size(8,8) );
  for(int j=1;j<= 8;++j)
    for(int i=1;i<= 8;++i)
      ref(i,j) = cT(i-1)+s;

  nt2::table<cT> x1 = nt2::rows(nt2::of_size(8, 8), s );

  NT2_TEST_EXPR_TYPE( ( nt2::rows(nt2::of_size(8, 8), s ) ), (value_type_<_>), (cT) );
  NT2_TEST_EQUAL(x1,ref);

  nt2::table<cT> x2 = nt2::rows(8, 8, s );
  NT2_TEST_EQUAL(x2,ref);

  nt2::table<cT> x3 = nt2::rows(8, s );
  NT2_TEST_EQUAL(x3,ref);
}

NT2_TEST_CASE_TPL( typed_expr, NT2_TYPES )
{
  using boost::mpl::_;
  using nt2::meta::value_type_;
  typedef std::complex<T> cT;

  cT s = cT(42);
  nt2::table<cT> ref( nt2::of_size(8,8) );
  for(int j=1;j<= 8;++j)
    for(int i=1;i<= 8;++i)
      ref(i,j) = cT(i-1)+s;

  nt2::table<int> t1(nt2::of_size(1) );
  t1(1) = 8;

  nt2::table<cT> x1 = nt2::rows( t1, s );
  NT2_TEST_EXPR_TYPE( ( nt2::rows(t1, s ) ), (value_type_<_>), (cT) );
  NT2_TEST_EQUAL(x1,ref);

  nt2::table<int> t2(nt2::of_size(1,2) );
  t2(1) = 4;
  t2(2) = 3;

  ref.resize( nt2::of_size(4,3) );
  for(int j=1;j<= 3;++j)
    for(int i=1;i<= 4;++i)
      ref(i,j) = cT(i-1)+s;

  nt2::table<cT> x2 = nt2::rows( t2, s );
  NT2_TEST_EQUAL(x2,ref);
}


#include "stdafx.h"
#include "AdditionProblem.h"
#include "Quiz.h"
#include <boost/array.hpp>
#include <sstream>

using namespace std;

AdditionProblem::AdditionProblem(vector<int>& numbers) {
	this->numbers = numbers;
}

AdditionProblem::AdditionProblem(int* numbers, int len) {
	this->numbers.assign(numbers, numbers + len);
}

LPCWSTR AdditionProblem::getEquation() {
	wstringstream ss;
	unsigned int i = 0;
	for(auto x : this->numbers)
	{
		++i;

		if (i >= this->numbers.size())
		{
			ss << x;
		}
		else
		{
			ss << x << " + ";
		}
	}

	LPCWSTR str = ss.str().c_str();

	return str;
}

bool AdditionProblem::isSolution(float answer) {
	return false;
}

vector<int> AdditionProblem::getNumbers() {
	return this->numbers;
}


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the functions log1p and expm1.  The accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //

   //
   // Catch all cases come last:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*",                          // test data group
      ".*",                          // test function
      4,                             // Max Peek error
      3);                            // Max mean error

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class T>
void do_test(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = &boost::math::log1p<value_type>;
#else
   pg funcp = &boost::math::log1p;
#endif

   boost::math::tools::test_result<value_type> result;
   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";
   //
   // test log1p against data:
   //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::log1p<value_type>;
#else
   funcp = &boost::math::log1p;
#endif
   result = boost::math::tools::test(
      data,
         bind_func(funcp, 0),
         extract_result(1));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::log1p", "log1p and expm1");
   std::cout << std::endl;
   //
   // test expm1 against data:
   //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::expm1<value_type>;
#else
   funcp = boost::math::expm1;
#endif
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0),
      extract_result(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::expm1", "log1p and expm1");
   std::cout << std::endl;
}

template <class T>
void test(T, const char* type_name)
{
#  include "log1p_expm1_data.ipp"

   do_test(log1p_expm1_data, type_name, "expm1 and log1p");

   //
   // C99 Appendix F special cases:
   static const T zero = 0;
   static const T m_one = -1;
   BOOST_CHECK_EQUAL(boost::math::log1p(zero), zero);
   BOOST_CHECK_EQUAL(boost::math::log1p(-zero), zero);
   BOOST_CHECK_EQUAL(boost::math::expm1(zero), zero);
   if(std::numeric_limits<T>::has_infinity)
   {
      BOOST_CHECK_EQUAL(boost::math::log1p(m_one), -std::numeric_limits<T>::infinity());
      BOOST_CHECK_EQUAL(boost::math::expm1(-std::numeric_limits<T>::infinity()), m_one);
      BOOST_CHECK_EQUAL(boost::math::expm1(std::numeric_limits<T>::infinity()), std::numeric_limits<T>::infinity());
#ifndef __BORLANDC__
      // When building with Borland's compiler, simply the *presence*
      // of these tests cause other unrelated tests to fail!!! :-(
      using namespace boost::math::policies;
      typedef policy<overflow_error<throw_on_error> > pol;
      BOOST_CHECK_THROW(boost::math::log1p(m_one, pol()), std::overflow_error);
      BOOST_CHECK_THROW(boost::math::expm1(std::numeric_limits<T>::infinity(), pol()), std::overflow_error);
#endif
   }
}

int test_main(int, char* [])
{
   expected_results();
   BOOST_MATH_CONTROL_FP;
   test(float(0), "float");
   test(double(0), "double");
   //
   // The long double version of these tests fails on some platforms
   // due to poor std lib support (not enough digits returned from
   // std::log and std::exp):
   //
#if !defined(BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS)
   test((long double)(0), "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
   test((boost::math::concepts::real_concept)(0), "real_concept");
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif
   return 0;
}


#include <fstream>
#include <array>
#include <random>
#include <fstream>
#include <iostream>
#include <set>
#include <array>
#include <random>
#include <vector>
#include <algorithm>
#include <stdio.h>
#include <exception>
#include <map>
#include <mutex>
#include <tuple>
#include <memory>
#include <utility>
#include <queue>

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include <scotch.h>
#include <mpi.h>

#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif

#include "tasktorrent/tasktorrent.hpp"
#include "mmio.hpp"
#include "cxxopts.hpp"

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef Eigen::SparseMatrix<double> SpMat;
typedef array<int, 2> int2;
typedef array<int, 3> int3;

const string FOLDER = "./";

struct range
{
    int lb;
    int ub;
    int k;
};

// Given (k, i, j), returns (k, max(i, j), min(i, j))
int3 lower(int3 kij)
{
    int k = kij[0];
    int i = kij[1];
    int j = kij[2];
    return {k, std::max(i,j), std::min(i,j)};
};

// Find the positions of c_rows into p_rows
//
// @in c_rows is a subset of p_rows
// @in c_rows, p_rows are sorted
// @post out[i] = j <=> c_rows[i] == p_rows[j]
vector<int> get_subids(vector<int> &c_rows, vector<int> &p_rows)
{
    int cn = c_rows.size();
    vector<int> subids(cn);
    int l = 0;
    for (int i = 0; i < cn; i++)
    {
        while (c_rows[i] != p_rows[l])
        {
            l++;
        }
        assert(l < p_rows.size());
        assert(p_rows[l] == c_rows[i]);
        subids[i] = l;
    }
    return subids;
};

// Creates a random (-1, 1) vector
// @pre size >= 0
// @post x, st -1 <= x[i] <= 1 for all 0 <= i < size
VectorXd random(int size, int seed)
{
    mt19937 rng;
    rng.seed(seed);
    uniform_real_distribution<double> dist(-1.0, 1.0);
    VectorXd x(size);
    for (int i = 0; i < size; i++)
    {
        x[i] = dist(rng);
    }
    return x;
}

// Given a sparse matrix A, creates rowval and colptr, the CSC representation of A
// without the self loops on the diagonal
// @pre A is a sparse symmetrix matrix
void get_csc_no_diag(SpMat &A, VectorXi *rowval, VectorXi *colptr) {
    int N = A.rows();
    int nnz = A.nonZeros();
    *rowval = VectorXi(nnz);
    *colptr = VectorXi(N + 1);
    int k = 0;
    (*colptr)[0] = 0;
    for (int j = 0; j < N; j++)
    {
        for (SpMat::InnerIterator it(A, j); it; ++it)
        {
            int i = it.row();
            if (i != j)
            {
                (*rowval)[k] = i;
                k++;
            }
        }
        (*colptr)[j + 1] = k;
    }
    rowval->conservativeResize(k);
}

// Algebraic partitioning
// N is the matrix size
// rowval, colptr is the CSC of symmetric A, without the diagonal
// nlevels, block_size are ND and partitioning parameters
// Outputs are
// - permtab, the permuation
// - rangtab, the colptr of the clusters (cluster i goes from rangtab[i] ... rangtab[i+1] in permtab ordering)
void algebraic_partitioning(int N, VectorXi &rowval, VectorXi &colptr, int nlevels, int block_size, VectorXi *permtab, VectorXi *rangtab, vector<int> *depth) {
    assert(N == colptr.size() - 1);
    SCOTCH_Graph *graph = SCOTCH_graphAlloc();
    int err = SCOTCH_graphInit(graph);
    assert(err == 0);
    err = SCOTCH_graphBuild(graph, 0, N, colptr.data(), nullptr, nullptr, nullptr, rowval.size(), rowval.data(), nullptr);
    assert(err == 0);
    err = SCOTCH_graphCheck(graph);
    assert(err == 0);
    // Create strat
    SCOTCH_Strat *strat = SCOTCH_stratAlloc();
    err = SCOTCH_stratInit(strat);
    assert(err == 0);
    assert(nlevels > 0);
    string orderingstr = "n{sep=(/levl<" + to_string(nlevels - 1) + "?g:z;),ose=b{cmin=" + to_string(block_size) + "}}";
    cout << "Using ordering " << orderingstr << endl;
    // string orderingstr = "n{sep=(/levl<" + to_string(nlevels-1) + "?g:z;)}";
    err = SCOTCH_stratGraphOrder(strat, orderingstr.c_str());
    assert(err == 0);
    // Order with SCOTCH
    *permtab = VectorXi::Zero(N);
    VectorXi peritab = VectorXi::Zero(N);
    *rangtab = VectorXi::Zero(N + 1);
    VectorXi treetab = VectorXi::Zero(N);
    int nblk = 0;
    err = SCOTCH_graphOrder(graph, strat, permtab->data(), peritab.data(), &nblk, rangtab->data(), treetab.data());
    assert(err == 0);
    assert(nblk >= 0);
    rangtab->conservativeResize(nblk + 1);
    // Compute depth
    *depth = vector<int>(nblk, 0);
    for (int i = 0; i < nblk; i++)
    {
        int p = treetab[i];
        while (p != -1)
        {
            p = treetab[p];
            (*depth)[i]++;
            assert(p == -1 || (p >= 0 && p < nblk));
        }
    }
}

struct Bloc
{
    // row irow of the block
    int i;
    // col irow of the block
    int j;
    // the matrix block
    unique_ptr<MatrixXd> matA;
    // A subset of nodes[i]->start ... nodes[i]->end
    vector<int> rows;
    // A subset of nodes[j]->start ... nodes[j]->end
    vector<int> cols;
    // Accumulation structures
    // number of gemm to accumulate on this block
    int n_accumulate;
    // data to be accumulated
    mutex to_accumulate_mtx;
    map<int, unique_ptr<MatrixXd>> to_accumulate; // The blocs to be accumulated on this
    // Debugging only
    atomic<bool> accumulating_busy;
    atomic<int> accumulated;
    Bloc(int i_, int j_) : i(i_), j(j_), matA(nullptr), n_accumulate(0), accumulating_busy(false), accumulated(0){};
    MatrixXd* A() {
        return matA.get();
    }
};

struct Node
{
    // irow of this node
    int irow;
    // start, end, size of this node
    int start;
    int end;
    int size;
    // nbrs, after (below the diagonal, in the column) and excluding self
    vector<int> nbrs;
    // used in the solve phase
    VectorXd xsol;
    // children in the etree
    vector<int> children;
    // parent in the etree
    int parent;
    Node(int irow_, int s_, int l_) : irow(irow_), start(s_), size(l_), parent(-1)
    {
        end = start + size;
    };
};

struct DistMat
{
    const int nlevels;
    const int block_size;

    // All the nodes
    // nodes[i] = ith pivot (diagonal bloc)
    vector<unique_ptr<Node>> nodes;
    // blocs[i,j] = non zero part of the matrix
    map<int2, unique_ptr<Bloc>> blocs;
    // permutation from natural ordering
    VectorXi perm;
    // original A
    SpMat A;
    // App = P A P^T
    SpMat App;
    // Super of supernodes
    int nblk;
    // Map nodes to ranks
    vector<int> node2rank;
    // Depth (in the ND tree) of each node
    vector<int> depth;
    // Map col to rank
    int col2rank(int col)
    {
        assert(node2rank.size() == nblk);
        assert(col >= 0 && col < nblk);
        return node2rank[col];
    }
    // Number of gemm to accumulate on block ij
    int n_to_accumulate(int2 ij)
    {
        return blocs.at(ij)->n_accumulate;
    }
    // Number of gemm accumulated on block ij
    int accumulated(int2 ij)
    {
        return blocs.at(ij)->accumulated.load();
    }

    // Some statistics/timings
    atomic<long long> gemm_us;
    atomic<long long> trsm_us;
    atomic<long long> potf_us;
    atomic<long long> scat_us;
    atomic<long long> allo_us;

    // Build all the supernodes based on rangtab
    // Returns i2irow, mapping row -> irow
    VectorXi build_supernodes(VectorXi& rangtab) {
        nodes = vector<unique_ptr<Node>>(nblk);
        int N = App.rows();
        assert(rangtab.size() > 0);
        assert(rangtab[0] == 0);
        assert(nblk == rangtab.size()-1);
        assert(rangtab(nblk) == N);
        VectorXi i2irow(N);
        double mean = 0.0;
        int mini = App.rows();
        int maxi = -1;
        for (int i = 0; i < nblk; i++)
        {
            nodes[i] = make_unique<Node>(i, rangtab[i], rangtab[i + 1] - rangtab[i]);
            for (int j = rangtab[i]; j < rangtab[i + 1]; j++)
            {
                i2irow[j] = i;
            }
            mean += nodes.at(i)->size;
            mini = min(mini, nodes.at(i)->size);
            maxi = max(maxi, nodes.at(i)->size);
        }
        printf("[%d] %d blocks, min size %d, mean size %f, max size %d\n", comm_rank(), nblk, mini, mean / nblk, maxi);
        return i2irow;
    }

    // Build elimination tree, ie, compute
    // - node->nbrs (irow)
    // - node->parent (irow)
    // - node->children (irow)
    // - block->rows (row)
    // - block->cols (row)
    // row = unknowns in App
    // irow = block #
    // Returns roots, the roots of the elimination tree (usually 1 - not always)
    vector<int> build_tree(VectorXi &i2irow) {
        vector<set<int>> rows_tmp(nblk); // The mapping block -> rows under
        vector<int> roots(0);
        for (int k = 0; k < nblk; k++)
        {
            auto &n = nodes.at(k);
            // Add local rows
            for (int j = n->start; j < n->end; j++)
            {
                for (SpMat::InnerIterator it(App, j); it; ++it)
                {
                    int i = it.row();
                    if (i >= n->end)
                    {
                        rows_tmp.at(k).insert(i);
                    }
                }
            }
            // Get sorted set of neighbros
            vector<int> rows(rows_tmp.at(k).begin(), rows_tmp.at(k).end());
            sort(rows.begin(), rows.end());
            // Convert to neighbors
            set<int> nbrs_tmp;
            for (auto i : rows)
                nbrs_tmp.insert(i2irow(i));
            n->nbrs = vector<int>(nbrs_tmp.begin(), nbrs_tmp.end());
            sort(n->nbrs.begin(), n->nbrs.end());
            // Diagonal bloc
            blocs[{k, k}] = make_unique<Bloc>(k, k);
            auto &b = blocs[{k, k}];
            b->rows = vector<int>(n->size);
            for (int i = 0; i < n->size; i++)
                b->rows[i] = n->start + i;
            b->cols = b->rows;
            // Below-diagonal bloc
            for (auto nirow : n->nbrs)
            {
                auto &nbr = nodes.at(nirow);
                blocs[{nirow, k}] = make_unique<Bloc>(nirow, k);
                auto &b = blocs[{nirow, k}];
                // Find rows
                auto lower = lower_bound(rows.begin(), rows.end(), nbr->start);
                auto upper = upper_bound(rows.begin(), rows.end(), nbr->end - 1);
                b->rows = vector<int>(lower, upper);
                b->cols = blocs.at({k, k})->rows;
            }
            // Add to parent
            if (n->nbrs.size() > 0)
            {
                assert(n->nbrs[0] > k);
                int prow = n->nbrs[0]; // parent in etree = first non zero in column
                n->parent = prow;
                auto &p = nodes.at(prow);
                for (auto i : rows_tmp.at(k))
                {
                    if (i >= p->end)
                    {
                        rows_tmp.at(prow).insert(i);
                    }
                }
                p->children.push_back(k);
                // if(comm_rank() == 0) printf("%d -> %d\n", k, prow);
            }
            else
            {
                n->parent = -1;
                roots.push_back(k);
            }
        }
        printf("%lu roots (should be 1 in general)\n", roots.size());
        return roots;
    }

    // Fill node2rank[k] with a rank for supernode k
    void distribute_tree(vector<int> &roots) {
        queue<int> toexplore;
        vector<range> node2range(nblk);
        for (auto k : roots)
        {
            node2range[k] = {0, comm_size(), 0};
            toexplore.push(k);
        }
        // Distribute tree
        while (!toexplore.empty())
        {
            int k = toexplore.front();
            // if(comm_rank() == 0) printf("exploring %d\n", k);
            auto r = node2range.at(k);
            toexplore.pop();
            auto &n = nodes.at(k);
            if (n->children.size() == 0)
            {
                // Done
            }
            else if (n->children.size() == 1)
            {
                // Same range, just cycle by 1
                auto c = n->children[0];
                assert(r.ub > r.lb);
                int newk = r.lb + (r.k - r.lb + 1) % (r.ub - r.lb);
                node2range[c] = {r.lb, r.ub, newk};
                // if(comm_rank() == 0) printf(" children %d\n", c);
                toexplore.push(c);
            }
            else
            {
                int nc = n->children.size();
                int step = (r.ub - r.lb) / nc;
                // if(comm_rank() == 0) printf("lb ub step nc %d %d %d %d\n", r.lb, r.ub, step, nc);
                if (step == 0)
                { // To many children. Cycle by steps of 1.
                    for (int i = 0; i < nc; i++)
                    {
                        auto c = n->children[i];
                        auto start = r.lb + i % (r.ub - r.lb);
                        auto end = start + 1;
                        assert(start < end);
                        node2range[c] = {start, end, start};
                        toexplore.push(c);
                        // if(comm_rank() == 0) printf(" children %d start %d end %d\n", c, start, end);
                    }
                }
                else
                {
                    for (int i = 0; i < nc; i++)
                    {
                        auto c = n->children[i];
                        auto start = r.lb + i * step;
                        auto end = r.lb + (i + 1) * step;
                        assert(start < end);
                        node2range[c] = {start, end, start};
                        toexplore.push(c);
                        // if(comm_rank() == 0) printf(" children %d start %d end %d\n", c, start, end);
                    }
                }
            }
            // if(comm_rank() == 0) printf(" childrnode2range size %d\n", node2range.size());
        }
        node2rank = vector<int>(nblk, 0);
        for (int i = 0; i < nblk; i++)
        {
            node2rank[i] = node2range.at(i).k;
            // if(comm_rank() == 0)
            // printf("[%d] Node %d - %d\n", comm_rank(), i, node2rank[i]);
        }
    }

    // Allocate blocks[i,k]->A() when column k resides on this rank
    // Fill blocks[i,k]->A() when column k resides on this rank
    void allocate_blocks(VectorXi& i2irow) {
        for (int k = 0; k < nblk; k++)
        {
            if (col2rank(k) == comm_rank())
            {
                // Allocate
                auto &n = nodes.at(k);
                auto &b = blocs.at({k, k});
                b->matA = make_unique<MatrixXd>(n->size, n->size);
                b->A()->setZero();
                for (auto nirow : n->nbrs)
                {
                    auto &b = blocs.at({nirow, k});
                    b->matA = make_unique<MatrixXd>(b->rows.size(), n->size);
                    b->A()->setZero();
                }
                // Fill
                for (int j = n->start; j < n->end; j++)
                {
                    for (SpMat::InnerIterator it(App, j); it; ++it)
                    {
                        int i = it.row();
                        if (i >= n->start)
                        {
                            // Find bloc
                            int irow = i2irow[i];
                            auto &b = blocs.at({irow, k});
                            // Find row
                            auto found = lower_bound(b->rows.begin(), b->rows.end(), i);
                            assert(found != b->rows.end());
                            int ii = distance(b->rows.begin(), found);
                            int jj = j - n->start;
                            (*b->A())(ii, jj) = it.value();
                        }
                    }
                }

            }
        }
    }

    DistMat(std::string filename, int nlevels, int block_size) : nlevels(nlevels), block_size(block_size), nblk(-1), gemm_us(0), trsm_us(0), potf_us(0), scat_us(0), allo_us(0)
    {
        std::cout << "Reading matrix file " << filename << std::endl;
        // Read matrix
        A = mmio::sp_mmread<double, int>(filename);
        // Initialize & prepare
        int N = A.rows();
        VectorXi colptr, rowval;
        get_csc_no_diag(A, &rowval, &colptr);
        // Partition the matrix
        VectorXi rangtab;
        algebraic_partitioning(N, rowval, colptr, nlevels, block_size, &perm, &rangtab, &depth);
        assert(rangtab.size() > 0);
        assert(perm.size() == N);
        nblk = rangtab.size()-1;
        // Permute matrix
        App = perm.asPermutation() * A * perm.asPermutation().transpose();
        // Create all supernodes
        // i2irow maps row/col -> irow (row/col -> block)
        VectorXi i2irow = build_supernodes(rangtab);
        // Compute elimination tree & neighbors
        // roots are the etree roots
        vector<int> roots = build_tree(i2irow);
        // Distribute the tree
        distribute_tree(roots);
        // Allocate blocs
        allocate_blocks(i2irow);
    }

    void print()
    {
        MatrixXd Aff = MatrixXd::Zero(perm.size(), perm.size());
        for (int k = 0; k < nblk; k++)
        {
            auto &n = nodes.at(k);
            int start = n->start;
            int size = n->size;
            Aff.block(start, start, size, size) = blocs.at({k, k})->A()->triangularView<Lower>();
            for (auto i : n->nbrs)
            {
                MatrixXd *Aik = blocs.at({i, k})->A();
                for (int ii = 0; ii < Aik->rows(); ii++)
                {
                    for (int jj = 0; jj < Aik->cols(); jj++)
                    {
                        Aff(blocs.at({i, k})->rows[ii], blocs.at({i, k})->cols[jj]) = (*Aik)(ii, jj);
                    }
                }
            }
        }
        cout << Aff << endl;
    }

    // Factor a diagonal pivot in-place
    void potf(int krow)
    {
        MatrixXd *Ass = blocs.at({krow, krow})->A();
        timer t0 = wctime();
        int err = LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', Ass->rows(), Ass->data(), Ass->rows());
        timer t1 = wctime();
        potf_us += (long long)(elapsed(t0, t1) * 1e6);
        assert(err == 0);
    }

    // Trsm a panel bloc in-place
    void trsm(int2 kirow)
    {
        int krow = kirow[0];
        int irow = kirow[1];
        MatrixXd *Ass = blocs.at({krow, krow})->A();
        MatrixXd *Ais = blocs.at({irow, krow})->A();
        timer t0 = wctime();
        cblas_dtrsm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit,
                    Ais->rows(), Ais->cols(), 1.0, Ass->data(), Ass->rows(), Ais->data(), Ais->rows());
        timer t1 = wctime();
        trsm_us += (long long)(elapsed(t0, t1) * 1e6);
    }

    // Perform a gemm between (i,k) and (j,k) and store the result at (i,j) in to_accumulate
    void gemm(int3 kijrow)
    {
        int krow = kijrow[0];
        int irow = kijrow[1];
        int jrow = kijrow[2];
        MatrixXd *Ais = blocs.at({irow, krow})->A();
        MatrixXd *Ajs = blocs.at({jrow, krow})->A();
        // Do the math
        timer t0, t1, t2;
        t0 = wctime();
        auto Aij_acc = make_unique<MatrixXd>(Ais->rows(), Ajs->rows());
        t1 = wctime();
        if (jrow == irow)
        { // Aii_ = -Ais Ais^T
            cblas_dsyrk(CblasColMajor, CblasLower, CblasNoTrans,
                        Ais->rows(), Ais->cols(), -1.0, Ais->data(), Ais->rows(), 0.0, Aij_acc->data(), Aij_acc->rows());
        }
        else
        { // Aij_ = -Ais Ajs^T
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans,
                        Ais->rows(), Ajs->rows(), Ais->cols(), -1.0, Ais->data(), Ais->rows(), Ajs->data(), Ajs->rows(), 0.0, Aij_acc->data(), Aij_acc->rows());
        }
        t2 = wctime();
        {
            auto &mtx = blocs.at({irow, jrow})->to_accumulate_mtx;
            auto &acc = blocs.at({irow, jrow})->to_accumulate;
            lock_guard<mutex> lock(mtx);
            acc[krow] = move(Aij_acc);
        }
        allo_us += (long long)(elapsed(t0, t1) * 1e6);
        gemm_us += (long long)(elapsed(t1, t2) * 1e6);
    }

    void accumulate(int3 kijrow)
    {
        int krow = kijrow[0];
        int irow = kijrow[1];
        int jrow = kijrow[2];
        auto &mtx = blocs.at({irow, jrow})->to_accumulate_mtx;
        auto &acc = blocs.at({irow, jrow})->to_accumulate;
        {
            assert(!blocs.at({irow, jrow})->accumulating_busy.load());
            blocs.at({irow, jrow})->accumulating_busy.store(true);
        }
        unique_ptr<MatrixXd> Aij_acc;
        MatrixXd *Aij = blocs.at({irow, jrow})->A();
        timer t0, t1;
        {
            lock_guard<mutex> lock(mtx);
            Aij_acc = move(acc.at(krow));
            acc.erase(acc.find(krow));
        }
        t0 = wctime();
        if (jrow == irow)
        { // Aii_ = -Ais Ais^T
            auto Iids = get_subids(blocs.at({irow, krow})->rows, blocs.at({irow, jrow})->rows);
            for (int j = 0; j < Aij_acc->cols(); j++)
            {
                for (int i = j; i < Aij_acc->rows(); i++)
                {
                    (*Aij)(Iids[i], Iids[j]) += (*Aij_acc)(i, j);
                }
            }
        }
        else
        { // Aij_ = -Ais Ajs^T
            auto Iids = get_subids(blocs.at({irow, krow})->rows, blocs.at({irow, jrow})->rows);
            auto Jids = get_subids(blocs.at({jrow, krow})->rows, blocs.at({irow, jrow})->cols);
            for (int j = 0; j < Aij_acc->cols(); j++)
            {
                for (int i = 0; i < Aij_acc->rows(); i++)
                {
                    (*Aij)(Iids[i], Jids[j]) += (*Aij_acc)(i, j);
                }
            }
        }
        t1 = wctime();
        scat_us += (long long)(elapsed(t0, t1) * 1e6);
        {
            assert(blocs.at({irow, jrow})->accumulating_busy.load());
            blocs.at({irow, jrow})->accumulating_busy.store(false);
        }
    }

    void factorize(const int n_threads, const int verb, const bool want_log)
    {
        for (int k = 0; k < nblk; k++) {
            const auto &n = nodes.at(k);
            for (auto i : n->nbrs) {
                for (auto j : n->nbrs) {
                    if (i >= j) {
                        auto &b = blocs.at({i, j});
                        b->n_accumulate++;
                    }
                }
            }
        }

        MPI_Barrier(MPI_COMM_WORLD);
        timer t0 = wctime();
        printf("Rank %d starting w/ %d threads\n", comm_rank(), n_threads);
        Logger log(1000000);
        Communicator comm(MPI_COMM_WORLD, verb);
        Threadpool tp(n_threads, &comm, verb, "[" + to_string(comm_rank()) + "]_");
        Taskflow<int> pf(&tp, verb);
        Taskflow<int2> tf(&tp, verb);
        Taskflow<int3> gf(&tp, verb);
        Taskflow<int3> rf(&tp, verb);
        const int my_rank = comm_rank();

        auto am_send_panel = comm.make_active_msg(
            [&](int &i, int &k, int &isize, int &ksize, view<double> &Aik, view<int> &js) {
                Bloc *b = this->blocs.at({i, k}).get();
                b->matA = make_unique<MatrixXd>(isize, ksize);
                memcpy(b->A()->data(), Aik.data(), Aik.size() * sizeof(double));
                for (auto j : js)
                {
                    gf.fulfill_promise(lower({k, i, j}));
                }
            });

        if (want_log)
        {
            tp.set_logger(&log);
            comm.set_logger(&log);
        }

        pf
            .set_mapping([&](int k) {
                assert(col2rank(k) == my_rank);
                return (k % n_threads);
            })
            .set_indegree([&](int k) {
                assert(col2rank(k) == my_rank);
                int ngemms = n_to_accumulate({k, k});
                return ngemms == 0 ? 1 : ngemms; // # gemms before ?
            })
            .set_task([&](int k) {
                assert(accumulated({k, k}) == n_to_accumulate({k, k}));
                assert(col2rank(k) == my_rank);
                potf(k);
            })
            .set_fulfill([&](int k) {
                assert(accumulated({k, k}) == n_to_accumulate({k, k}));
                assert(col2rank(k) == my_rank);
                auto &n = nodes.at(k);
                for (auto i : n->nbrs)
                {
                    tf.fulfill_promise({k, i});
                }
            })
            .set_name([&](int k) {
                return "[" + to_string(my_rank) + "]_potf_" + to_string(k) + "_lvl" + to_string(depth.at(k));
            })
            .set_priority([](int k) {
                return 3.0;
            });

        tf
            .set_mapping([&](int2 ki) {
                assert(col2rank(ki[0]) == my_rank);
                return (ki[0] % n_threads);
            })
            .set_indegree([&](int2 ki) {
                assert(col2rank(ki[0]) == my_rank);
                int k = ki[0];
                int i = ki[1];
                assert(i > k);
                return n_to_accumulate({i, k}) + 1; // # gemm before + potf
            })
            .set_task([&](int2 ki) {
                assert(col2rank(ki[0]) == my_rank);
                assert(accumulated({ki[1], ki[0]}) == n_to_accumulate({ki[1], ki[0]}));
                trsm(ki);
            })
            .set_fulfill([&](int2 ki) {
                assert(col2rank(ki[0]) == my_rank);
                assert(accumulated({ki[1], ki[0]}) == n_to_accumulate({ki[1], ki[0]}));
                int k = ki[0];
                int i = ki[1];
                Node *n = nodes.at(k).get();
                map<int, vector<int>> deps;
                for (auto j : n->nbrs)
                {
                    int dest = col2rank(lower({k, i, j})[2]);
                    if (dest != my_rank)
                    {
                        deps[dest] = {};
                    }
                }
                for (auto j : n->nbrs)
                {
                    int dest = col2rank(lower({k, i, j})[2]);
                    if (dest != my_rank)
                    {
                        deps[dest].push_back(j);
                    }
                    else
                    {
                        gf.fulfill_promise(lower({k, i, j}));
                    }
                }
                for (auto dep : deps)
                {
                    int dest = dep.first;
                    auto js = dep.second;
                    MatrixXd *Aik = blocs.at({i, k})->A();
                    int isize = Aik->rows();
                    int ksize = Aik->cols();
                    assert(Aik->size() == isize * ksize);
                    auto vAik = view<double>(Aik->data(), Aik->size());
                    auto vJs = view<int>(js.data(), js.size());
                    am_send_panel->send(dest, i, k, isize, ksize, vAik, vJs);
                }
            })
            .set_name([&](int2 ki) {
                return "[" + to_string(my_rank) + "]_trsm_" + to_string(ki[0]) + "_" + to_string(ki[1]) + "_lvl" + to_string(depth[ki[0]]);
            })
            .set_priority([](int2 k) {
                return 2.0;
            });

        gf
            .set_mapping([&](int3 kij) {
                assert(col2rank(kij[2]) == my_rank);
                return (kij[0] % n_threads);
            })
            .set_indegree([&](int3 kij) {
                assert(col2rank(kij[2]) == my_rank);
                int i = kij[1];
                int j = kij[2];
                assert(j <= i);
                return (i == j ? 1 : 2); // Trsms
            })
            .set_task([&](int3 kij) {
                assert(col2rank(kij[2]) == my_rank);
                gemm(kij);
            })
            .set_fulfill([&](int3 kij) {
                assert(col2rank(kij[2]) == my_rank);
                int k = kij[0];
                int i = kij[1];
                int j = kij[2];
                assert(k <= j);
                assert(j <= i);
                // printf("gf %d %d %d -> rf %d %d %d\n", my_rank, k, i, j, k, i, j);
                rf.fulfill_promise(kij);
            })
            .set_name([&](int3 kij) {
                return "[" + to_string(my_rank) + "]_gemm_" + to_string(kij[0]) + "_" + to_string(kij[1]) + "_" + to_string(kij[2]) + "_lvl" + to_string(depth[kij[0]]);
            })
            .set_priority([](int3) {
                return 1.0;
            });

        rf
            .set_mapping([&](int3 kij) {
                assert(col2rank(kij[2]) == my_rank);
                return (kij[1] + kij[2]) % n_threads; // any i & j -> same thread. So k cannot appear in this expression
            })
            .set_indegree([&](int3 kij) {
                assert(col2rank(kij[2]) == my_rank);
                return 1; // The corresponding gemm
            })
            .set_task([&](int3 kij) {
                assert(col2rank(kij[2]) == my_rank);
                blocs.at({kij[1], kij[2]})->accumulated++;
                accumulate(kij);
            })
            .set_fulfill([&](int3 kij) {
                int i = kij[1];
                int j = kij[2];
                if (i == j)
                {
                    // printf("rf %d %d %d -> pf %d\n", k, i, j, i);
                    pf.fulfill_promise(i);
                }
                else
                {
                    // printf("rf %d %d %d -> tf %d %d\n", k, i, j, j, i);
                    tf.fulfill_promise({j, i});
                }
            })
            .set_name([&](int3 kij) {
                return "[" + to_string(my_rank) + "]_acc_" + to_string(kij[0]) + "_" + to_string(kij[1]) + "_" + to_string(kij[2]) + "_lvl" + to_string(depth[kij[0]]);
            })
            .set_priority([](int3) {
                return 4.0;
            })
            .set_binding([](int3) {
                return true;
            });

        for (int k = 0; k < nblk; k++)
        {
            if (col2rank(k) == my_rank)
            {
                if (n_to_accumulate({k, k}) == 0)
                {
                    pf.fulfill_promise(k);
                }
            }
        }

        tp.join();
        MPI_Barrier(MPI_COMM_WORLD);
        printf("Tp & Comms done\n");
        timer t1 = wctime();
        printf("Factorization done, time %3.2e s.\n", elapsed(t0, t1));
        printf("Potf %3.2e s., %3.2e s./thread\n", double(potf_us / 1e6), double(potf_us / 1e6) / n_threads);
        printf("Trsm %3.2e s., %3.2e s./thread\n", double(trsm_us / 1e6), double(trsm_us / 1e6) / n_threads);
        printf("Gemm %3.2e s., %3.2e s./thread\n", double(gemm_us / 1e6), double(gemm_us / 1e6) / n_threads);
        printf("Allo %3.2e s., %3.2e s./thread\n", double(allo_us / 1e6), double(allo_us / 1e6) / n_threads);
        printf("Scat %3.2e s., %3.2e s./thread\n", double(scat_us / 1e6), double(scat_us / 1e6) / n_threads);
        printf(">>>>snchol my_rank %d n_ranks %d n_threads %d n_cores_total %d matrix_size %d block_size %d nlevels %d fact_time %3.2e\n",
                           my_rank, comm_size(), n_threads, n_threads * comm_size(), A.rows(), block_size, nlevels, elapsed(t0, t1));

        auto am_send_pivot = comm.make_active_msg(
            [&](int &k, int &ksize, view<double> &Akk) {
                auto &b = this->blocs.at({k, k});
                b->matA = make_unique<MatrixXd>(ksize, ksize);
                memcpy(b->A()->data(), Akk.data(), Akk.size() * sizeof(double));
            });

        auto am_send_panel2 = comm.make_active_msg(
            [&](int &i, int &k, int &isize, int &ksize, view<double> &Aik) {
                auto &b = this->blocs.at({i, k});
                b->matA = make_unique<MatrixXd>(isize, ksize);
                memcpy(b->A()->data(), Aik.data(), Aik.size() * sizeof(double));
            });

        if (my_rank != 0)
        {
            for (int k = 0; k < nblk; k++)
            {
                if (col2rank(k) == my_rank)
                {
                    // Send column to 0
                    auto &n = nodes.at(k);
                    { // Pivot
                        MatrixXd *Akk = blocs.at({k, k})->A();
                        int ksize = Akk->rows();
                        auto vAkk = view<double>(Akk->data(), Akk->size());
                        am_send_pivot->send(0, k, ksize, vAkk);
                    }
                    for (auto i : n->nbrs)
                    {
                        MatrixXd *Aik = blocs.at({i, k})->A();
                        int ksize = Aik->cols();
                        int isize = Aik->rows();
                        auto vAik = view<double>(Aik->data(), Aik->size());
                        am_send_panel2->send(0, i, k, isize, ksize, vAik);
                    }
                }
            }
        }
        else
        {
            for (int k = 0; k < nblk; k++)
            {
                if (col2rank(k) != 0)
                {
                    comm.recv_process();
                    auto &n = nodes.at(k);
                    for (auto i : n->nbrs)
                        comm.recv_process();
                }
            }
        }

        while(! comm.is_done()) {
            comm.progress();
        }

        if (want_log)
        {
            ofstream logfile;
            string filename = FOLDER + "/snchol_" + to_string(comm_size()) + "_" + to_string(n_threads) + "_" + to_string(App.rows()) + ".log." + to_string(my_rank);
            printf("[%d] Logger saved to %s\n", my_rank, filename.c_str());
            logfile.open(filename);
            logfile << log;
            logfile.close();
        }
    }

    VectorXd solve(VectorXd &b)
    {
        assert(comm_rank() == 0);
        VectorXd xglob = perm.asPermutation() * b;
        // Set solution on each node
        for (int krow = 0; krow < nblk; krow++)
        {
            auto &k = nodes.at(krow);
            k->xsol = xglob.segment(k->start, k->size);
        }
        // Forward
        for (int krow = 0; krow < nblk; krow++)
        {
            auto &k = nodes.at(krow);
            // Pivot xs <- Lss^-1 xs
            MatrixXd *Lss = blocs.at({krow, krow})->A();
            cblas_dtrsv(CblasColMajor, CblasLower, CblasNoTrans, CblasNonUnit, Lss->rows(), Lss->data(), Lss->rows(), k->xsol.data(), 1);
            // Neighbors
            for (int irow : k->nbrs)
            {
                auto &n = nodes.at(irow);
                MatrixXd *Lns = blocs.at({irow, krow})->A();
                VectorXd xn(Lns->rows());
                // xn = -Lns xs
                cblas_dgemv(CblasColMajor, CblasNoTrans, Lns->rows(), Lns->cols(), -1.0, Lns->data(), Lns->rows(), k->xsol.data(), 1, 0.0, xn.data(), 1);
                // Reduce into xn
                auto Iids = get_subids(blocs.at({irow, krow})->rows, blocs.at({irow, irow})->cols);
                for (int i = 0; i < xn.size(); i++)
                {
                    n->xsol(Iids[i]) += xn(i);
                }
            }
        }
        // Backward
        for (int krow = nblk - 1; krow >= 0; krow--)
        {
            auto &k = nodes.at(krow);
            // Neighbors
            for (int irow : k->nbrs)
            {
                auto &n = nodes.at(irow);
                MatrixXd *Lns = blocs.at({irow, krow})->A();
                VectorXd xn(Lns->rows());
                // Fetch from xn
                auto Iids = get_subids(blocs.at({irow, krow})->rows, blocs.at({irow, irow})->cols);
                for (int i = 0; i < xn.size(); i++)
                {
                    xn(i) = n->xsol(Iids[i]);
                }
                // xs -= Lns^T xn
                cblas_dgemv(CblasColMajor, CblasTrans, Lns->rows(), Lns->cols(), -1.0, Lns->data(), Lns->rows(), xn.data(), 1, 1.0, k->xsol.data(), 1);
            }
            // xs = Lss^-T xs
            MatrixXd *Lss = blocs.at({krow, krow})->A();
            cblas_dtrsv(CblasColMajor, CblasLower, CblasTrans, CblasNonUnit, Lss->rows(), Lss->data(), Lss->rows(), k->xsol.data(), 1);
        }
        // Back to x
        for (int krow = 0; krow < nblk; krow++)
        {
            auto &k = nodes.at(krow);
            xglob.segment(k->start, k->size) = k->xsol;
        }
        return perm.asPermutation().transpose() * xglob;
    }
};

void run_cholesky(const std::string filename, const int nlevels, const int n_threads, const int block_size, const int verb, const bool log)
{
    printf("[%d] Hello from %s\n", comm_rank(), processor_name().c_str());
    DistMat dm(filename, nlevels, block_size);
    SpMat A = dm.A;
    dm.factorize(n_threads, verb, log);
    if (comm_rank() == 0)
    {
        VectorXd b = random(A.rows(), 2019);
        VectorXd x = dm.solve(b);
        double res = (A * x - b).norm() / b.norm();
        printf("|Ax-b|/|b| = %e\n", res);
        if(res <= 1e-12) {
            printf("Test ok!");
        } else {
            printf("Error!");
            exit(1);
        }
    }
}

int main(int argc, const char **argv)
{
    int req = MPI_THREAD_FUNNELED;
    int prov = -1;
    int err = MPI_Init_thread(NULL, NULL, req, &prov);
    assert(err == 0 && prov == req);

    std::stringstream sstr;
    sstr << comm_size();
    const std::string comm_size_str = sstr.str();

    cxxopts::Options options("2d_cholesky", "2D dense cholesky using TaskTorrent");
    options.add_options()
        ("help", "Show help and exit")
        ("n_threads", "Number of threads", cxxopts::value<int>()->default_value("4"))
        ("verb", "Verbosity level", cxxopts::value<int>()->default_value("0"))
        ("block_size", "Block size", cxxopts::value<int>()->default_value("10"))
        ("matrix", "Filename to matrix in MM format", cxxopts::value<std::string>()->default_value("neglapl_2_32.mm"))
        ("nlevels", "Number of ND levels", cxxopts::value<int>()->default_value("10"))
        ("log", "Enable logging");
    auto result = options.parse(argc, argv);

    if (result.count("help")) {
        std::cout << options.help({"", "Group"}) << endl;
        exit(0);
    }

    const int n_threads = result["n_threads"].as<int>();
    const int verb = result["verb"].as<int>();
    const int block_size = result["block_size"].as<int>();
    const bool log = result["log"].as<bool>();
    const std::string filename = result["matrix"].as<std::string>();
    const int nlevels = result["nlevels"].as<int>();

    run_cholesky(filename, nlevels, n_threads, block_size, verb, log);
    MPI_Finalize();
    return 0;
}


#include "rasterWindow\rasterwindow.h"
//#include "lightwindow.h"
#include <QtWidgets/QApplication>
//#include "macrodef.h"
#include "svpng.inc"
#include "SDF.h"
#include "Scene.h"

#include <dlib/matrix.h>

int main(int argc, char *argv)
{
	//QApplication a(argc, argv);
	//LightWindow w;
	//w.show();
	//return a.exec();

	CircleSDF circle_1 = CircleSDF(2.0f, 0.3f, 0.3f, 0.10f);
	CircleSDF circle_2 = CircleSDF(0.8f, 0.7f, 0.7f, 0.05f);
	CircleSDF circle_3 = CircleSDF(0.0f, 0.7f, 0.65f, 0.05f);
	CircleSDF circle_4 = CircleSDF(0.0f, 0.35f, 0.3f, 0.10f);
	PlaneSDF plane_1 = PlaneSDF(0.5, 0.0, 0.7, 0.0, -1.0);
	Scene my_scene{ WIDTH,HEIGHT,SAMPLE_N};

	my_scene.addSDF(circle_1, BoolOp::UNION_OP)
		.addSDF(circle_4, BoolOp::UNION_OP)
		.addSDF(circle_2, BoolOp::UNION_OP)
		.addSDF(circle_3, BoolOp::UNION_OP)
		.addSDF(plane_1, BoolOp::SUBTRACT_OP)
	;

	unsigned char rgb[WIDTH * HEIGHT * 3], *p = rgb;
	//unsigned x, y;
	FILE *fp;
	fopen_s(&fp,"rgb.png", "wb");

#ifdef parallel
	dlib::parallel_for(0, HEIGHT, [&](int y) {
			dlib::parallel_for(0, WIDTH, [&](int x) {
				p[0] = p[1] = p[2] = int(fminf(my_scene.sample(float(x) / WIDTH, float(y) / HEIGHT) * 255.0f, 255.0f));
				p += 3;
			});
		});

#else

	for (int y = 0; y < HEIGHT; y++) {
		for (int x = 0; x <WIDTH; x++ , p+=3) {
			p[0] = p[1] = p[2] = int(fminf(
				my_scene.sample(float(x) / WIDTH, float(y) / HEIGHT) * 255.0f, 255.0f));
			//p[0] = color;    /* R */
			//p[1] = color;    /* G */
			//p[2] = color;    /* B */
		}
	}
#endif
	svpng(fp, WIDTH, HEIGHT, rgb, 0);
	fclose(fp);
	return 0;
}


/**
 * @file dt_tools.cpp
 * @brief Reader for the BOS ascii files
 * @author Oleg Borschuk
 * @version 0.1
 * @date 2012-03-01
 */

#include "dt_tools.h"
#include "bs_kernel.h"
//#include "localization.h"
#include <math.h>
#include "toupper.h"

using namespace boost;

#ifdef BSPY_EXPORTING_PLUGIN

#include <boost/python.hpp>
using namespace boost::python;

#endif //BSPY_EXPORTING_PLUGIN

/*! \brief Nonzero if YEAR is a leap year (every 4 years,\n
except every 100th isn't, and every 400th is).  */
#define IS_LEAP_YEAR(YEAR) \
  ((YEAR) % 4 == 0 && ((YEAR) % 100 != 0 || (YEAR) % 400 == 0))

namespace blue_sky
{

  dt_tools::dt_tools (bs_type_ctor_param)
    {
    }
  dt_tools::dt_tools (const dt_tools& /*rhs*/)
        : bs_refcounter ()
    {
    }
  dt_tools::~dt_tools ()
    {
    }

  int
  dt_tools::cstr2d (const char *buf, double &date) const
    {
      int day, month, year;

      date = ecl2d (buf);
      if (date < 0)
        {
          if (sscanf (buf, "%d.%d.%d", &day, &month, &year) != 3)
            {
              fprintf (stderr, "Error: invalid date format: %s\n", buf);
              return -1;
            }
          date = ymd2d (year, month, day);
        }
      return 0;
    }

  void
  dt_tools::d2cstr (const double date, char *buf) const
    {
      int d, m, y;
      d2ymd (date, y, m, d);
      sprintf (buf, "%02d.%02d.%04d", d, m, y);
    }

  int
  dt_tools::cstr2t (const char *buf, double &tm) const
    {
      int h, m;
      double s;

      if (sscanf (buf, "%d:%d:%lf", &h, &m, &s) != 3)
        {
          fprintf (stderr, "Error: invalid time format: %s\n", buf);
          return -1;
        }

      // Check month and year

      if (h < 0 || h > 23)
        {
          fprintf (stderr, "Error: hours should be 0..23: %s\n", buf);
          return -1;
        }

      if (m < 0 || m > 59)
        {
          fprintf (stderr, "Error: minutes should be 0..59: %s\n", buf);
          return -1;
        }
      if (s < 0 || s >= 60.0)
        {
          fprintf (stderr, "Error: seconds should be 0..59: %s\n", buf);
          return -1;
        }
      tm = hms2t (h, m, s); //((double)h + ((double)m + (double)s / 60.0) / 60.0) / 24.0;
      return 0;
    }

  void
  dt_tools::t2cstr (const double tm, char *buf) const
    {
      int h, m, s;
      //double ss;

      //ss = d2hms (tm, h, m, s);
      t2hms (tm, h, m, s);
      //ss += s;

      sprintf (buf, "%02d:%02d:%02d", h, m, s);
    }

  double
  dt_tools::d2ymd (const double date, int &year, int &month, int &day) const
    {
      static const int days_in_month[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
      double next_date, datetime, d;
      datetime = date;
      d = floor(date);
      for (year = 0; d > 0; year++)
        {
          if (IS_LEAP_YEAR (1900 + year))
            next_date = d - 366;
          else
            next_date = d - 365;
          if (next_date <= 0)
            break;
          d = next_date;
        }

      for (month = 0; d > 0; month++)
        {
          next_date = d - days_in_month[month];
          if (IS_LEAP_YEAR (1900 + year) && month == 1)
            next_date--;
          if (next_date <= 0)
            break;
          d = next_date;
        }

      day = (int) d;
      month++;
      year += 1900;

      return (datetime - floor(datetime));
    }

  double
  dt_tools::ymd2d (const int y_, const int m_, const int d_) const
    {
      int days_in_current_month;
      static const int days_in_month[12] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
      int i;
      double date;
      int month = m_, year = y_, day = d_;

      // Check month and year

      if (year < 1900)
        {
          fprintf (stderr, "Error: years earlier 1900 not supported\n");
          return -1;
        }

      if (month < 1 || month > 12)
        {
          fprintf (stderr, "Error: invalid (nonexistent) date\n");
          return -1;
        }

      date = 0;

      for (i = 1900; i < year; i++)
        {
        if (IS_LEAP_YEAR (i))
          date += 366;
        else
          date += 365;
        }

      month--;

      for (i = 0; i < month; i++)
        {
          date += days_in_month[i];
          if (IS_LEAP_YEAR (year) && i == 1) // February of leap year
            date++;
        }
      date += day;

      days_in_current_month = days_in_month[month];
      if (IS_LEAP_YEAR (year) && month == 1) // February of leap year
        days_in_current_month++;

      // check day
      if (day < 1 || day > days_in_current_month)
        {
          fprintf (stderr, "Error: invalid (nonexistent) date\n");
          return -1;
        }

      return date;
    }

  double
  dt_tools::t2hms (const double tm, int &h, int &m, int &s) const
    {
      double d, n, hh, mm, ss;

      d = modf (tm + 1.0e-13, &n);
      d = modf (d * 24.0, &hh);
      d = modf (d * 60.0, &mm);
      d = modf (d * 60.0, &ss);
      h = (int)hh;
      m = (int)mm;
      s = (int)ss;

      return d;
    }

  double
  dt_tools::hms2t (const int h, const int m, const int s) const
    {
      return ((double)h + ((double)m + (double)s / 60.0) / 60.0) / 24.0;
    }

  void
  dt_tools::d2ecl (const double date, char *buf) const
    {
      int d, m, y;
      d2ymd (date, y, m, d);

      switch (m)
      {
        case 1:
          sprintf (buf, "%2.2d \'JAN\' %4.4d /", d, y);
          break;
        case 2:
          sprintf (buf, "%2.2d \'FEB\' %4.4d /", d, y);
          break;
        case 3:
          sprintf (buf, "%2.2d \'MAR\' %4.4d /", d, y);
          break;
        case 4:
          sprintf (buf, "%2.2d \'APR\' %4.4d /", d, y);
          break;
        case 5:
          sprintf (buf, "%2.2d \'MAY\' %4.4d /", d, y);
          break;
        case 6:
          sprintf (buf, "%2.2d \'JUN\' %4.4d /", d, y);
          break;
        case 7:
          sprintf (buf, "%2.2d \'JUL\' %4.4d /", d, y);
          break;
        case 8:
          sprintf (buf, "%2.2d \'AUG\' %4.4d /", d, y);
          break;
        case 9:
          sprintf (buf, "%2.2d \'SEP\' %4.4d /", d, y);
          break;
        case 10:
          sprintf (buf, "%2.2d \'OCT\' %4.4d /", d, y);
          break;
        case 11:
          sprintf (buf, "%2.2d \'NOV\' %4.4d /", d, y);
          break;
        case 12:
          sprintf (buf, "%2.2d \'DEC\' %4.4d /", d, y);
          break;
      }
    }

  double
  dt_tools::ecl2d (const char *buf) const
    {
      char string[512];
      char *ptr_start = 0;
      char *ptr_end = 0;
      int i, year, day, month;

      if (!buf)
        return -3;

      // Need to add reading check
      if (sscanf (buf, "%d %s %d", &day, string, &year) != 3)
        {
          return -1;
        }

      locale_ucase(string);
      ptr_start = string;

      while (*ptr_start == '\'' || *ptr_start == '\"')
        ++ptr_start;
      for (i = 0; ptr_start[i] != '\0'; ++i)
        ;
      ptr_end = &(ptr_start[i]) - 1;
      while (*ptr_end == '\'' || *ptr_end == '\"')
        --ptr_end;
      *(ptr_end + 1) = '\0';
      if (!strcmp (ptr_start, "JAN"))
        month = 1;
      else if (!strcmp (ptr_start, "FEB"))
        month = 2;
      else if (!strcmp (ptr_start, "MAR"))
        month = 3;
      else if (!strcmp (ptr_start, "APR"))
        month = 4;
      else if (!strcmp (ptr_start, "MAY"))
        month = 5;
      else if (!strcmp (ptr_start, "JUN"))
        month = 6;
      else if (!strcmp (ptr_start, "JLY") || !strcmp (ptr_start, "JUL"))
        month = 7;
      else if (!strcmp (ptr_start, "AUG"))
        month = 8;
      else if (!strcmp (ptr_start, "SEP"))
        month = 9;
      else if (!strcmp (ptr_start, "OCT"))
        month = 10;
      else if (!strcmp (ptr_start, "NOV"))
        month = 11;
      else if (!strcmp (ptr_start, "DEC"))
        month = 12;
      else
        return -4;

      return ymd2d (year, month, day);
    }

#ifdef BSPY_EXPORTING_PLUGIN
  std::string
  dt_tools::py_str () const
    {
      std::stringstream s;
      //s << file_name << "\n";
      return s.str ();
    }

  boost::python::list
  dt_tools::d2date (const double d) const
    {
      boost::python::list l;
      int day, month, year, hour, minute, second;
      double dd;
      dd = d2ymd (d, year, month, day);
      t2hms (dd, hour, minute, second);
      l.append (year);
      l.append (month);
      l.append (day);
      l.append (hour);
      l.append (minute);
      l.append (second);
      return l;
    }

  double
  dt_tools::date2d (int year, int month, int day, int hour, int minute, int second) const
    {
      return ymd2d (year, month, day) + hms2t (hour, minute, second);
    }

  std::string
  dt_tools::d2str (double d) const
    {
      char b[1024];
      d2cstr (d, b);
      return std::string (b);
    }
  std::string
  dt_tools::t2str (double t) const
    {
      char b[1024];
      t2cstr (t, b);
      return std::string (b);
    }
#endif //BSPY_EXPORTING_PLUGIN
/////////////////////////////////BS Register
/////////////////////////////////Stuff//////////////////////////

  BLUE_SKY_TYPE_STD_CREATE (dt_tools);
  BLUE_SKY_TYPE_STD_COPY (dt_tools);

  BLUE_SKY_TYPE_IMPL(dt_tools,  dt_tools_iface, "dt_tools", "Date time tools", "Date time tools");

}  // blue_sky namespace


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

#ifndef BOOST_ASTRONOMY_COORDINATE_CARTESIAN_DIFFERENTIAL_HPP
#define BOOST_ASTRONOMY_COORDINATE_CARTESIAN_DIFFERENTIAL_HPP

#include <tuple>
#include <type_traits>

#include <boost/geometry/strategies/strategy_transform.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/static_assert.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/units/systems/si/dimensionless.hpp>

#include <boost/astronomy/detail/is_base_template_of.hpp>
#include <boost/astronomy/coordinate/base_differential.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

//!Represents differential of cartesian representation
//!Uses three coordinate to represent a differential (dx, dy, dz)
template
<
    typename CoordinateType = double,
    typename XQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>,
    typename YQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>,
    typename ZQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>
>
struct cartesian_differential: base_differential<3, bg::cs::cartesian, CoordinateType>
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG(
        ((std::is_same<typename bu::get_dimension<XQuantity>::type,
        typename bu::get_dimension<YQuantity>::type>::value) &&
        (std::is_same<typename bu::get_dimension<YQuantity>::type,
        typename bu::get_dimension<ZQuantity>::type>::value)),
        "All components must have same dimensions");
    ///@endcond

public:

    typedef XQuantity quantity1;
    typedef YQuantity quantity2;
    typedef ZQuantity quantity3;

    // default constructor no initialization
    cartesian_differential() {}

    //!constructs object from provided components of differential
    cartesian_differential(XQuantity const& dx, YQuantity const& dy, ZQuantity const& dz)
    {
        this->set_dx_dy_dz(dx, dy, dz);
    }

    //!constructs object from boost::geometry::model::point object
    template
    <
        std::size_t OtherDimensionCount,
        typename OtherCoordinateSystem,
        typename OtherCoordinateType
    >
    cartesian_differential
    (
        bg::model::point
        <
            OtherCoordinateType,
            OtherDimensionCount,
            OtherCoordinateSystem
        > const& pointObject
    )
    {
        bg::transform(pointObject, this->diff);
    }

    //!copy constructor
    cartesian_differential
    (
        cartesian_differential
        <
            CoordinateType,
            XQuantity,
            YQuantity,
            ZQuantity
        > const& object
    )
    {
        this->diff = object.get_differential();
    }

    //!constructs object from any type of differential
    template <typename Differential>
    cartesian_differential(Differential const& other)
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_differential, Differential>::value),
            "No constructor found with given argument type");

        bg::transform(other.get_differential(), this->diff);
    }

    //! returns the (dx, dy, dz) in the form of tuple
    std::tuple<XQuantity, YQuantity, ZQuantity> get_dx_dy_dz() const
    {
        return std::make_tuple(this->get_dx(), this->get_dy(), this->get_dz());
    }

    //!returns the dx component of differential
    XQuantity get_dx() const
    {
        return XQuantity::from_value(bg::get<0>(this->diff));
    }

    //!returns the dy component of differential
    YQuantity get_dy() const
    {
        return YQuantity::from_value(bg::get<1>(this->diff));
    }
    //!returns the dz component of differential
    ZQuantity get_dz() const
    {
        return ZQuantity::from_value(bg::get<2>(this->diff));
    }

    //!set value of (dx, dy, dz) in current object
    void set_dx_dy_dz(XQuantity const& dx,  YQuantity const& dy, ZQuantity const& dz)
    {
        this->set_dx(dx);
        this->set_dy(dy);
        this->set_dz(dz);
    }

    //!set value of dx component of differential
    void set_dx(XQuantity const& dx)
    {
        bg::set<0>(this->diff, dx.value());
    }

    //!set value of dy component of differential
    void set_dy(YQuantity const& dy)
    {
        bg::set<1>(this->diff, dy.value());
    }

    //!set value of dz component of differential
    void set_dz(ZQuantity const& dz)
    {
        bg::set<2>(this->diff, dz.value());
    }

    //!operator for addition of differential
    template
    <
        typename Addend
    >
    cartesian_differential
    <
        CoordinateType,
        XQuantity,
        YQuantity,
        ZQuantity
    >
    operator +(Addend const& addend) const
    {
        bg::model::point
        <
            typename std::conditional
            <
                sizeof(typename Addend::type) >= sizeof(CoordinateType),
                typename Addend::type,
                CoordinateType
            >::type,
            3,
            bg::cs::cartesian
        > result;

        auto cartesian = make_cartesian_differential(addend);

	    //performing calculation to find the sum
	    bg::set<0>(result, (this->get_dx().value() +
	        static_cast<quantity1>(cartesian.get_dx()).value()));
	    bg::set<1>(result, (this->get_dy().value() +
	        static_cast<quantity2>(cartesian.get_dy()).value()));
	    bg::set<2>(result, (this->get_dz().value() +
	        static_cast<quantity3>(cartesian.get_dz()).value()));

	    return cartesian_differential
	        <
	            CoordinateType,
	            XQuantity,
	            YQuantity,
	            ZQuantity
	        >(result);
    }

    //!operator for multiplication of differential
    template
    <
        typename OtherQuantity
    >
    auto operator *(OtherQuantity const& dt) const
    {

    	cartesian_differential<CoordinateType, XQuantity, YQuantity, ZQuantity>
            temp(this->diff);

        cartesian_differential
        <
        	CoordinateType,
        	bu::quantity<typename bu::multiply_typeof_helper
            <
                typename XQuantity::unit_type,
                typename OtherQuantity::unit_type>::type
			>,
            bu::quantity<typename bu::multiply_typeof_helper
            <
                typename YQuantity::unit_type,
                typename OtherQuantity::unit_type>::type
            >,
            bu::quantity<typename bu::multiply_typeof_helper
            <
                typename ZQuantity::unit_type,
                typename OtherQuantity::unit_type>::type
            >
        > product
        (
        	temp.get_dx() * dt,
        	temp.get_dy() * dt,
        	temp.get_dz() * dt
        );

        return product;
    }

}; //cartesian_differential

//!constructs object from provided components of differential
template
<
    typename CoordinateType,
    template <typename Unit1, typename CoordinateType_> class XQuantity,
    template <typename Unit2, typename CoordinateType_> class YQuantity,
    template <typename Unit3, typename CoordinateType_> class ZQuantity,
    typename Unit1,
    typename Unit2,
    typename Unit3
>
cartesian_differential
<
    CoordinateType,
    XQuantity<Unit1, CoordinateType>,
    YQuantity<Unit2, CoordinateType>,
    ZQuantity<Unit3, CoordinateType>
>
make_cartesian_differential
(
    XQuantity<Unit1, CoordinateType> const& dx,
    YQuantity<Unit2, CoordinateType> const& dy,
    ZQuantity<Unit3, CoordinateType> const& dz
)
{
    return cartesian_differential
        <
            CoordinateType,
            XQuantity<Unit1, CoordinateType>,
            YQuantity<Unit2, CoordinateType>,
            ZQuantity<Unit3, CoordinateType>
        >(dx, dy, dz);
}

//!constructs object from provided components of differential with different units
template
<
    typename ReturnCoordinateType,
    typename ReturnXQuantity,
    typename ReturnYQuantity,
    typename ReturnZQuantity,
    typename CoordinateType,
    typename XQuantity,
    typename YQuantity,
    typename ZQuantity
>
cartesian_differential
<
    ReturnCoordinateType,
    ReturnXQuantity,
    ReturnYQuantity,
    ReturnZQuantity
>
make_cartesian_differential
(
	cartesian_differential
	<
		CoordinateType,
		XQuantity,
		YQuantity,
		ZQuantity
	> const& other
)
{
    return make_cartesian_differential(
        static_cast<ReturnXQuantity>(other.get_dx()),
        static_cast<ReturnYQuantity>(other.get_dy()),
        static_cast<ReturnZQuantity>(other.get_dz())
    );
}

//!constructs object from provided differential
template
<
    typename CoordinateType,
    typename XQuantity,
    typename YQuantity,
    typename ZQuantity
>
cartesian_differential
<
	CoordinateType,
	XQuantity,
	YQuantity,
	ZQuantity
>
make_cartesian_differential
(
	cartesian_differential
	<
		CoordinateType,
		XQuantity,
		YQuantity,
		ZQuantity
	> const& other
)
{
    return cartesian_differential
        <
            CoordinateType,
            XQuantity,
            YQuantity,
            ZQuantity
        >(other);
}

 //!constructs object from boost::geometry::model::point object
template
<
    typename CoordinateType = double,
    typename XQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>,
    typename YQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>,
    typename ZQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>,
    std::size_t OtherDimensionCount,
    typename OtherCoordinateSystem,
    typename OtherCoordinateType
>
cartesian_differential
<
	CoordinateType,
	XQuantity,
	YQuantity,
	ZQuantity
>
make_cartesian_differential
(
    bg::model::point
    <
    	OtherCoordinateType,
    	OtherDimensionCount,
    	OtherCoordinateSystem
    > const& pointObject
)
{
    return cartesian_differential
        <
            CoordinateType,
            XQuantity,
            YQuantity,
            ZQuantity
        >(pointObject);
}

 //!constructs object from any type of differential
template <typename OtherDifferential>
cartesian_differential
<
    typename OtherDifferential::type,
    typename OtherDifferential::quantity3,
    typename OtherDifferential::quantity3,
    typename OtherDifferential::quantity3
>
make_cartesian_differential(OtherDifferential const& other)
{
    bg::model::point<typename OtherDifferential::type, 3, bg::cs::cartesian> result;
    bg::transform(other.get_differential(), result);

    return cartesian_differential
        <
        	typename OtherDifferential::type,
        	typename OtherDifferential::quantity3,
        	typename OtherDifferential::quantity3,
        	typename OtherDifferential::quantity3
        >(result);
}
}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_CARTESIAN_DIFFERENTIAL_HPP