//
// Copyright (c) 2020 INRIA
//

#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/frames-derivatives.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME);
  FrameIndex frameId = model.addFrame(frame);

  BOOST_CHECK(model.getFrameId("rand") == frameId);
  BOOST_CHECK(model.frames[frameId].parent == jointId);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  computeForwardKinematicsDerivatives(model,data,q,v,a);

  Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero();
  Data::Matrix6x partial_dq_local_world_aligned(6,model.nv); partial_dq_local_world_aligned.setZero();
  Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero();
  Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero();
  Data::Matrix6x partial_dv_local_world_aligned(6,model.nv); partial_dv_local_world_aligned.setZero();
  Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero();

  getFrameVelocityDerivatives(model,data,frameId,WORLD,
                              partial_dq,partial_dv);

  getFrameVelocityDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
                              partial_dq_local_world_aligned,partial_dv_local_world_aligned);

  getFrameVelocityDerivatives(model,data,frameId,LOCAL,
                              partial_dq_local,partial_dv_local);

  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
  computeJointJacobians(model,data_ref,q);
  getFrameJacobian(model,data_ref,frameId,WORLD,J_ref);
  getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
  getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local);

  BOOST_CHECK(data_ref.oMf[frameId].isApprox(data.oMf[frameId]));
  BOOST_CHECK(partial_dv.isApprox(J_ref));
  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned));
  BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));

  // Check against finite differences
  Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero();
  Data::Matrix6x partial_dq_fd_local_world_aligned(6,model.nv); partial_dq_fd_local_world_aligned.setZero();
  Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero();
  Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero();
  Data::Matrix6x partial_dv_fd_local_world_aligned(6,model.nv); partial_dv_fd_local_world_aligned.setZero();
  Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero();
  const double alpha = 1e-8;

  // dvel/dv
  Eigen::VectorXd v_plus(v);
  Data data_plus(model);
  forwardKinematics(model,data_ref,q,v);
  Motion v0 = getFrameVelocity(model,data,frameId,WORLD);
  Motion v0_local_world_aligned = getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED);
  Motion v0_local = getFrameVelocity(model,data,frameId,LOCAL);
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    forwardKinematics(model,data_plus,q,v_plus);

    partial_dv_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha;
    partial_dv_fd_local_world_aligned.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - v0_local_world_aligned).toVector()/alpha;
    partial_dv_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha;
    v_plus[k] -= alpha;
  }

  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha)));
  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned,sqrt(alpha)));
  BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha)));

  // dvel/dq
  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
  forwardKinematics(model,data_ref,q,v);
  updateFramePlacements(model,data_ref);

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    forwardKinematics(model,data_plus,q_plus,v);
    updateFramePlacements(model,data_plus);

    Motion v_plus_local_world_aligned = getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
    SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
    v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
    partial_dq_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha;
    partial_dq_fd_local_world_aligned.col(k) = (v_plus_local_world_aligned - v0_local_world_aligned).toVector()/alpha;
    partial_dq_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha;
    v_eps[k] -= alpha;
  }

  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha)));
  BOOST_CHECK(partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned,sqrt(alpha)));
  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));

}

BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME);
  FrameIndex frameId = model.addFrame(frame);

  BOOST_CHECK(model.getFrameId("rand") == frameId);
  BOOST_CHECK(model.frames[frameId].parent == jointId);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  computeForwardKinematicsDerivatives(model,data,q,v,a);

  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
  Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero();
  Data::Matrix6x v_partial_dq_local_world_aligned(6,model.nv); v_partial_dq_local_world_aligned.setZero();
  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
  Data::Matrix6x a_partial_dq_local_world_aligned(6,model.nv); a_partial_dq_local_world_aligned.setZero();
  Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero();
  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
  Data::Matrix6x a_partial_dv_local_world_aligned(6,model.nv); a_partial_dv_local_world_aligned.setZero();
  Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero();
  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
  Data::Matrix6x a_partial_da_local_world_aligned(6,model.nv); a_partial_da_local_world_aligned.setZero();
  Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero();

  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
                                  v_partial_dq,
                                  a_partial_dq,a_partial_dv,a_partial_da);

  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
                                  v_partial_dq_local_world_aligned,
                                  a_partial_dq_local_world_aligned,
                                  a_partial_dv_local_world_aligned,
                                  a_partial_da_local_world_aligned);

  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
                                  v_partial_dq_local,
                                  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);

  // Check v_partial_dq against getFrameVelocityDerivatives
  {
    Data data_v(model);
    computeForwardKinematicsDerivatives(model,data_v,q,v,a);

    Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
    Data::Matrix6x v_partial_dq_ref_local_world_aligned(6,model.nv); v_partial_dq_ref_local_world_aligned.setZero();
    Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero();
    Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
    Data::Matrix6x v_partial_dv_ref_local_world_aligned(6,model.nv); v_partial_dv_ref_local_world_aligned.setZero();
    Data::Matrix6x v_partial_dv_ref_local(6,model.nv); v_partial_dv_ref_local.setZero();

    getFrameVelocityDerivatives(model,data_v,frameId,WORLD,
                                v_partial_dq_ref,v_partial_dv_ref);

    BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
    BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref));

    getFrameVelocityDerivatives(model,data_v,frameId,LOCAL_WORLD_ALIGNED,
                                v_partial_dq_ref_local_world_aligned,v_partial_dv_ref_local_world_aligned);

    BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
    BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));

    getFrameVelocityDerivatives(model,data_v,frameId,LOCAL,
                                v_partial_dq_ref_local,v_partial_dv_ref_local);

    BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
    BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local));
  }

  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
  computeJointJacobians(model,data_ref,q);
  getFrameJacobian(model,data_ref,frameId,WORLD,J_ref);
  getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
  getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local);

  BOOST_CHECK(a_partial_da.isApprox(J_ref));
  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
  BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));

  // Check against finite differences
  Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero();
  Data::Matrix6x a_partial_da_fd_local_world_aligned(6,model.nv); a_partial_da_fd_local_world_aligned.setZero();
  Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero();
  const double alpha = 1e-8;

  Eigen::VectorXd v_plus(v), a_plus(a);
  Data data_plus(model);
  forwardKinematics(model,data_ref,q,v,a);

  // dacc/da
  Motion a0 = getFrameAcceleration(model,data,frameId,WORLD);
  Motion a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
  Motion a0_local = getFrameAcceleration(model,data,frameId,LOCAL);
  for(int k = 0; k < model.nv; ++k)
  {
    a_plus[k] += alpha;
    forwardKinematics(model,data_plus,q,v,a_plus);

    a_partial_da_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
    a_partial_da_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
    a_partial_da_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
    a_plus[k] -= alpha;
  }
  BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha)));
  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned,sqrt(alpha)));
  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));

  // dacc/dv
  Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero();
  Data::Matrix6x a_partial_dv_fd_local_world_aligned(6,model.nv); a_partial_dv_fd_local_world_aligned.setZero();
  Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero();
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    forwardKinematics(model,data_plus,q,v_plus,a);

    a_partial_dv_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;    a_partial_dv_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
    a_partial_dv_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
    v_plus[k] -= alpha;
  }

  BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha)));
  BOOST_CHECK(a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned,sqrt(alpha)));
  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));

  // dacc/dq
  a_partial_dq.setZero();
  a_partial_dv.setZero();
  a_partial_da.setZero();

  a_partial_dq_local_world_aligned.setZero();
  a_partial_dv_local_world_aligned.setZero();
  a_partial_da_local_world_aligned.setZero();

  a_partial_dq_local.setZero();
  a_partial_dv_local.setZero();
  a_partial_da_local.setZero();

  Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
  Data::Matrix6x a_partial_dq_fd_local_world_aligned(6,model.nv); a_partial_dq_fd_local_world_aligned.setZero();
  Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero();

  computeForwardKinematicsDerivatives(model,data,q,v,a);
  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
                                  v_partial_dq,
                                  a_partial_dq,a_partial_dv,a_partial_da);

  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
                                  v_partial_dq_local_world_aligned,
                                  a_partial_dq_local_world_aligned,
                                  a_partial_dv_local_world_aligned,
                                  a_partial_da_local_world_aligned);

  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
                                  v_partial_dq_local,
                                  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);

  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
  forwardKinematics(model,data_ref,q,v,a);
  updateFramePlacements(model,data_ref);
  a0 = getFrameAcceleration(model,data,frameId,WORLD);
  a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
  a0_local = getFrameAcceleration(model,data,frameId,LOCAL);

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    forwardKinematics(model,data_plus,q_plus,v,a);
    updateFramePlacements(model,data_plus);

    a_partial_dq_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
    Motion a_plus_local_world_aligned = getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
    const SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
    a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
    a_partial_dq_fd_local_world_aligned.col(k) = (a_plus_local_world_aligned - a0_local_world_aligned).toVector()/alpha;
    a_partial_dq_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
    v_eps[k] -= alpha;
  }

  BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha)));
  BOOST_CHECK(a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned,sqrt(alpha)));
  BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha)));

  // Test other signatures
  Data::Matrix6x v_partial_dq_other(6,model.nv); v_partial_dq_other.setZero();
  Data::Matrix6x v_partial_dq_local_other(6,model.nv); v_partial_dq_local_other.setZero();
  Data::Matrix6x v_partial_dq_local_world_aligned_other(6,model.nv); v_partial_dq_local_world_aligned_other.setZero();
  Data::Matrix6x v_partial_dv_other(6,model.nv); v_partial_dv_other.setZero();
  Data::Matrix6x v_partial_dv_local_other(6,model.nv); v_partial_dv_local_other.setZero();
  Data::Matrix6x v_partial_dv_local_world_aligned_other(6,model.nv); v_partial_dv_local_world_aligned_other.setZero();
  Data::Matrix6x a_partial_dq_other(6,model.nv); a_partial_dq_other.setZero();
  Data::Matrix6x a_partial_dq_local_world_aligned_other(6,model.nv); a_partial_dq_local_world_aligned_other.setZero();
  Data::Matrix6x a_partial_dq_local_other(6,model.nv); a_partial_dq_local_other.setZero();
  Data::Matrix6x a_partial_dv_other(6,model.nv); a_partial_dv_other.setZero();
  Data::Matrix6x a_partial_dv_local_world_aligned_other(6,model.nv); a_partial_dv_local_world_aligned_other.setZero();
  Data::Matrix6x a_partial_dv_local_other(6,model.nv); a_partial_dv_local_other.setZero();
  Data::Matrix6x a_partial_da_other(6,model.nv); a_partial_da_other.setZero();
  Data::Matrix6x a_partial_da_local_world_aligned_other(6,model.nv); a_partial_da_local_world_aligned_other.setZero();
  Data::Matrix6x a_partial_da_local_other(6,model.nv); a_partial_da_local_other.setZero();

  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
                                  v_partial_dq_other,
                                  v_partial_dv_other,
                                  a_partial_dq_other,
                                  a_partial_dv_other,
                                  a_partial_da_other);

  BOOST_CHECK(v_partial_dq_other.isApprox(v_partial_dq));
  BOOST_CHECK(v_partial_dv_other.isApprox(a_partial_da));
  BOOST_CHECK(a_partial_dq_other.isApprox(a_partial_dq));
  BOOST_CHECK(a_partial_dv_other.isApprox(a_partial_dv));
  BOOST_CHECK(a_partial_da_other.isApprox(a_partial_da));

  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
                                  v_partial_dq_local_world_aligned_other,
                                  v_partial_dv_local_world_aligned_other,
                                  a_partial_dq_local_world_aligned_other,
                                  a_partial_dv_local_world_aligned_other,
                                  a_partial_da_local_world_aligned_other);

  BOOST_CHECK(v_partial_dq_local_world_aligned_other.isApprox(v_partial_dq_local_world_aligned));
  BOOST_CHECK(v_partial_dv_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
  BOOST_CHECK(a_partial_dq_local_world_aligned_other.isApprox(a_partial_dq_local_world_aligned));
  BOOST_CHECK(a_partial_dv_local_world_aligned_other.isApprox(a_partial_dv_local_world_aligned));
  BOOST_CHECK(a_partial_da_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));

  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
                                  v_partial_dq_local_other,
                                  v_partial_dv_local_other,
                                  a_partial_dq_local_other,
                                  a_partial_dv_local_other,
                                  a_partial_da_local_other);

  BOOST_CHECK(v_partial_dq_local_other.isApprox(v_partial_dq_local));
  BOOST_CHECK(v_partial_dv_local_other.isApprox(a_partial_da_local));
  BOOST_CHECK(a_partial_dq_local_other.isApprox(a_partial_dq_local));
  BOOST_CHECK(a_partial_dv_local_other.isApprox(a_partial_dv_local));
  BOOST_CHECK(a_partial_da_local_other.isApprox(a_partial_da_local));
}

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/prim.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>
#include <vector>

TEST(ProbDistributionsPoissonBinomial, lpmf_works_on_scalar_arguments) {
  using stan::math::poisson_binomial_lpmf;
  using vec = Eigen::Matrix<double, Eigen::Dynamic, 1>;

  vec p(3, 1);
  p << 0.5, 0.2, 0.7;

  EXPECT_NEAR(-2.12026, poisson_binomial_lpmf(0, p), 0.001);
  EXPECT_NEAR(-0.84397, poisson_binomial_lpmf(1, p), 0.001);
  EXPECT_NEAR(-0.967584, poisson_binomial_lpmf(2, p), 0.001);
  EXPECT_NEAR(-2.65926, poisson_binomial_lpmf(3, p), 0.001);
}

TEST(ProbDistributionsPoissonBinomial, lpmf_works_on_vectorial_y) {
  using stan::math::poisson_binomial_lpmf;
  using vec = Eigen::Matrix<double, Eigen::Dynamic, 1>;

  vec p(3, 1);
  p << 0.5, 0.2, 0.7;
  std::vector<int> y{2, 2};

  EXPECT_NEAR(-0.967584 * 2, poisson_binomial_lpmf(y, p), 0.001);
}

TEST(ProbDistributionsPoissonBinomial, lpmf_works_on_vectorial_y_and_theta) {
  using stan::math::poisson_binomial_lpmf;
  using vec = Eigen::Matrix<double, Eigen::Dynamic, 1>;

  vec p(3, 1);
  p << 0.5, 0.2, 0.7;
  std::vector<int> y{2, 0};
  std::vector<vec> ps{p, p};

  EXPECT_NEAR(-0.967584 - 2.12026, poisson_binomial_lpmf(y, ps), 0.001);
}

TEST(ProbDistributionsPoissonBinomial, lpmf_check_error_scalar_y_oob) {
  static double inff = std::numeric_limits<double>::infinity();

  Eigen::Matrix<double, Eigen::Dynamic, 1> theta(3);
  theta << 0.5, 0.2, 0.7;

  EXPECT_THROW(stan::math::poisson_binomial_lpmf(-1, theta), std::domain_error);
  EXPECT_THROW(stan::math::poisson_binomial_lpmf(4, theta), std::domain_error);
}

TEST(ProbDistributionsPoissonBinomial, lpmf_check_error_theta_is_not_prob) {
  static double inff = std::numeric_limits<double>::infinity();

  Eigen::Matrix<double, Eigen::Dynamic, 1> theta(3);
  theta << 0.5, 0.2, 1.1;
  Eigen::Matrix<double, Eigen::Dynamic, 1> theta2(3);
  theta2 << 0.5, 0.2, inff;

  EXPECT_THROW(stan::math::poisson_binomial_lpmf(1, theta), std::domain_error);
  EXPECT_THROW(stan::math::poisson_binomial_lpmf(1, theta2), std::domain_error);
}

TEST(ProbDistributionsPoissonBinomial, lpmf_check_error_vectorial_y_oob) {
  static double inff = std::numeric_limits<double>::infinity();

  Eigen::Matrix<double, Eigen::Dynamic, 1> theta(3);
  theta << 0.5, 0.2, 0.1;
  std::vector<int> ys1{-1, 2};
  std::vector<int> ys2{4, 3};

  EXPECT_THROW(stan::math::poisson_binomial_lpmf(ys1, theta),
               std::domain_error);
  EXPECT_THROW(stan::math::poisson_binomial_lpmf(ys2, theta),
               std::domain_error);
}

TEST(ProbDistributionsPoissonBinomial,
     lpmf_check_error_vectorial_y_theta_is_not_prob) {
  static double inff = std::numeric_limits<double>::infinity();

  Eigen::Matrix<double, Eigen::Dynamic, 1> theta(3);
  theta << inff, 0.2, 0.1;
  std::vector<int> y{0, 2};

  EXPECT_THROW(stan::math::poisson_binomial_lpmf(y, theta), std::domain_error);
}

TEST(ProbDistributionsPoissonBinomial,
     lpmf_check_error_vectorial_theta_is_not_prob) {
  using vec = Eigen::Matrix<double, Eigen::Dynamic, 1>;
  static double inff = std::numeric_limits<double>::infinity();

  vec theta1(3);
  theta1 << -0.1, 0.2, 0.1;
  vec theta2(3);
  theta2 << 0.5, 0.2, 1.1;
  vec theta3(3);
  theta3 << 0.5, 0.2, inff;

  std::vector<int> ys{0, 2, 3};
  std::vector<vec> thetas{theta1, theta2, theta3};

  EXPECT_THROW(stan::math::poisson_binomial_lpmf(ys, thetas),
               std::domain_error);
}

TEST(ProbDistributionsPoissonBinomial,
     lpmf_check_error_vectorial_y_oob_with_vectorial_theta) {
  using mat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
  using vec = Eigen::Matrix<double, Eigen::Dynamic, 1>;

  vec theta1(3);
  theta1 << 0.5, 0.2, 0.1;
  vec theta2(3);
  theta2 << 0.5, 0.2, 0.1;

  std::vector<int> ys{-1, 4};
  std::vector<vec> thetas{theta1, theta2};

  EXPECT_THROW(stan::math::poisson_binomial_lpmf(ys, thetas),
               std::domain_error);
}

TEST(ProbDistributionsPoissonBinomial,
     lpmf_check_error_vectorial_y_and_theta_inconsistent_sizes) {
  using mat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
  using vec = Eigen::Matrix<double, Eigen::Dynamic, 1>;

  vec theta(3);
  theta << 0.5, 0.2, 0.1;

  std::vector<int> ys{0, 1};
  std::vector<vec> thetas{theta, theta, theta};

  EXPECT_THROW(stan::math::poisson_binomial_lpmf(ys, thetas),
               std::invalid_argument);
}

/*
 * Since there is no easy way to compute Poisson binomial quantiles with Boost,
 * test the distribution by emulating a binomial, i.e., by setting all
 * success probabilities to the same value
 */
TEST(ProbDistributionsPoissonBinomial, chiSquareGoodnessFitTest) {
  using mat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
  using vec = Eigen::Matrix<double, Eigen::Dynamic, 1>;

  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::binomial_distribution<> dist(100, 0.6);
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++) {
    loc[i - 1] = i - 1;
  }

  int count = 0;
  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 2));

  vec probs(100);
  probs.fill(0.6);
  while (count < N) {
    int a = stan::math::poisson_binomial_rng(probs, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i]) {
      ++i;
    }
    ++bin[i];
    count++;
  }

  double chi = 0;
  for (int j = 0; j < K; j++) {
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);
  }

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


#include <bits/stdint-uintn.h>
#include <gmp.h>
#include <gmpxx.h>

#include <boost/multiprecision/cpp_int.hpp>
#include <cassert>
#include <cuhe++.hpp>
#include <iostream>
#include <random>

using namespace std;
using namespace cuHEpp;
namespace mp = boost::multiprecision;

int main()
{
    constexpr int numTest = 100000;
    random_device seed_gen;
    default_random_engine engine(seed_gen());
    uniform_int_distribution<uint64_t> dist(0, P);

    // Add Test
    for (int i = 0; i < numTest; i++) {
        __uint128_t a = dist(engine);
        __uint128_t b = dist(engine);
        INTorus A(a);
        INTorus B(b);
        assert((A + B).value == ((a + b) % P));
    }
    cout << "Add PASS" << endl;

    // Add and assignment Test
    for (int i = 0; i < numTest; i++) {
        __uint128_t a = dist(engine);
        __uint128_t b = dist(engine);
        INTorus A(a);
        INTorus B(b);
        A += B;
        assert(A.value == ((a + b) % P));
    }
    cout << "Add and assignment PASS" << endl;

    // Sub Test
    for (int i = 0; i < numTest; i++) {
        __uint128_t a = dist(engine);
        __uint128_t b = dist(engine);
        INTorus A(a);
        INTorus B(b);
        assert((A - B).value == ((a + (P - b)) % P));
    }
    cout << "Sub PASS" << endl;

    // Sub and assignment Test
    for (int i = 0; i < numTest; i++) {
        __uint128_t a = dist(engine);
        __uint128_t b = dist(engine);
        INTorus A(a);
        INTorus B(b);
        A -= B;
        // cout<<A.value<<":"<<static_cast<uint64_t>((a+(P-b))%P)<<endl;
        assert(A.value == ((a + (P - b)) % P));
    }
    cout << "Sub and assignment PASS" << endl;

    // Mul Test
    for (int i = 0; i < numTest; i++) {
        __uint128_t a = dist(engine);
        __uint128_t b = dist(engine);
        INTorus A(a);
        INTorus B(b);
        // cout<<(A*B).value<<":"<<static_cast<uint64_t>((a*b)%P)<<endl;
        assert((A * B).value == ((a * b) % P));
    }
    cout << "Mul PASS" << endl;

    // Mul and assignment Test
    for (int i = 0; i < numTest; i++) {
        __uint128_t a = dist(engine);
        __uint128_t b = dist(engine);
        INTorus A(a);
        INTorus B(b);
        A *= B;
        // cout<<A.value<<":"<<static_cast<uint64_t>((a*b)%P)<<endl;
        assert(A.value == ((a * b) % P));
    }
    cout << "Mul and assignment PASS" << endl;

#ifdef ENABLE_LSHTEST
    // Lsh Test
    for (int upper = 1; upper <= 6; upper++) {
        for (int l = 32 * (upper - 1); l < 32 * upper; l++) {
            // cout<<l<<endl;
            for (int i = 0; i < numTest; i++) {
                uint64_t temp = dist(engine);
                INTorus A(temp);
                // mp::cpp_int a = temp;
                mpz_class a = temp;
                // cout<<static_cast<uint64_t>(a)<<":";
                // if((A<<l).value!=(static_cast<uint64_t>((a<<l)%P))){
                if ((A << l).value != (a << l) % P) {
                    mpz_class res = (a << l) % P;
                    cout << (A << l).value << ":" << res.get_str() << endl;
                    std::cout << l << std::endl;
                    // cout<<(A<<l).value -
                    // static_cast<uint64_t>((a<<l)%P)<<endl;
                }
                // assert((A<<l).value==(static_cast<uint64_t>((a<<l)%P)));
                assert((A << l).value == (a << l) % P);
            }
            __uint128_t temp = ((1UL << 32) - 1) << 32;
            INTorus A(temp);
            mp::cpp_int a = temp;
            if ((A << l).value != (static_cast<uint64_t>((a << l) % P))) {
                cout << (A << l).value << ":"
                     << static_cast<uint64_t>((a << l) % P) << endl;
                cout << "Here" << endl;
            }
            assert((A << l).value == (static_cast<uint64_t>((a << l) % P)));
        }
        cout << "Lsh" << upper * 32 << " PASS" << endl;
    }
#endif

    // InvPow2 Test
    for (int i = 1; i <= 31; i++) {
        assert((INTorus(1U << i, false) * cuHEpp::InvPow2(i)).value == 1);
    }
    cout << "InvPow2 PASS" << endl;
}


#include <NTL/xdouble.h>
#include <NTL/RR.h>

NTL_START_IMPL

NTL_CHEAP_THREAD_LOCAL
long xdouble::oprec = 10;

void xdouble::SetOutputPrecision(long p)
{
   if (p < 1) p = 1;

   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("xdouble: output precision too big");

   oprec = p;
}

void xdouble::normalize()
{
   if (x == 0)
      e = 0;
   else if (x > 0) {
      while (x < NTL_XD_HBOUND_INV) { x *= NTL_XD_BOUND; e--; }
      while (x > NTL_XD_HBOUND) { x *= NTL_XD_BOUND_INV; e++; }
   }
   else {
      while (x > -NTL_XD_HBOUND_INV) { x *= NTL_XD_BOUND; e--; }
      while (x < -NTL_XD_HBOUND) { x *= NTL_XD_BOUND_INV; e++; }
   }

   if (e >= NTL_OVFBND)
      ResourceError("xdouble: overflow");

   if (e <= -NTL_OVFBND)
      ResourceError("xdouble: underflow");
}

xdouble to_xdouble(double a)
{
   if (a == 0 || a == 1 || (a > 0 && a >= NTL_XD_HBOUND_INV && a <= NTL_XD_HBOUND)
       || (a < 0 && a <= -NTL_XD_HBOUND_INV && a >= -NTL_XD_HBOUND)) {

      return xdouble(a, 0);

   }

   if (!IsFinite(&a))
      ArithmeticError("double to xdouble conversion: non finite value");

   xdouble z = xdouble(a, 0);
   z.normalize();
   return z;
}

void conv(double& xx, const xdouble& a)
{
   double x;
   long e;

   x = a.x;
   e = a.e;

   while (e > 0) { x *= NTL_XD_BOUND; e--; }
   while (e < 0) { x *= NTL_XD_BOUND_INV; e++; }

   xx = x;
}

xdouble operator+(const xdouble& a, const xdouble& b)
{
   xdouble z;

   if (a.x == 0)
      return b;

   if (b.x == 0)
     return a;

   if (a.e == b.e) {
      z.x = a.x + b.x;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else if (a.e > b.e) {
      if (a.e > b.e+1)
         return a;

      z.x = a.x + b.x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else {
      if (b.e > a.e+1)
         return b;

      z.x = a.x*NTL_XD_BOUND_INV + b.x;
      z.e = b.e;
      z.normalize();
      return z;
   }
}

xdouble operator-(const xdouble& a, const xdouble& b)
{
   xdouble z;

   if (a.x == 0)
      return -b;

   if (b.x == 0)
      return a;

   if (a.e == b.e) {
      z.x = a.x - b.x;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else if (a.e > b.e) {
      if (a.e > b.e+1)
         return a;

      z.x = a.x - b.x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return z;
   }
   else {
      if (b.e > a.e+1)
         return -b;

      z.x = a.x*NTL_XD_BOUND_INV - b.x;
      z.e = b.e;
      z.normalize();
      return z;
   }
}

xdouble operator-(const xdouble& a)
{
   xdouble z;
   z.x = -a.x;
   z.e = a.e;
   return z;
}

xdouble operator*(const xdouble& a, const xdouble& b)
{
   xdouble z;

   z.e = a.e + b.e;
   z.x = a.x * b.x;
   z.normalize();
   return z;
}

xdouble operator/(const xdouble& a, const xdouble& b)
{
   xdouble z;

   if (b.x == 0) ArithmeticError("xdouble division by 0");

   z.e = a.e - b.e;
   z.x = a.x / b.x;
   z.normalize();
   return z;
}

long compare(const xdouble& a, const xdouble& b)
{
   xdouble z = a - b;

   if (z.x < 0)
      return -1;
   else if (z.x == 0)
      return 0;
   else
      return 1;
}

long sign(const xdouble& z)
{
   if (z.x < 0)
      return -1;
   else if (z.x == 0)
      return 0;
   else
      return 1;
}

xdouble trunc(const xdouble& a)
{
   if (a.x >= 0)
      return floor(a);
   else
      return ceil(a);
}

xdouble floor(const xdouble& aa)
{
   xdouble z;

   xdouble a = aa;
   ForceToMem(&a.x);

   if (a.e == 0) {
      z.x = floor(a.x);
      z.e = 0;
      z.normalize();
      return z;
   }
   else if (a.e > 0) {
      return a;
   }
   else {
      if (a.x < 0)
         return to_xdouble(-1);
      else
         return to_xdouble(0);
   }
}

xdouble ceil(const xdouble& aa)
{
   xdouble z;

   xdouble a = aa;
   ForceToMem(&a.x);

   if (a.e == 0) {
      z.x = ceil(a.x);
      z.e = 0;
      z.normalize();
      return z;
   }
   else if (a.e > 0) {
      return a;
   }
   else {
      if (a.x < 0)
         return to_xdouble(0);
      else
         return to_xdouble(1);
   }
}

xdouble to_xdouble(const ZZ& a)
{
   RRPush push;
   RR::SetPrecision(NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   conv(t, a);

   double x;
   conv(x, t.mantissa());

   xdouble y, z, res;

   conv(y, x);
   power2(z, t.exponent());

   res = y*z;

   return res;
}

void conv(ZZ& x, const xdouble& a)
{
   xdouble b = floor(a);

   RRPush push;
   RR::SetPrecision(NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   conv(t, b);
   conv(x, t);
}

xdouble fabs(const xdouble& a)
{
   xdouble z;

   z.e = a.e;
   z.x = fabs(a.x);
   return z;
}

xdouble sqrt(const xdouble& a)
{
   if (a == 0)
      return to_xdouble(0);

   if (a < 0)
      ArithmeticError("xdouble: sqrt of negative number");

   xdouble t;

   if (a.e & 1) {
      t.e = (a.e - 1)/2;
      t.x = sqrt(a.x * NTL_XD_BOUND);
   }
   else {
      t.e = a.e/2;
      t.x = sqrt(a.x);
   }

   t.normalize();

   return t;
}

void power(xdouble& z, const xdouble& a, const ZZ& e)
{
   xdouble b, res;

   b = a;

   res = 1;
   long n = NumBits(e);
   long i;

   for (i = n-1; i >= 0; i--) {
      res = res * res;
      if (bit(e, i))
         res = res * b;
   }

   if (sign(e) < 0)
      z = 1/res;
   else
      z = res;
}

void power(xdouble& z, const xdouble& a, long e)
{
   NTL_ZZRegister(E);
   E = e;
   power(z, a, E);
}

void power2(xdouble& z, long e)
{
   long hb = NTL_XD_HBOUND_LOG;
   long b = 2*hb;

   long q, r;

   q = e/b;
   r = e%b;

   while (r >= hb) {
      r -= b;
      q++;
   }

   while (r < -hb) {
      r += b;
      q--;
   }

   if (q >= NTL_OVFBND)
      ResourceError("xdouble: overflow");

   if (q <= -NTL_OVFBND)
      ResourceError("xdouble: underflow");

   double x = _ntl_ldexp(1.0, r);

   z.x = x;
   z.e = q;
}

void MulAdd(xdouble& z, const xdouble& a, const xdouble& b, const xdouble& c)
// z = a + b*c
{
   double x;
   long e;

   e = b.e + c.e;
   x = b.x * c.x;

   if (x == 0) {
      z = a;
      return;
   }

   if (a.x == 0) {
      z.e = e;
      z.x = x;
      z.normalize();
      return;
   }

   if (a.e == e) {
      z.x = a.x + x;
      z.e = e;
      z.normalize();
      return;
   }
   else if (a.e > e) {
      if (a.e > e+1) {
         z = a;
         return;
      }

      z.x = a.x + x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return;
   }
   else {
      if (e > a.e+1) {
         z.x = x;
         z.e = e;
         z.normalize();
         return;
      }

      z.x = a.x*NTL_XD_BOUND_INV + x;
      z.e = e;
      z.normalize();
      return;
   }
}

void MulSub(xdouble& z, const xdouble& a, const xdouble& b, const xdouble& c)
// z = a - b*c
{
   double x;
   long e;

   e = b.e + c.e;
   x = b.x * c.x;

   if (x == 0) {
      z = a;
      return;
   }

   if (a.x == 0) {
      z.e = e;
      z.x = -x;
      z.normalize();
      return;
   }

   if (a.e == e) {
      z.x = a.x - x;
      z.e = e;
      z.normalize();
      return;
   }
   else if (a.e > e) {
      if (a.e > e+1) {
         z = a;
         return;
      }

      z.x = a.x - x*NTL_XD_BOUND_INV;
      z.e = a.e;
      z.normalize();
      return;
   }
   else {
      if (e > a.e+1) {
         z.x = -x;
         z.e = e;
         z.normalize();
         return;
      }

      z.x = a.x*NTL_XD_BOUND_INV - x;
      z.e = e;
      z.normalize();
      return;
   }
}

double log(const xdouble& a)
{
   static const double LogBound = log(NTL_XD_BOUND); // GLOBAL (assumes C++11 thread-safe init)
   if (a.x <= 0) {
      ArithmeticError("log(xdouble): argument must be positive");
   }

   return log(a.x) + a.e*LogBound;
}

xdouble xexp(double x)
{
   const double LogBound = log(NTL_XD_BOUND);

   double y = x/LogBound;
   double iy = floor(y+0.5);

   if (iy >= NTL_OVFBND)
      ResourceError("xdouble: overflow");

   if (iy <= -NTL_OVFBND)
      ResourceError("xdouble: underflow");

   double fy = y - iy;

   xdouble res;
   res.e = long(iy);
   res.x = exp(fy*LogBound);
   res.normalize();
   return res;
}

/**************  input / output routines **************/

void ComputeLn2(RR&);
void ComputeLn10(RR&);

long ComputeMax10Power()
{
   RRPush push;
   RR::SetPrecision(NTL_BITS_PER_LONG);

   RR ln2, ln10;
   ComputeLn2(ln2);
   ComputeLn10(ln10);

   long k = to_long( to_RR(NTL_OVFBND/2) * ln2 / ln10 );
   return k;
}

xdouble PowerOf10(const ZZ& e)
{
   static NTL_CHEAP_THREAD_LOCAL long init = 0;
   static NTL_CHEAP_THREAD_LOCAL long k = 0;

   NTL_TLS_LOCAL(xdouble, v10k);

   if (!init) {
      k = ComputeMax10Power();
      RRPush push;
      RR::SetPrecision(NTL_DOUBLE_PRECISION);
      v10k = to_xdouble(power(to_RR(10), k));
      init = 1;
   }

   ZZ e1;
   long neg;

   if (e < 0) {
      e1 = -e;
      neg = 1;
   }
   else {
      e1 = e;
      neg = 0;
   }

   long r;
   ZZ q;

   r = DivRem(q, e1, k);

   RRPush push;
   RR::SetPrecision(NTL_DOUBLE_PRECISION);
   xdouble x1 = to_xdouble(power(to_RR(10), r));

   xdouble x2 = power(v10k, q);
   xdouble x3 = x1*x2;

   if (neg) x3 = 1/x3;

   return x3;
}

ostream& operator<<(ostream& s, const xdouble& a)
{
   if (a == 0) {
      s << "0";
      return s;
   }

   RRPush push;
   long temp_p = long(log(fabs(log(fabs(a))) + 1.0)/log(2.0)) + 10;
   RR::SetPrecision(temp_p);

   RR ln2, ln10, log_2_10;
   ComputeLn2(ln2);
   ComputeLn10(ln10);
   log_2_10 = ln10/ln2;
   ZZ log_10_a = to_ZZ(
  (to_RR(a.e)*to_RR(2*NTL_XD_HBOUND_LOG) + log(fabs(a.x))/log(2.0))/log_2_10);

   xdouble b;
   long neg;

   if (a < 0) {
      b = -a;
      neg = 1;
   }
   else {
      b = a;
      neg = 0;
   }

   ZZ k = xdouble::OutputPrecision() - log_10_a;

   xdouble c, d;

   c = PowerOf10(to_ZZ(xdouble::OutputPrecision()));
   d = PowerOf10(log_10_a);

   b = b / d;
   b = b * c;

   while (b < c) {
      b = b * 10.0;
      k++;
   }

   while (b >= c) {
      b = b / 10.0;
      k--;
   }

   b = b + 0.5;
   k = -k;

   ZZ B;
   conv(B, b);

   long bp_len = xdouble::OutputPrecision()+10;

   UniqueArray<char> bp_store;
   bp_store.SetLength(bp_len);
   char *bp = bp_store.get();

   long len, i;

   len = 0;
   do {
      if (len >= bp_len) LogicError("xdouble output: buffer overflow");
      bp[len] = IntValToChar(DivRem(B, B, 10));
      len++;
   } while (B > 0);

   for (i = 0; i < len/2; i++) {
      char tmp;
      tmp = bp[i];
      bp[i] = bp[len-1-i];
      bp[len-1-i] = tmp;
   }

   i = len-1;
   while (bp[i] == '0') i--;

   k += (len-1-i);
   len = i+1;

   bp[len] = '\0';

   if (k > 3 || k < -len - 3) {
      // use scientific notation

      if (neg) s << "-";
      s << "0." << bp << "e" << (k + len);
   }
   else {
      long kk = to_long(k);

      if (kk >= 0) {
         if (neg) s << "-";
         s << bp;
         for (i = 0; i < kk; i++)
            s << "0";
      }
      else if (kk <= -len) {
         if (neg) s << "-";
         s << "0.";
         for (i = 0; i < -len-kk; i++)
            s << "0";
         s << bp;
      }
      else {
         if (neg) s << "-";
         for (i = 0; i < len+kk; i++)
            s << bp[i];

         s << ".";

         for (i = len+kk; i < len; i++)
            s << bp[i];
      }
   }

   return s;
}

istream& operator>>(istream& s, xdouble& x)
{
   long c;
   long cval;
   long sign;
   ZZ a, b;

   if (!s) NTL_INPUT_ERROR(s, "bad xdouble input");

   c = s.peek();
   while (IsWhiteSpace(c)) {
      s.get();
      c = s.peek();
   }

   if (c == '-') {
      sign = -1;
      s.get();
      c = s.peek();
   }
   else
      sign = 1;

   long got1 = 0;
   long got_dot = 0;
   long got2 = 0;

   a = 0;
   b = 1;

   cval = CharToIntVal(c);

   if (cval >= 0 && cval <= 9) {
      got1 = 1;

      while (cval >= 0 && cval <= 9) {
         mul(a, a, 10);
         add(a, a, cval);
         s.get();
         c = s.peek();
         cval = CharToIntVal(c);
      }
   }

   if (c == '.') {
      got_dot = 1;

      s.get();
      c = s.peek();
      cval = CharToIntVal(c);

      if (cval >= 0 && cval <= 9) {
         got2 = 1;

         while (cval >= 0 && cval <= 9) {
            mul(a, a, 10);
            add(a, a, cval);
            mul(b, b, 10);
            s.get();
            c = s.peek();
            cval = CharToIntVal(c);
         }
      }
   }

   if (got_dot && !got1 && !got2)  NTL_INPUT_ERROR(s, "bad xdouble input");

   ZZ e;

   long got_e = 0;
   long e_sign;

   if (c == 'e' || c == 'E') {
      got_e = 1;

      s.get();
      c = s.peek();

      if (c == '-') {
         e_sign = -1;
         s.get();
         c = s.peek();
      }
      else if (c == '+') {
         e_sign = 1;
         s.get();
         c = s.peek();
      }
      else
         e_sign = 1;

      cval = CharToIntVal(c);

      if (cval < 0 || cval > 9) NTL_INPUT_ERROR(s, "bad xdouble input");

      e = 0;
      while (cval >= 0 && cval <= 9) {
         mul(e, e, 10);
         add(e, e, cval);
         s.get();
         c = s.peek();
         cval = CharToIntVal(c);
      }
   }

   if (!got1 && !got2 && !got_e) NTL_INPUT_ERROR(s, "bad xdouble input");

   xdouble t1, t2, v;

   if (got1 || got2) {
      conv(t1, a);
      conv(t2, b);
      v = t1/t2;
   }
   else
      v = 1;

   if (sign < 0)
      v = -v;

   if (got_e) {
      if (e_sign < 0) negate(e, e);
      t1 = PowerOf10(e);
      v = v * t1;
   }

   x = v;
   return s;
}

NTL_END_IMPL


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_IMPL_LOGS_F_LOG_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_IMPL_LOGS_F_LOG_HPP_INCLUDED

#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/if_else_zero.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/exponential/functions/scalar/impl/logs/f_kernel.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/log_2hi.hpp>
#include <nt2/include/constants/log_2lo.hpp>
#include <nt2/include/constants/log2_em1.hpp>
#include <nt2/include/constants/log10_ehi.hpp>
#include <nt2/include/constants/log10_elo.hpp>
#include <nt2/include/constants/log10_2hi.hpp>
#include <nt2/include/constants/log10_2lo.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <nt2/include/functions/simd/is_nan.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif
#ifndef BOOST_SIMD_NO_DENORMALS
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/constants/smallestposval.hpp>
#include <nt2/include/constants/twotonmb.hpp>
#include <nt2/include/constants/mlogtwo2nmb.hpp>
#include <nt2/include/constants/mlog2two2nmb.hpp>
#include <nt2/include/constants/mlog10two2nmb.hpp>
#endif

  //////////////////////////////////////////////////////////////////////////////
  // how to compute the various logarithms
  //////////////////////////////////////////////////////////////////////////////
  // The method is mainly taken from the cephes library:
  // first reduce the the data
  // a0 is supposed > 0
  // the input a0 is split into a mantissa and an exponent
  // the mantissa m is between sqrt(0.5) and sqrt(2) and the correspondint exponent is e
  // a0 = m*2^e
  // then the log? calculus is split in two parts (? being nothing: natural logarithm,  2: base 2 logarithm,  10 base ten logarithm)
  // as log?(a) = log?(2^e)+log?(m)
  // 1) computing log?(m)
  //   first put x = m-1 (so -0.29 <  x < 0.414)
  //   write log(m)   = log(1+x)   = x + x*x/2 + x*x*x*g(x)
  //   write log2(m)  = log2(1+x)  = C2*log(x)   C2 =  log(2)  the multiplication have to be taken seriously as C2 is not exact
  //   write log10(m) = log10(1+x) = C10*log(x)  C10=  log(10) the multiplication have to be taken seriously as C10 is not exact
  // then g(x) has to be approximated
  // g is ((log(1+x)/x-1)/x-1/2)/x
  // It is not a good idea to approximate directly log(1+x) instead of g,  because this will lead to bad precision around 1.
  //
  // in this approximation one can choose a best approximation rational function given by remez algorithm.
  // there exist a classical solution which is a polynomial p8 one of degree 8 that gives 0.5ulps everywhere
  // this is what is done in the kernel_t::log impl;
  // Now,  it is possible to choose a rational fraction or a polynomial of lesser degree to approximate g
  // providing faster but less accurate logs.
  // 2) computing log?(2^e)
  // see the explanations relative to each case
  // 3) finalize
  // This is simply treating invalid entries
  // 4) For denormal we use the fact that log(x) =  log?(x*y)-log?(y) and that if y is
  // the constant two2nmb if x is denormal x*y and y are not.
  //////////////////////////////////////////////////////////////////////////////

namespace nt2 { namespace details
{
  //////////////////////////////////////////////////////////////////////////////
  // math log functions
  //////////////////////////////////////////////////////////////////////////////

  template < class A0 >
  struct logarithm< A0, tag::simd_type, float>
  {
    typedef typename meta::as_logical<A0>::type                 lA0;
    typedef typename meta::as_integer<A0, signed>::type    int_type;
    typedef typename meta::scalar_of<A0>::type                  sA0;
    typedef kernel<A0, tag::simd_type, float>              kernel_t;

    static inline A0 log(const A0& a0)
    {
      A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
      A0 t = Zero<A0>();
      lA0 denormal = lt(nt2::abs(z), Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      t = if_else_zero(denormal, Mlogtwo2nmb<A0>());
#endif
      //log(2.0) in double is 6.931471805599453e-01
      //double(0.693359375f)+double(-0.00021219444f)  is  6.931471805600000e-01 at 1.0e-14 of log(2.0)
      // let us call Log_2hi 0.693359375f anf Log_2lo -0.00021219444f
      // We use thi to correct the sum where this could matter a lot
      // log(a0) = fe*Log_2hi+ (0.5f*x*x +(fe*Log_2lo+y))
      // These operations are order dependent: the parentheses do matter
      A0 x, fe, x2, y;
      kernel_t::log(z, fe, x, x2, y);
      y = nt2::fma(fe, Log_2lo<A0>(), y);
      y = nt2::fma(Mhalf<A0>(), x2, y);
#ifdef BOOST_SIMD_NO_DENORMALS
      return finalize(a0, nt2::fma(Log_2hi<A0>(), fe, x+y));
#else
      return finalize(a0, nt2::fma(Log_2hi<A0>(), fe, x+y+t));
#endif
    }

    static inline A0 log2(const A0& a0)
    {
      A0 z =  a0;
#ifndef BOOST_SIMD_NO_DENORMALS
      lA0 denormal = lt(nt2::abs(z), Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      A0 t = if_else_zero(denormal, Mlog2two2nmb<A0>());
#endif
      //here let l2em1 = log2(e)-1, the computation is done as:
      //log2(a0) = ((l2em1*x+(l2em1*(y+x*x/2)))+(y+x*x/2)))+x+fe for best results
      // once again the order is very important.
      A0 x, fe, x2, y;
      kernel_t::log(z, fe, x, x2, y);
      y =  nt2::fma(Mhalf<A0>(),x2, y);
      z = nt2::fma(x,Log2_em1<A0>(),y*Log2_em1<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
      return finalize(a0, ((z+y)+x)+fe);
#else
      return finalize(a0, ((z+y)+x)+fe+t);
#endif
    }

    static inline A0 log10(const A0& a0)
    {
      A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
      lA0 denormal = lt(nt2::abs(z), Smallestposval<A0>());
      z = if_else(denormal, z*Twotonmb<A0>(), z);
      A0 t = if_else_zero(denormal, Mlog10two2nmb<A0>());
#endif
      // here there are two multiplication:  log of fraction by log10(e) and base 2 exponent by log10(2)
      // and we have to split log10(e) and log10(2) in two parts to get extra precision when needed
      A0 x, fe, x2, y;
      kernel_t::log(z, fe, x, x2, y);
      y = nt2::amul(y, Mhalf<A0>(), x2);
      z = mul(x+y, Log10_elo<A0>());
      z = nt2::amul(z, y, Log10_ehi<A0>());
      z = nt2::amul(z, x, Log10_ehi<A0>());
      z = nt2::amul(z, fe, Log10_2hi<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
      return finalize(a0, nt2::amul(z, fe, Log10_2lo<A0>()));
#else
      return finalize(a0, nt2::amul(z+t, fe, Log10_2lo<A0>()));
#endif
    }
  private:
    static inline A0 finalize(const A0& a0, const A0& y)
    {
      typedef typename meta::as_logical<A0>::type              lA0;
    #ifdef BOOST_SIMD_NO_NANS
      lA0 test = nt2::is_ltz(a0);
    #else
      lA0 test = nt2::logical_or(nt2::is_ltz(a0), nt2::is_nan(a0));
    #endif
      A0 y1 = nt2::if_nan_else(test, y);
    #ifndef BOOST_SIMD_NO_INFINITIES
      y1 = if_else(nt2::is_equal(a0, nt2::Inf<A0>()), a0, y1);
    #endif
      return if_else(is_eqz(a0), nt2::Minf<A0>(), y1);
    }
  };
} }

#endif


#include <dlib/optimization.h>
#include <iostream>
#include "../include/optimize.h"
#include <random>
#include <algorithm>
#include <stdlib.h>
#define CHANGE_WEIGHTS
#include "omp.h"
using namespace std;
using namespace dlib;

static double sum(std::vector<double> *v){
    double s = 0;
    for (int i = 0; i < v->size(); i++){
        s += (*v)[i];
    }
    return s;
}

Optimizer::Optimizer(){
}

Optimizer::Optimizer(int n, BDD *bdd, std::string name, Parameter *param){
    srand(time(NULL));
    this->bdd = bdd;
    this->num_of_vars = n;
    this->solved_flag = 0;
    this->x = new std::vector<double>;
    this->unsat_clauses = new std::vector<int>;
    for(int i=0; i< n; i++){
        double xi = (double) std::rand();
        this->x->push_back( 1 - 2 * (xi/(double)RAND_MAX));
    }
    this->optimizer_name = name;
    this->param = param;
    this->number_of_trials_this_start = 0;
}

double Optimizer::fval_for_optimizer(const column_vector& m)
{
    std::vector<double> x;
    for (int i=0; i < this->bdd->num_of_vars; i++){
        x.push_back((double)m(i));
    }
    return this->bdd->fval(&x);
}

const column_vector Optimizer::grad_for_optimizer (const column_vector& m)
{
    std::vector<double> *grad;
    std::vector<double> x;
    for (int i=0; i < this->bdd->num_of_vars; i++){
        x.push_back((double)m(i));
    }
    column_vector res(this->bdd->num_of_vars);
    grad = this->bdd->grad(&x);
    for (int i=0; i < this->bdd->num_of_vars; i++){
        res(i) = (*grad)[i];
    }
    return res;
}

double fval_for_optimizer_nlopt(const std::vector<double> &x, std::vector<double> &grad, void *data){
    Optimizer *opt = (Optimizer *) data;
    std::vector<double> *grad_p = opt->bdd->grad(&x);
    for ( int i = 0; i<grad.size(); i++) grad[i] = (*grad_p)[i];
    return opt->bdd->fval(&x);
}

double fval_for_optimizer_nlopt_gdfree(const std::vector<double> &x, std::vector<double> &grad, void *data){
    Optimizer *opt = (Optimizer *) data;
    return opt->bdd->fval(&x);
}

void Optimizer::update_weights(){
    for ( int i = 0; i < this->unsat_clauses->size(); i++){
        (*this->bdd->clause_weights) [ (*this->unsat_clauses)[i] ] *= this->param->multiplier;
    }
    this->bdd->sum_of_clause_weights = sum(this->bdd->clause_weights);
}

static std::vector<double> *rounding( column_vector *m, int n){
    std::vector<double> *res = new std::vector<double>;
    for ( int i = 0; i < n; i++){
        if ( (*m)(i) > 0 )
            res->push_back(1);
        else res->push_back(-1);
    }
    return res;
}

static std::vector<double> *rounding( std::vector<double> *x, int n){
    std::vector<double> *res = new std::vector<double>;
    for ( int i = 0; i < n; i++){
        if ( (*x)[i] > 0 )
            res->push_back(1);
        else res->push_back(-1);
    }
    return res;
}

static void random_restart(std::vector<double> *x){
    int n = x->size();
    for(int i=0; i< n; i++){
        double xi = (double) std::rand();
        (*x)[i] =  1 - 2 * (xi/(double)RAND_MAX);
    }

}

static int count(std::vector<double> *x){
    int count = 0;
    for( int i =0; i< x->size();  i++){
        if ((*x)[i] < 0) count++;
    }
    return count;
}

static int count(column_vector &m, int n){
    int count = 0;
    for( int i =0; i< n;  i++){
        if (m(i) < 0) count++;
    }
    return count;
}

static void print_vector(std::vector<double> *x){
    int n = x->size();
    for(int i=0; i< n; i++){
        std::cout<<(*x)[i]<<",";
    }
    std::cout<<std::endl;
}

static void print_solution(std::vector<double> *x){
    int n = x->size();
    std::cout<<"v ";
    for(int i=0; i< n; i++){
        if ((*x)[i] < 0){
            std::cout<<i+1<<" ";
        }
        else
            std::cout<<-(i+1)<<" ";
    }
    std::cout<<std::endl;
}

static void print_solution(column_vector &m, int n){
    std::cout<<"v ";
    for(int i=0; i< n; i++){
        if (m(i) < 0){
            std::cout<<i+1<<" ";
        }
        else
            std::cout<<-(i+1)<<" ";
    }
    std::cout<<std::endl;
}

double Optimizer::minimize()
{
    random_restart(this->x);
    int n = this->num_of_vars;
    static int min_unsat_for_maxsat = 1e9;
    double tol = 1e-40;
    while(1){
        if (  number_of_trials_this_start  >= this->param->max_trial_per_start ){
          //   if (this->param->verbose) std::cout<<"c Random restart"<<std::endl;
             random_restart(this->x);
             this->bdd->restart_weights();
             this->number_of_trials_this_start = 0;
        }
        this->number_of_trials_this_start ++;
        double stop_val = this->bdd->sum_of_clause_weights;
        column_vector starting_point(n);
        std::vector<double> *x = new std::vector<double>(n);
        for(int i = 0; i < n; i++){
            (*x)[i] = (*this->x)[i];
            starting_point(i) = (double) (*x)[i];
        }
        std::vector<double> *roundedx;
        std::string package;
        if (this->optimizer_name=="CG"){
            if (this->param->verbose)
                std::cout<<"c Solver: CG"<<std::endl;
            package = "DLIB";
            find_min_box_constrained(cg_search_strategy(),
                            objective_delta_stop_strategy(tol),
                           [this](const column_vector& a){ return this->fval_for_optimizer(a);},
                           [this](const column_vector& a){ return this->grad_for_optimizer(a);},
                           starting_point, -1.0, 1.0);
            roundedx = rounding(&starting_point,n);
                std::cout<<"c CG finds local maximum with value "<<-this->bdd->fval(roundedx)
                    <<". Goal: "<<this->bdd->sum_of_clause_weights<< std::endl;
        }
        else if (this->optimizer_name=="BFGS"){
            if (this->param->verbose)
                std::cout<<"c Solver: BFGS"<<std::endl;
            package = "DLIB";
            find_min_box_constrained(bfgs_search_strategy(),
                            objective_delta_stop_strategy(tol),
                           [this](const column_vector& a){ return this->fval_for_optimizer(a);},
                           [this](const column_vector& a){ return this->grad_for_optimizer(a);},
                           starting_point, -1.0, 1.0);
            roundedx = rounding(&starting_point,n);
            std::cout<<"c BFGS finds local maximum with value "<<-this->bdd->fval(roundedx)
                    <<". Goal: "<<this->bdd->sum_of_clause_weights<< std::endl;
        }
        else if ( this->optimizer_name=="SLSQP"){
            if (this->param->verbose)
                std::cout<<"c Solver: SLSQP"<<std::endl;
            package = "NLOPT";
            nlopt::opt opt(nlopt::LD_SLSQP, n);
            std::vector<double> lb(n);
            std::vector<double> ub(n);
            for ( int i =0; i<n;i++){
                lb[i] = -1;
                ub[i] = 1;
            }
            opt.set_lower_bounds(lb);
            opt.set_upper_bounds(ub);
            opt.set_min_objective(fval_for_optimizer_nlopt, this);
            opt.set_xtol_rel(tol);
            opt.set_ftol_abs(tol);
            double minf;
            try{
                nlopt::result result = opt.optimize(*x, minf);
                    std::cout << "c SLSQP founds local maximum with value"<< std::setprecision(10) << -minf
                    <<". Goal: "<<this->bdd->sum_of_clause_weights<< std::endl;
                }
            catch(std::exception &e) {
                if (this->param->verbose)
                    std::cout << "c nlopt failed: " << e.what() << std::endl;
            }
            roundedx = rounding(x,n);
        }
        else if ( this->optimizer_name=="MMA"){
            if (this->param->verbose)
                std::cout<<"c Solver: MMA"<<std::endl;
            package = "NLOPT";
       // nlopt::opt opt(nlopt::LD_MMA, n);
            nlopt::opt opt(nlopt::LD_CCSAQ, n);
            std::vector<double> lb(n);
            std::vector<double> ub(n);
            for ( int i =0; i<n;i++){
                lb[i] = -1;
                ub[i] = 1;
            }
            opt.set_lower_bounds(lb);
            opt.set_upper_bounds(ub);
            opt.set_min_objective(fval_for_optimizer_nlopt, this);
            opt.set_xtol_rel(tol);
            opt.set_ftol_abs(tol);
            double minf;
            try{
                nlopt::result result = opt.optimize(*x, minf);
                    std::cout << "c MMA finds local minimum with value "
                    << std::setprecision(10) << -minf
                    <<". Goal: "<<this->bdd->sum_of_clause_weights<< std::endl;
            }
            catch(std::exception &e) {
                if (this->param->verbose)
                    std::cout << "c nlopt failed: " << e.what() << std::endl;
            }
            roundedx = rounding(x,n);
        }
        else{ std::cout<<"c undefined optimizer: "<<this->optimizer_name<<std::endl; exit(0);}

        int num_unsat_clause, unsat_weight;
        this->bdd->verify_solution(roundedx,this->unsat_clauses, &num_unsat_clause, &unsat_weight);
        //double fval = fval_for_optimizer(starting_point);
        #ifdef CHANGE_WEIGHTS
            this->update_weights();
        #endif
        if (this->param->ismaxsat && (!this->param->iswcnf)){
            #pragma omp critical
            {
            if (num_unsat_clause < min_unsat_for_maxsat){
                min_unsat_for_maxsat = num_unsat_clause;
             }
                std::cout<<"o "<<min_unsat_for_maxsat<<std::endl;
            }
        }
        else if (this->param->ismaxsat && (this->param->iswcnf)){
            #pragma omp critical
            {
            if ( (unsat_weight < this->param->wcnf_weight) && (num_unsat_clause < min_unsat_for_maxsat)){
                min_unsat_for_maxsat = num_unsat_clause;
             }
                std::cout<<"o "<<min_unsat_for_maxsat<<std::endl;
            }
        }
        else{
           #pragma omp critical
            {
            if (num_unsat_clause < min_unsat_for_maxsat){
                std::cout<<"o "<<num_unsat_clause<<std::endl;
                min_unsat_for_maxsat = num_unsat_clause;
             }
            }
        }
        #pragma omp critical
        {
        if (num_unsat_clause<=this->param->n_violance_tolerance){
            cout<<"c solved by "<<this->optimizer_name<<". trials: "<<this->number_of_trials_this_start<<std::endl;
            cout<<"s SATISFIABLE "<<std::endl;
            if ( package == "DLIB" ){
                    print_solution(starting_point,n);
            }
            else if ( package == "NLOPT"){
                    print_solution(x);
            }
            this->solved_flag = 1;
            exit(0);
      }
      }
    }
    return 0;
}

Optimizer_Portfolio::Optimizer_Portfolio(){
}

Optimizer_Portfolio::Optimizer_Portfolio(int num_of_vars, BDD *original_bdd, Parameter *param){
    this->num_of_vars = num_of_vars;
    this->bdd = original_bdd;
    this->param = param;
}

void Optimizer_Portfolio::solve(){
    BDD bdd_group[this->param->ncores];
    Optimizer optimizer_group[this->param->ncores];
    std::vector<std::string> solvers{"CG","SLSQP","MMA","BFGS"};
    if (this->param->solver != "PORTFOLIO"){
       solvers.clear();
       solvers.push_back(this->param->solver);
       cout<<"solver: "<<this->param->solver<<endl;
    }
    for ( int i = 0; i < this->param->ncores; i++){
        bdd_group[i] = BDD(this->bdd);
        optimizer_group[i] = Optimizer(this->num_of_vars, &bdd_group[i], solvers[i % solvers.size()], this->param);
    }
    int num_trials = 0;
    bool solved_flag = 0;
    if (this->param->verbose)
        cout<<"c ncores "<<this->param->ncores<<endl;
    int nthreads =  param->ncores;
    #pragma omp parallel for num_threads(this->param->ncores)
    for ( int i = 0; i < nthreads; i++){
            int tid = omp_get_thread_num();
            optimizer_group[i].minimize();
            solved_flag |= optimizer_group[i].solved_flag;
    }
    cout<<"c number of trials: "<<num_trials<<endl;
}


#include <ql/types.hpp>
#include <ql/math/randomnumbers/seedgenerator.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/methods/montecarlo/sample.hpp>
#include <boost/static_assert.hpp>
#include <boost/random.hpp>

#include <math/randomnumbers/counter_based_engine.hpp>
#include <math/randomnumbers/threefry.hpp>
#include <math/randomnumbers/philox.hpp>
#include <math/randomnumbers/boostrngbinding.hpp>
#include <settings.hpp>

using namespace QuantLib;

namespace QLExtension {
	// Returns uniform rsg, caller responsible for array size
	void originalthreefry(uint64_t seed, uint64_t skip, uint64_t buffer[], uint64_t length)
	{
		boost::random::counter_based_engine<uint64_t, boost::random::threefry<2, uint64_t>, 32> rng(seed);
		rng.restart({0,0});		// fixed it
		rng.discard(skip);

		for (uint64_t i = 0; i < length; i++)
		{
			buffer[i] = rng();
		}
	}

	// Returns uniform rsg, caller responsible for array size
	void uniformthreefry(uint64_t seed, uint64_t skip, double buffer[], uint64_t length)
	{
		BoostThreefryUniformRng rng(seed);
		rng.restart(0);		// fixed it
		rng.discard(skip);

		for (uint64_t i = 0; i < length; i++)
		{
			buffer[i] = rng.nextReal();
		}
	}

	// Returns normal rsg, caller responsible for array size
	void normalthreefry(uint64_t seed, uint64_t skip, double buffer[], uint64_t length)
	{
		InverseCumulativeNormal ic;
		BoostThreefryUniformRng rng(seed);
		rng.restart(0);		// fixed it
		rng.discard(skip);

		for (uint64_t i = 0; i < length; i++)
		{
			buffer[i] = ic(rng.nextReal());
		}
	}

	// Returns normal rsg, caller responsible for array size
	/*
	void normalthreefry2(uint64_t seed, uint64_t skip, double buffer[], uint64_t length)
	{
		boost::random::counter_based_engine<uint64_t, boost::random::threefry<2, uint64_t>, 32> rng(seed);
		rng.restart({ 0, 0 });		// fixed it
		rng.discard(skip);

		boost::normal_distribution <> norm(0, 1);
		boost::variate_generator < boost::random::counter_based_engine<uint64_t, boost::random::threefry<2, uint64_t>, 32> &, boost::normal_distribution <>>
			unnorm(rng, norm);

		for (uint64_t i = 0; i < length; i++)
		{
			buffer[i] = unnorm();
		}
	}
	*/

	// Returns normal rsg, caller responsible for array size
	void gammathreefry(uint64_t seed, uint64_t skip, double buffer[], uint64_t length, Real shape, Real scale)
	{
		QLExtension::InverseCumulativeGamma ic(shape, scale);		// (1,1)
		BoostThreefryUniformRng rng(seed);
		rng.restart(0);		// fixed it
		rng.discard(skip);

		for (uint64_t i = 0; i < length; i++)
		{
			buffer[i] = ic(rng.nextReal());
		}
	}
}


/* Copyright 2020 Oinam Romesh Meitei

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
#include "fmath.hpp"
#include "pulsec.h"
#include "agradc.h"
#include "grad_ana.h"
#include "pulsehelper.h"

#include <iostream>
#include <iomanip>
#include <chrono>

namespace py = pybind11;

gausgrad gaus_getnamp(int nqubit, int &ngaus, double duration,
						 std::vector< std::vector<double > > &amp,
						 std::vector< std::vector<double > > &sigma,
						 std::vector< std::vector<double > > &mean,
						 std::vector<double > freq,
						 std::vector<double> &tlist,
						 std::vector<std::complex<double> > &ini_vec,
						 std::vector< std::vector< Eigen::SparseMatrix
									   <double,0,ptrdiff_t> > > hdrive,
						 std::vector< std::complex<double> > dsham,
						 std::vector< int> &states,
						 Eigen::MatrixXcd &cham){

  double tmpamp;
  int tlen = tlist.size();

  std::vector< std::vector< std::vector<double > > >
    expterm(nqubit, std::vector<std::vector<double> > (ngaus, std::vector< double> (tlen)));

  std::vector<std::vector<double > > tseq;
  int nwindow = 0;
  double gamp, gsig, gmean, esigmean;
  std::vector<double > agradient;

  std::vector<std::vector<double > > tamp(nqubit, std::vector<double> (tlen));
  int i,j,k;
  for (i=0; i<nqubit; i++){
    for (j=0;j<ngaus; j++){
      for (k=0;k<tlen; k++){
	expterm[i][j][k] = fmath::expd(-sigma[i][j] * sigma[i][j] *
				       (tlist[k] - mean[i][j]) *
				       (tlist[k] - mean[i][j]));
	tamp[i][k] += amp[i][j] * expterm[i][j][k];
      }
    }
  }

  pulsec pobj(tamp, tseq, freq, duration, nqubit, nwindow);

  gradc aobj = grad_ana(tlist, ini_vec, pobj, hdrive, dsham, states, cham);

  for(int i=0; i< nqubit; i++){
    for(int j=0; j< ngaus; j++){
      gamp = 0.0;
      gsig = 0.0;
      gmean = 0.0;
      for(int k=0; k< tlen;k++){
	esigmean = aobj.gradient[i][k] * expterm[i][j][k];

	gamp += esigmean;
	gsig += esigmean * amp[i][j] * -2.0 * sigma[i][j] *
	  (tlist[k] - mean[i][j])*(tlist[k] - mean[i][j]);
	gmean += esigmean * amp[i][j] * 2.0 * sigma[i][j] * sigma[i][j] *
	  (tlist[k] - mean[i][j]);

      }
      agradient.push_back(gamp);
      agradient.push_back(gsig);
      agradient.push_back(gmean);
    }
  }
  gausgrad gausobj(aobj.energy, aobj.norm, agradient, tamp);

  return gausobj;
}

std::vector<double> gaus_gettamp(int &nqubit, int &ngaus,
				std::vector<double> &tlist,
				std::vector< std::vector<double > > &amp,
				std::vector< std::vector<double > > &sigma,
				std::vector< std::vector<double > > &mean,
				std::vector< std::vector<double > > &gradient_){

  double gamp, gsig, gmean, esigmean;
  std::vector<double > agradient;
  int tlen = tlist.size();

  for(int i=0; i< nqubit; i++){
    for(int j=0; j< ngaus; j++){
      gamp = 0.0;
      gsig = 0.0;
      gmean = 0.0;
      for(int k=0; k< tlen;k++){
	esigmean = gradient_[i][k] * fmath::expd(-sigma[i][j]*sigma[i][j] *
						(tlist[k] - mean[i][j]) *
						(tlist[k] - mean[i][j]));
	gamp += esigmean;
	gsig += esigmean * amp[i][j] * -2.0 * sigma[i][j] *
	  (tlist[k] - mean[i][j])*(tlist[k] - mean[i][j]);
	gmean += esigmean * amp[i][j] * 2.0 * sigma[i][j] * sigma[i][j] *
	  (tlist[k] - mean[i][j]);

      }
      agradient.push_back(gamp);
      agradient.push_back(gsig);
      agradient.push_back(gmean);
    }
  }
  return agradient;
}

PYBIND11_MODULE(pulse_helper,m){
  m.def("gaus_getnamp", &gaus_getnamp, "gaus_getnamp");
  m.def("gaus_gettamp", &gaus_gettamp, "gaus_gettamp");
  py::class_<gausgrad>(m,"gausgrad")
    .def(py::init<
	 double &, double &,
	 std::vector<double> &,
	 std::vector<std::vector< double > > & > ())
    .def_readonly("energy", &gausgrad::energy)
    .def_readonly("norm", &gausgrad::norm)
    .def_readonly("gradient",&gausgrad::gradient)
    .def_readonly("amp",&gausgrad::amp);
}


#include <igl/writeDMAT.h>
#include <igl/readDMAT.h>
#include <igl/readMESH.h>
#include <igl/get_seconds.h>

#include <GaussIncludes.h>
#include <ForceSpring.h>
#include <FEMIncludes.h>
#include <PhysicalSystemParticles.h>
#include <ConstraintFixedPoint.h>

#include <boost/filesystem.hpp>
#include <nlohmann/json.hpp>

#include <omp.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <memory>
#include <algorithm>

#include "GreedyCubop.h"

using json = nlohmann::json;
namespace fs = boost::filesystem;
using namespace std;
using namespace Gauss;
using namespace FEM;
using namespace ParticleSystem;

typedef PhysicalSystemFEM<double, NeohookeanTet> NeohookeanTets;

typedef World<double,
    std::tuple<PhysicalSystemParticleSingle<double> *, NeohookeanTets *>,
    std::tuple<ForceSpringFEMParticle<double> *>, std::tuple<ConstraintFixedPoint<double> *> > MyWorld;

// Globals
Eigen::MatrixXd V; // Verts
Eigen::MatrixXi T; // Tet indices
Eigen::MatrixXi F; // Face indices
fs::path model_root;
int goal_tet_count = 0;
double starting_time;

void eig_to_VEC(const Eigen::VectorXd &eig_vec, VECTOR &vec_vec) {
    vec_vec.resizeAndWipe(eig_vec.size());
    std::memcpy(&vec_vec(0), eig_vec.data(), eig_vec.size() * sizeof(double));
}

Eigen::VectorXd VEC_to_eig(VECTOR &vec) {
    Eigen::VectorXd eig(vec.size());
    std::memcpy(eig.data(), &vec(0), vec.size() * sizeof(double));
    return eig;
}

std::string ZeroPadNumber(int num)
{
    std::ostringstream ss;
    ss << std::setw( 7 ) << std::setfill( '0' ) << num;
    return ss.str();
}

template<typename T>
T get_json_value(const json &j, const std::string &key, T def) {
    try {
        return j.at(key);
    }
    catch (nlohmann::detail::out_of_range& e){
        return def;
    }
}

class MyGreedyCubop : public GreedyCubop {
public:
    MyGreedyCubop(const Eigen::MatrixXd &V, const Eigen::MatrixXi &T, const Eigen::MatrixXd &U, double YM, double poisson, double density, const std::vector<Eigen::VectorXd> &reduced_forces, const std::vector<Eigen::VectorXd> &red_displacements) : GreedyCubop(), m_reduced_forces(reduced_forces), m_red_displacements(red_displacements) {
        m_tets = make_unique<NeohookeanTets>(V,T);
        for(auto element: m_tets->getImpl().getElements()) {
            element->setDensity(density);
            element->setParameters(YM, poisson);
        }
        // m_world.addSystem(m_tets.get());
        // m_world.finalize();

        m_U = U;
    }

protected:
    /**
     * Return the total number of points we have to choose from.
     * For example, for FEM piecewise-linear tetrahedra elements, this is just the number of tets.
     */
    int numTotalPoints() {
        return m_tets->getImpl().getNumElements();
    }

    /**
     * The main method that subclasses must implement. This should evaluate the reduced force density (or just the force, since
     * the volume/area is constant anyway) at the given point.
     *
     *  pointId - an index in [0, getNumTotalPoints()). Implementations must map this index deterministically to a cubature point.
     */
    void evalPointForceDensity( int pointId, VECTOR& q, VECTOR& gOut ) {
        // THIS ISN'T THREAD SAFE WITHOUT COPYING THE WORLD
        // Make a copy of the world every time... Not the most efficient, but still faster than being single threaded
        // double start = igl::get_seconds();

        MyWorld m_world;
        m_world.addSystem(m_tets.get());
        m_world.finalize();

        // update position of only the tet pointId
        Eigen::VectorXd eig_q = VEC_to_eig(q);
        auto gauss_map_q = mapDOFEigen(m_tets->getQ(), m_world);
        const int n_verts = 4;
        std::vector<int> verts(n_verts);
        for(int i = 0; i < n_verts; i++) {
            const int tet_vert = m_tets->getImpl().getElement(pointId)->getQDOFList()[i]->getGlobalId();
            verts[i] = tet_vert;

            for(int j = 0; j < 3; j++) {
                gauss_map_q[tet_vert + j] = m_U.transpose().row(tet_vert + j) * eig_q;
            }
        }
        // gauss_map_q = m_U.transpose() * eig_q;

        // Get the element force
        Eigen::VectorXd sampled_force(12);
        m_tets->getImpl().getElement(pointId)->getInternalForce(sampled_force, m_world.getState());

        // Assemble and project the full force
        Eigen::SparseVector<double> full_force(m_U.cols());
        for(int i = 0; i < n_verts; i++) {
            int vert_index = verts[i];//m_tets->getImpl().getElement(pointId)->getQDOFList()[i]->getGlobalId();
            for(int j = 0; j < 3; j++) {
                full_force.coeffRef(vert_index + j) = sampled_force[i * 3 + j];
            }
        }

        eig_to_VEC(m_U * full_force, gOut);
        // std::cout << "evalPointForceDensity: " << igl::get_seconds() - start << "s" << std::endl;
    }

    /**
     * At the end of each iteration (_not_ a sub-train iteration), when some cubature has been optimized, this will be called.
     * Implementations should probably overwrite this and write the cubature out to file or something.
     */
    void handleCubature( std::vector<int>& selectedPoints, VECTOR& weights, Real relErr ) {
        stringstream my_out;

        my_out << "n_tets: " << selectedPoints.size() << endl;
        my_out << "relErr: " << relErr << endl;
        my_out << "Selected tets: ";
        for (std::vector<int>::iterator i = selectedPoints.begin(); i != selectedPoints.end(); ++i)
        {
            my_out << *i << ", ";
        }
        my_out << endl;

        std::vector<int> nonzero_indices;
        std::vector<double> nonzero_weights;
        int n_nonzero = 0;
        my_out << "Weights: ";
        for (int i = 0; i < weights.size(); ++i)
        {
            if(weights(i) > 0.00001) {
                n_nonzero++;
                nonzero_indices.push_back(selectedPoints[i]);
                nonzero_weights.push_back(weights(i));
            }

            my_out << weights(i) << ", ";
        }
        my_out << endl << "n_nonzero: " << n_nonzero << endl;
        my_out << endl;
        my_out << "Running time so far: " << igl::get_seconds() - starting_time << "s" << std::endl;
        my_out << endl;

        Eigen::VectorXi Is = Eigen::Map<Eigen::VectorXi>(&nonzero_indices[0], nonzero_indices.size());
        Eigen::VectorXd Ws = Eigen::Map<Eigen::VectorXd>(&nonzero_weights[0], nonzero_weights.size());

        fs::path energy_model_dir = model_root / ("energy_model/an08/pca_dim_" + std::to_string(m_U.rows()) + "/");
        fs::path this_iteration_output_dir = energy_model_dir / (ZeroPadNumber(n_nonzero) + "_samples/");
        fs::create_directories(this_iteration_output_dir);

        fs::path indices_path = this_iteration_output_dir / "indices.dmat";
        fs::path weights_path = this_iteration_output_dir / "weights.dmat";
        igl::writeDMAT(indices_path.string(), Is);
        igl::writeDMAT(weights_path.string(), Ws);

        fs::path details_path = this_iteration_output_dir / "details.txt";
        ofstream fout(details_path.string());
        fout << my_out.str();
        fout.close();

        cout << my_out.str();
        cout << "Saved weights and indices to " << this_iteration_output_dir.string() << endl << endl;

        const bool DEBUG = false;
        if(DEBUG) {
            int example_id = 500 % m_reduced_forces.size();//250;
            Eigen::VectorXd actual_g = m_reduced_forces[example_id];
            Eigen::VectorXd pred_g = get_predicted_force(example_id, nonzero_indices, nonzero_weights);

            std::cout << actual_g.transpose() << std::endl;
            std::cout << pred_g.transpose() << std::endl;

            std::cout << (actual_g - pred_g).transpose().cwiseAbs() << std::endl;

            std::cout << (actual_g - pred_g).squaredNorm() << std::endl;
        }

        // output everything every frame
        {
            fs::path indices_path = energy_model_dir / "indices.dmat";
            fs::path weights_path = energy_model_dir / "weights.dmat";
            igl::writeDMAT(indices_path.string(), Is);
            igl::writeDMAT(weights_path.string(), Ws);

            fs::path details_path = energy_model_dir / "details.txt";
            ofstream fout(details_path.string());
            fout << my_out.str();
            fout.close();
        }
        if(n_nonzero >= goal_tet_count) {
            cout << "Reached goal number of tets. Exiting." << endl;
            exit(0);
        }
    }

    Eigen::VectorXd get_predicted_force(int example_id, const std::vector<int> &nonzero_indices, const std::vector<double> &nonzero_weights) {
        Eigen::VectorXd pred_g = Eigen::VectorXd::Zero(m_U.rows());

        MyWorld world;
        world.addSystem(m_tets.get());
        world.finalize();
        auto gauss_map_q = mapDOFEigen(m_tets->getQ(), world);
        gauss_map_q =  m_U.transpose() * m_red_displacements[example_id]; // Update the mesh

        int n_sample_tets = nonzero_indices.size();
        Eigen::SparseMatrix<double> neg_energy_sample_jac(m_U.cols(), T.rows());// m_cubature_indices.size());
        neg_energy_sample_jac.reserve(Eigen::VectorXi::Constant(m_U.cols(), T.cols() * 3));

        Eigen::VectorXd cubature_weights = Eigen::Map<const Eigen::VectorXd>(&nonzero_weights[0], nonzero_weights.size());

        int n_force_per_element = T.cols() * 3;
        Eigen::MatrixXd element_forces(n_sample_tets, n_force_per_element);
        Eigen::VectorXd energy_samp(n_sample_tets);

        for(int i = 0; i < n_sample_tets; i++) { // TODO parallel
            int tet_index = nonzero_indices[i];
            energy_samp[i] = m_tets->getImpl().getElement(tet_index)->getStrainEnergy(world.getState());
            // std::cout << energy_samp[i] << std::endl;
            //Forces
            Eigen::VectorXd sampled_force(n_force_per_element);
            m_tets->getImpl().getElement(tet_index)->getInternalForce(sampled_force, world.getState());
            element_forces.row(i) = sampled_force;
        }

        // std::cout << element_forces << std::endl;

        for(int i = 0; i < n_sample_tets; i++) {
            int tet_index = nonzero_indices[i];
            for(int j = 0; j < 4; j++) {
                int vert_index = m_tets->getImpl().getElement(tet_index)->getQDOFList()[j]->getGlobalId();
                for(int k = 0; k < 3; k++) {
                    neg_energy_sample_jac.insert(vert_index + k, i) = element_forces(i, j*3 + k);
                }
            }
        }
        pred_g = m_U * (neg_energy_sample_jac * cubature_weights);

        return pred_g;
    }

private:
    // MyWorld m_world;
    unique_ptr<NeohookeanTets> m_tets;
    Eigen::MatrixXd m_U;
    const std::vector<Eigen::VectorXd> &m_reduced_forces;
    const std::vector<Eigen::VectorXd> &m_red_displacements;
};

std::vector<int> get_file_numbers_for_prefix(std::string prefix, fs::path dir) {
    std::vector<int> nums;
    for (auto i = fs::directory_iterator(dir); i != fs::directory_iterator(); i++)
    {
        fs::path cand_path = i->path();

        std::string name = cand_path.filename().string();
        if(strncmp(name.c_str(), prefix.c_str(), prefix.size()) == 0) { // Is forces
            std::string num_str = name.substr(prefix.size());
            nums.push_back(std::stoi(num_str, nullptr));
        }
    }
    std::sort(nums.begin(), nums.end());
    return nums;
}

std::vector<Eigen::VectorXd> load_forces(fs::path training_data_root, fs::path reduced_basis_path) {
    std::cout << "Loading reduced internal forces..." << std::endl;
    Eigen::MatrixXd U;
    igl::readDMAT(reduced_basis_path.string(), U);
    U.transposeInPlace();

    std::string prefix = "internalForces_";
    std::vector<int> forces_nums = get_file_numbers_for_prefix(prefix, training_data_root);
    std::vector<Eigen::VectorXd> recorded_forces(forces_nums.size());

    std::cout << "num threads: " << omp_get_num_threads() << std::endl;
    #pragma omp parallel for
    for (int i = 0; i < forces_nums.size(); i++)
    {//   std::vector<int>::iterator i = forces_nums.begin(); i != forces_nums.end(); ++i
        fs::path dis_path = training_data_root / (prefix + std::to_string(forces_nums[i]) + ".dmat");
        Eigen::VectorXd F;
        igl::readDMAT(dis_path.string(), F);
        recorded_forces[i] = U * F;
    }

    std::cout << "Done." << std::endl;

    return recorded_forces;
}

std::vector<Eigen::VectorXd> load_displacements(fs::path training_data_root, fs::path reduced_basis_path) {
    std::cout << "Loading displacements..." << std::endl;
    Eigen::MatrixXd U;
    igl::readDMAT(reduced_basis_path.string(), U);
    U.transposeInPlace();

    std::string prefix = "displacements_";
    std::vector<int> displacements_nums = get_file_numbers_for_prefix(prefix, training_data_root);
    std::vector<Eigen::VectorXd> recorded_displacements(displacements_nums.size());

    #pragma omp parallel for
    for (int i = 0; i < displacements_nums.size(); i++)
    {
        fs::path dis_path = training_data_root / (prefix + std::to_string(displacements_nums[i]) + ".dmat");
        Eigen::MatrixXd Q;
        igl::readDMAT(dis_path.string(), Q);
        Eigen::VectorXd q = Eigen::Map<Eigen::VectorXd>(Q.transpose().data(), Q.rows() * Q.cols());
        recorded_displacements[i]=(U*q);
    }
    std::cout << "Done." << std::endl;

    return recorded_displacements;
}

void progress_bar(double progress, int barWidth = 70) {
    std::cout << "[";
    int pos = barWidth * progress;
    for (int i = 0; i < barWidth; ++i) {
        if (i < pos) std::cout << "=";
        else if (i == pos) std::cout << ">";
        else std::cout << " ";
    }
    std::cout << "] " << int(progress * 100.0) << " %\r";
    std::cout.flush();
}

std::vector<Eigen::VectorXd> get_forces_from_reduced_displacements(const std::vector<Eigen::VectorXd> &red_displacements, const Eigen::MatrixXd &V, const Eigen::MatrixXi &T, const Eigen::MatrixXd &U, double YM, double poisson, double density) {
    std::cout << "Generating forces for reduced poses..." << std::endl;
    unique_ptr<NeohookeanTets> tets = make_unique<NeohookeanTets>(V,T);

    for(auto element: tets->getImpl().getElements()) {
        element->setDensity(density);//1000.0);
        element->setParameters(YM, poisson);
    }
    std::vector<Eigen::VectorXd> forces(red_displacements.size());
    int n_done = 0;
    double start = igl::get_seconds();
    #pragma omp parallel
    {
        MyWorld world;
        world.addSystem(tets.get());
        world.finalize();

        #pragma omp for
        for(int i = 0; i < red_displacements.size(); i++) {
            Eigen::Map<Eigen::VectorXd> gauss_map_q = mapDOFEigen(tets->getQ(), world);
            gauss_map_q = U.transpose() * red_displacements[i];

            AssemblerEigenVector<double> internal_force;
            getInternalForceVector(internal_force, *tets, world);
            forces[i] = (U * (*internal_force));
            // std::cout << forces.back() << std::endl;
            #pragma omp atomic
            n_done++;

            if(i % 20 == 0) progress_bar(n_done / (double) red_displacements.size());
        }
    }
    std::cout << "Generating forces: " << igl::get_seconds() - start << "s" << std::endl;
    std::cout << std::endl;
    return forces;
}

int main(int argc, char **argv) {
    starting_time = igl::get_seconds();

    if(argc < 3) {
        cout << "Need to pass in a path to model root and a goal number of tets." << endl;
        exit(1);
    }

    // ---- Set Up
    model_root = fs::path(argv[1]);
    goal_tet_count = std::stoi(argv[2]);

    fs::path reduced_basis_path = model_root / "pca_results" / "ae_pca_components.dmat";

    int pca_dim;
    if(argc == 4) {
        pca_dim = std::stoi(argv[3]);
        reduced_basis_path = model_root / "pca_results" / ("pca_components_" + std::to_string(pca_dim) + ".dmat");
    }

    fs::path model_config_path = model_root / "model_config.json";
    std::ifstream fin_model(model_config_path.string());
    json model_config;
    fin_model >> model_config;

    std::string tdr = model_config["training_dataset"];
    fs::path training_data_root = tdr;
    std::cout << training_data_root << std::endl;
    fs::path mesh_path = model_root / "tets.mesh";
    fs::path sim_config_path = model_root / "sim_config.json";

    // Load sim config
    std::ifstream fin(sim_config_path.string());
    json sim_config;
    fin >> sim_config;
    double YM = sim_config["material_config"]["youngs_modulus"];
    double poisson = sim_config["material_config"]["poissons_ratio"];
    double density = sim_config["material_config"]["density"];

    // Load data
    igl::readMESH(mesh_path.string(), V, T, F);

    Eigen::MatrixXd U;
    igl::readDMAT(reduced_basis_path.string(), U);
    U.transposeInPlace();
    std::cout << U.rows() << " " << U.cols() << std::endl;

    // std::vector<Eigen::VectorXd> reduced_forces = load_forces(training_data_root, reduced_basis_path); // TODO maybe subsample?
    std::vector<Eigen::VectorXd> red_displacements = load_displacements(training_data_root, reduced_basis_path);
    // std::random_shuffle(red_displacements.begin(), red_displacements.end());
    // std::vector<Eigen::VectorXd> less_displacements(red_displacements.begin(), red_displacements.begin() + 100);
    std::vector<Eigen::VectorXd> reduced_forces = get_forces_from_reduced_displacements(red_displacements, V, T, U, YM, poisson, density);

    // *** TODO ***
    // I should test this by summing up the reduced forces at each tet and making sure they are equal to the full reduced force
    // It's also possible that I should be generating the full reduced forces after projecting into, and out of, the reduced space
    // for each training example...

    // ---- Convert it all to the cubacode format
    int n_poses = reduced_forces.size();
    int r = reduced_forces[0].size();
    cout << "T: " << n_poses << endl;
    cout << "r: " << r << endl;
    cout << "r*T: " << (n_poses*r) << endl;
    cout << "Tets: " << T.rows() << endl;

    // I have to construct the *reduced* forces evaluated at each of the total T tets.
    VECTOR trainingForces(r * n_poses);
    for(int i = 0; i < n_poses; i++) {
        for(int j = 0; j < r; j++) {
            trainingForces(i*r + j) = reduced_forces[i][j];
        }
    }

    //What's this? -> It's all the configurations for each training pose
    std::vector<VECTOR> training_poses(red_displacements.size());
    TrainingSet trainingSet;
    for(int i = 0; i < n_poses; i++) {
        eig_to_VEC(red_displacements[i], training_poses[i]);
        trainingSet.push_back(&training_poses[i]);
    }

    // Set up the optimization
    MyGreedyCubop cubop(V, T, U, YM, poisson, density, reduced_forces, red_displacements);

    // Params
    Real relErrTol = get_json_value(model_config["learning_config"]["energy_model_config"], "rel_error_tol", 0.05);//0.05; // What's a good val?
    int maxNumPoints = goal_tet_count * 5; // some sane limit, for overnight runs
    int numCandsPerIter = 200;//100;//T.rows() / 100;  //100;// default 100;  // |C|
    int itersPerFullNNLS = r/2; // r/2 in the paper
    int numSamplesPerSubtrain = 50; //training_poses.size() / 4; // default 50;   // T_s

    std::cout << "relErrTol: " << relErrTol << std::endl;
    std::cout << "maxNumPoints: " << maxNumPoints << std::endl;
    std::cout << "numCandsPerIter: " << numCandsPerIter << std::endl;
    std::cout << "itersPerFullNNLS: " << itersPerFullNNLS << std::endl;
    std::cout << "numSamplesPerSubtrain: " << numSamplesPerSubtrain << std::endl;

    cout << "Working" << endl;

    cubop.run(
        trainingSet,
        trainingForces,
        relErrTol,
        maxNumPoints,
        numCandsPerIter,
        itersPerFullNNLS,
        numSamplesPerSubtrain
    );

    cout << "Didn't reach goal number of tets." << endl;

    return 0;
}


#include <armadillo>
#include <string>
#include "TSP.h"

using namespace std;

int main(int argc, char **argv) {
  string fname = "../test.tsp";

  if (argc > 1) {
    fname = argv[1];
  }

  auto dist = get_dist_tsp(fname);
  int cities = dist.n_rows;
  cout <<dist;

  ivec sol_seq = linspace<ivec>(0, cities-1, cities);
  //  cout <<sol_seq;
  ivec sol = new_solution(cities);
  cout <<"Fitness Sequential: " <<fitness(sol_seq, dist) <<endl;

  return 0;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     isovist_gradients.cpp
 * \author   Collin Johnson
 *
 * Definition of VoronoiIsovistGradients.
 */

#include "hssh/local_topological/area_detection/gateways/isovist_gradients.h"
#include "hssh/local_topological/area_detection/local_topo_isovist_field.h"
#include "math/savitzky_golay.h"
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/as_array.hpp>
#include <cmath>
#include <numeric>

namespace vulcan
{
namespace hssh
{

VoronoiIsovistGradients::VoronoiIsovistGradients(const VoronoiEdges& edges) : edges_(edges)
{
    for (const auto& e : edges_) {
        boost::push_back(isovistCells_, boost::as_array(e));
    }

    gradients_.resize(isovistCells_.size());
    initializeCellToIndex();
}

void VoronoiIsovistGradients::calculateGradients(utils::Isovist::Scalar scalar, const VoronoiIsovistField& field)
{
    for (const auto& edgeCells : edges_) {
        calculateGradientsForEdge(edgeCells, scalar, field);
    }
}

void VoronoiIsovistGradients::initializeCellToIndex(void)
{
    cellToIndex_.reserve(isovistCells_.size());
    int index = 0;
    for (auto& cell : isovistCells_) {
        cellToIndex_[cell] = index++;
    }
}

void VoronoiIsovistGradients::calculateGradientsForEdge(const CellVector& cells,
                                                        utils::Isovist::Scalar scalar,
                                                        const VoronoiIsovistField& field)
{
    edgeScalars_.clear();

    std::transform(cells.begin(), cells.end(), std::back_inserter(edgeScalars_), [&](cell_t cell) {
        return field.at(cell).scalar(scalar);
    });

    edgeDerivs_.resize(edgeScalars_.size());

    if (edgeDerivs_.size() >= 7) {
        math::savitzky_golay_first_deriv(edgeScalars_.begin(),
                                         edgeScalars_.end(),
                                         edgeDerivs_.begin(),
                                         math::SGWindowRadius::three);
    } else if (edgeDerivs_.size() >= 5) {
        math::savitzky_golay_first_deriv(edgeScalars_.begin(),
                                         edgeScalars_.end(),
                                         edgeDerivs_.begin(),
                                         math::SGWindowRadius::two);
    } else {
        std::adjacent_difference(edgeScalars_.begin(), edgeScalars_.end(), edgeDerivs_.begin());
        edgeDerivs_[0] = 0.0;
    }

    for (std::size_t n = 0; n < cells.size(); ++n) {
        int cellIndex = cellToIndex_[cells[n]];

        // Take the absolute value of the derivative to account for it being a function of the direction
        // of iteration along the edge. If we iterated the other direction, then it would have the opposite
        // sign -- thus causing some otherwise obvious gateways to "hide"
        gradients_[cellIndex] = position_value_t(cells[n], std::abs(edgeDerivs_[n]), edgeScalars_[n]);

        if ((scalar == utils::Isovist::kWeightedOrientation) || (scalar == utils::Isovist::kOrientation)) {
            gradients_[cellIndex].value = std::abs(wrap_to_pi_2(edgeDerivs_[n]));
        }
    }
}

}   // namespace hssh
}   // namespace vulcan