//
// Copyright (c) 2017-2019 CNRS INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-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_kinematics_derivatives_all)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  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));

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

  for(size_t i = 1; i < (size_t)model.njoints; ++i)
  {
    BOOST_CHECK(data.oMi[i].isApprox(data_ref.oMi[i]));
    BOOST_CHECK(data.v[i].isApprox(data_ref.v[i]));
    BOOST_CHECK(data.ov[i].isApprox(data_ref.oMi[i].act(data_ref.v[i])));
    BOOST_CHECK(data.a[i].isApprox(data_ref.a[i]));
    BOOST_CHECK(data.oa[i].isApprox(data_ref.oMi[i].act(data_ref.a[i])));
  }

  computeJointJacobians(model,data_ref,q);
  BOOST_CHECK(data.J.isApprox(data_ref.J));

  computeJointJacobiansTimeVariation(model, data_ref, q, v);
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
}

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

  Model model;
  buildModels::humanoidRandom(model);

  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);

  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  Data::Matrix6x partial_dq(6,model.nv); partial_dq.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(6,model.nv); partial_dv_local.setZero();

  getJointVelocityDerivatives(model,data,jointId,WORLD,
                              partial_dq,partial_dv);

  getJointVelocityDerivatives(model,data,jointId,LOCAL,
                              partial_dq_local,partial_dv_local);

  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
  computeJointJacobians(model,data_ref,q);
  getJointJacobian(model,data_ref,jointId,WORLD,J_ref);
  getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local);

  BOOST_CHECK(partial_dv.isApprox(J_ref));
  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(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(6,model.nv); partial_dv_fd_local.setZero();
  const double alpha = 1e-6;

  // dvel/dv
  Eigen::VectorXd v_plus(v);
  Data data_plus(model);
  forwardKinematics(model,data_ref,q,v);
  Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId]));
  Motion v0_local(data_ref.v[jointId]);
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    forwardKinematics(model,data_plus,q,v_plus);

    partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
    partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
    v_plus[k] -= alpha;
  }

  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,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);
  v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]);

  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);

    partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
    partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
    v_eps[k] -= alpha;
  }

  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha)));

  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));

//  computeJointJacobiansTimeVariation(model,data_ref,q,v);
//  Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.setZero();
//  getJointJacobianTimeVariation<WORLD>(model,data_ref,jointId,dJ_ref);
//  BOOST_CHECK(partial_dq.isApprox(dJ_ref));
//
//  std::cout << "partial_dq\n" << partial_dq << std::endl;
//  std::cout << "dJ_ref\n" << dJ_ref << std::endl;

}

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

  Model model;
  buildModels::humanoidRandom(model);

  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);

  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  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 a_partial_dq(6,model.nv); a_partial_dq.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(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(6,model.nv); a_partial_da_local.setZero();

  getJointAccelerationDerivatives(model,data,jointId,WORLD,
                                  v_partial_dq,
                                  a_partial_dq,a_partial_dv,a_partial_da);

  getJointAccelerationDerivatives(model,data,jointId,LOCAL,
                                  v_partial_dq_local,
                                  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);

  // Check v_partial_dq against getJointVelocityDerivatives
  {
    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_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
    Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero();

    getJointVelocityDerivatives(model,data_v,jointId,WORLD,
                                v_partial_dq_ref,v_partial_dv_ref);

    BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
    getJointVelocityDerivatives(model,data_v,jointId,LOCAL,
                                v_partial_dq_ref_local,v_partial_dv_ref);

    BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
  }

  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
  computeJointJacobians(model,data_ref,q);
  getJointJacobian(model,data_ref,jointId,WORLD,J_ref);
  getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local);

  BOOST_CHECK(a_partial_da.isApprox(J_ref));
  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(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(data_ref.oMi[jointId].act(data_ref.a[jointId]));
  Motion a0_local(data_ref.a[jointId]);
  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) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
    a_partial_da_fd_local.col(k) = (data_plus.a[jointId] - 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.isApprox(a_partial_da_fd_local,sqrt(alpha)));
  motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_da,a_partial_da_local);
  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(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) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
    a_partial_dv_fd_local.col(k) = (data_plus.a[jointId] - 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.isApprox(a_partial_dv_fd_local,sqrt(alpha)));
  motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_dv,a_partial_dv_local);
  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.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(6,model.nv); a_partial_dq_fd_local.setZero();
//  a.setZero();

  computeForwardKinematicsDerivatives(model,data,q,v,a);
  getJointAccelerationDerivatives(model,data,jointId,WORLD,
                                  v_partial_dq,
                                  a_partial_dq,a_partial_dv,a_partial_da);

  getJointAccelerationDerivatives(model,data,jointId,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);
  a0 = data_ref.oMi[jointId].act(data_ref.a[jointId]);
  a0_local = data_ref.a[jointId];

  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);

    a_partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
    a_partial_dq_fd_local.col(k) = (data_plus.a[jointId] - 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.isApprox(a_partial_dq_fd_local,sqrt(alpha)));

}

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

  Model model;
  buildModels::humanoidRandom(model,true);

  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));

  const Model::JointIndex jointId = model.existJointName("rarm4_joint")?model.getJointId("rarm4_joint"):(Model::Index)(model.njoints-1);
  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
  Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
  Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();

  // LOCAL: da/dv == dJ/dt + dv/dq
//  {
//    Data::Matrix6x rhs(6,model.nv); rhs.setZero();
//
//    v_partial_dq.setZero();
//    a_partial_dq.setZero();
//    a_partial_dv.setZero();
//    a_partial_da.setZero();
//
//    computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
//    computeForwardKinematicsDerivatives(model,data,q,v,a);
//
//    getJointAccelerationDerivatives<LOCAL>(model,data,jointId,
//                                           v_partial_dq,
//                                           a_partial_dq,a_partial_dv,a_partial_da);
//
//    getJointJacobianTimeVariation<LOCAL>(model,data_ref,jointId,rhs);
//
//    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
//    getJointVelocityDerivatives<LOCAL>(model,data_ref,jointId,
//                                       v_partial_dq_ref,v_partial_dv_ref);
//    rhs += v_partial_dq_ref;
//    BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));
//
//    std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
//  }

  // WORLD: da/dv == dJ/dt + dv/dq
  {
    Data::Matrix6x rhs(6,model.nv); rhs.setZero();

    v_partial_dq.setZero();
    a_partial_dq.setZero();
    a_partial_dv.setZero();
    a_partial_da.setZero();

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

    getJointAccelerationDerivatives(model,data,jointId,WORLD,
                                    v_partial_dq,
                                    a_partial_dq,a_partial_dv,a_partial_da);

    getJointJacobianTimeVariation(model,data_ref,jointId,WORLD,rhs);

    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
    getJointVelocityDerivatives(model,data_ref,jointId,WORLD,
                                v_partial_dq_ref,v_partial_dv_ref);
    rhs += v_partial_dq_ref;
    BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));

//    std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
  }

//  // WORLD: da/dq == d/dt(dv/dq)
//  {
//    const double alpha = 1e-8;
//    Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
//
//    Data data_plus(model);
//    v_plus = v + alpha * a;
//    q_plus = integrate(model,q,alpha*v);
//
//    computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
//    computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
//
//    Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
//    Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();
//
//    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
//
//    v_partial_dq.setZero();
//    a_partial_dq.setZero();
//    a_partial_dv.setZero();
//    a_partial_da.setZero();
//
//    getJointVelocityDerivatives<WORLD>(model,data_ref,jointId,
//                                       v_partial_dq_ref,v_partial_dv_ref);
//    getJointVelocityDerivatives<WORLD>(model,data_plus,jointId,
//                                       v_partial_dq_plus,v_partial_dv_plus);
//
//    getJointAccelerationDerivatives<WORLD>(model,data_ref,jointId,
//                                           v_partial_dq,
//                                           a_partial_dq,a_partial_dv,a_partial_da);
//
//    Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
//    {
//      Data data_fd(model);
//      VectorXd q_fd(model.nq), v_eps(model.nv); v_eps.setZero();
//      for(int k = 0; k < model.nv; ++k)
//      {
//        v_eps[k] += alpha;
//        q_fd = integrate(model,q,v_eps);
//        forwardKinematics(model,data_fd,q_fd,v,a);
//        a_partial_dq_fd.col(k) = (data_fd.oMi[jointId].act(data_fd.a[jointId]) - data_ref.oa[jointId]).toVector()/alpha;
//        v_eps[k] = 0.;
//      }
//    }
//
//    Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
//    BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));
//
//    std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
//    std::cout << "a_partial_dq_fd\n" << a_partial_dq_fd << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
//  }

  // LOCAL: da/dq == d/dt(dv/dq)
  {
    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);

    Data data_plus(model);
    v_plus = v + alpha * a;
    q_plus = integrate(model,q,alpha*v);

    computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
    computeForwardKinematicsDerivatives(model,data_ref,q,v,a);

    Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
    Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();

    v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();

    v_partial_dq.setZero();
    a_partial_dq.setZero();
    a_partial_dv.setZero();
    a_partial_da.setZero();

    getJointVelocityDerivatives(model,data_ref,jointId,LOCAL,
                                       v_partial_dq_ref,v_partial_dv_ref);
    getJointVelocityDerivatives(model,data_plus,jointId,LOCAL,
                                v_partial_dq_plus,v_partial_dv_plus);

    getJointAccelerationDerivatives(model,data_ref,jointId,LOCAL,
                                    v_partial_dq,
                                    a_partial_dq,a_partial_dv,a_partial_da);

    Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
    BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));

//    std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
//    std::cout << "rhs\n" << rhs << std::endl;
  }

}

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

  Model model;
  buildModels::humanoidRandom(model,true);

  Data data(model), data_ref(model), data_plus(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);

  const Model::JointIndex joint_id = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);

  computeJointJacobians(model,data,q);
  computeJointKinematicHessians(model,data);

  Data data2(model);
  computeJointKinematicHessians(model,data2,q);
  BOOST_CHECK(data2.J.isApprox(data.J));

  const Eigen::DenseIndex matrix_offset = 6 * model.nv;

  for(int k = 0; k < model.nv; ++k)
  {
    Eigen::Map<Data::Matrix6x> dJ(data.kinematic_hessians.data() + k*matrix_offset,6,model.nv);
    Eigen::Map<Data::Matrix6x> dJ2(data2.kinematic_hessians.data() + k*matrix_offset,6,model.nv);

    BOOST_CHECK(dJ2.isApprox(dJ));
  }

  for(int i = 0; i < model.nv; ++i)
  {
    for(int j = i; j < model.nv; ++j)
    {
      bool j_is_children_of_i = false;
      for(int parent = j; parent >= 0; parent = data.parents_fromRow[(size_t)parent])
      {
        if(parent == i)
        {
          j_is_children_of_i = true;
          break;
        }
      }

      if(j_is_children_of_i)
      {
        if(i==j)
        {
          Eigen::Map<Data::Motion::Vector6> SixSi(  data.kinematic_hessians.data()
                                                  + i * matrix_offset
                                                  + i * 6);
          BOOST_CHECK(SixSi.isZero());
        }
        else
        {
          Eigen::Map<Data::Motion::Vector6> SixSj(  data.kinematic_hessians.data()
                                                  + i * matrix_offset
                                                  + j * 6);

          Eigen::Map<Data::Motion::Vector6> SjxSi(  data.kinematic_hessians.data()
                                                  + j * matrix_offset
                                                  + i * 6);

          BOOST_CHECK(SixSj.isApprox(-SjxSi));
        }
      }
      else
      {
        Eigen::Map<Data::Motion::Vector6> SixSj(  data.kinematic_hessians.data()
                                                + i * matrix_offset
                                                + j * 6);

        Eigen::Map<Data::Motion::Vector6> SjxSi(  data.kinematic_hessians.data()
                                                + j * matrix_offset
                                                + i * 6);

        BOOST_CHECK(SixSj.isZero());
        BOOST_CHECK(SjxSi.isZero());
      }
    }
  }

  const double eps = 1e-8;
  Data::Matrix6x J_ref(6,model.nv), J_plus(6,model.nv);
  J_ref.setZero(); J_plus.setZero();

  computeJointJacobians(model,data_ref,q);
  VectorXd v_plus(VectorXd::Zero(model.nv));

  const Eigen::DenseIndex outer_offset = model.nv * 6;

  // WORLD
  getJointJacobian(model,data_ref,joint_id,WORLD,J_ref);
  Data::Tensor3x kinematic_hessian_world = getJointKinematicHessian(model,data,joint_id,WORLD);
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_plus[k] = eps;
    const VectorXd q_plus = integrate(model,q,v_plus);
    computeJointJacobians(model,data_plus,q_plus);
    J_plus.setZero();
    getJointJacobian(model,data_plus,joint_id,WORLD,J_plus);

    Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps;
    Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_world.data() + k * outer_offset,6,model.nv);

//    std::cout << "k: " << k << std::endl;
//    std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
//    std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
    BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps)));
    v_plus[k] = 0.;
  }

  // LOCAL_WORLD_ALIGNED
  computeJointJacobians(model,data_ref,q);
  getJointJacobian(model,data_ref,joint_id,LOCAL_WORLD_ALIGNED,J_ref);
  Data::Tensor3x kinematic_hessian_local_world_aligned = getJointKinematicHessian(model,data,joint_id,LOCAL_WORLD_ALIGNED);
  Data::Matrix3x dt_last_fd(3,model.nv);
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_plus[k] = eps;
    const VectorXd q_plus = integrate(model,q,v_plus);
    computeJointJacobians(model,data_plus,q_plus);
    J_plus.setZero();
    getJointJacobian(model,data_plus,joint_id,LOCAL_WORLD_ALIGNED,J_plus);

    SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];
    tMt_plus.rotation().setIdentity();

    dt_last_fd.col(k) = (data_plus.oMi[joint_id].translation() - data_ref.oMi[joint_id].translation())/eps;

    Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps;
    Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_local_world_aligned.data() + k * outer_offset,6,model.nv);

//    std::cout << "k: " << k << std::endl;
//    std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
//    std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
    BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps)));
    v_plus[k] = 0.;
  }

  Data::Matrix6x J_world(6,model.nv); J_world.setZero();
  getJointJacobian(model,data_ref,joint_id,LOCAL_WORLD_ALIGNED,J_world);

  BOOST_CHECK(dt_last_fd.isApprox(J_world.topRows<3>(),sqrt(eps)));

  // LOCAL
  computeJointJacobians(model,data_ref,q);
  getJointJacobian(model,data_ref,joint_id,LOCAL,J_ref);
  Data::Tensor3x kinematic_hessian_local = getJointKinematicHessian(model,data,joint_id,LOCAL);
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_plus[k] = eps;
    const VectorXd q_plus = integrate(model,q,v_plus);
    computeJointJacobians(model,data_plus,q_plus);
    J_plus.setZero();
    getJointJacobian(model,data_plus,joint_id,LOCAL,J_plus);

//    const SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];

    Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps;
    Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_local.data() + k * outer_offset,6,model.nv);

//    std::cout << "k: " << k << std::endl;
//    std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
//    std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
    BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps)));
    v_plus[k] = 0.;
  }
}

BOOST_AUTO_TEST_SUITE_END()


/*!
 * \file ProjBezierScheme.hpp
 * \author Jun Yoshida
 * \copyright (c) 2020 Jun Yoshida.
 * The project is released under the MIT License.
 * \date February 23, 2020: created
 */

#pragma once

#include <vector>

#include <Eigen/Dense>

#include "math/Bezier.hpp"
#include "math/BezierBox.hpp"
#include "PathScheme.hpp"
#include "ProjSpatialScheme.hpp"

template <class T>
class ProjBezierScheme
    : public ProjSpatialScheme<T>
{
    static_assert(std::is_base_of<T, PathScheme<Eigen::Vector2d>>::value,
                  "The base scheme must be derived from PathScheme<Eigen::Vector2d>.");

public:
    using typename PathScheme<Eigen::Vector3d>::PathElement;
    using typename ProjSpatialScheme<T>::BaseScheme;
    using typename ProjSpatialScheme<T>::vertex_type;
    enum : size_t { max_degree = 3 };

protected:
    template <class>
    struct BezVar_impl;

    template <size_t...Is>
    struct BezVar_impl<std::index_sequence<Is...>> {
        using type = BezierVariant<Eigen::Vector3d,Is...>;
    };

public:
    using BezierVarType
        = typename BezVar_impl<std::make_index_sequence<max_degree+1>>::type;

private:
    BezierBox m_bezv;
    Eigen::Vector3d m_lastpt;

public:
    using ProjSpatialScheme<T>::ProjSpatialScheme;

    virtual ~ProjBezierScheme() = default;

    //! Submit the buffered paths to the base scheme.
    //! The intersections of Bezier curves are considered.
    void submit() noexcept
    {
        auto divided_bez
            = m_bezv.project(
                ProjSpatialScheme<Eigen::Vector2d>::getDepthVec());
        for(auto& bez : divided_bez) {
            std::array<vertex_type,3> ctrlpt;
            if (bez.getActive() == 0) {
                ctrlpt[0] = bez.source();
            }
            else {
                std::copy(bez.begin()+1, bez.end(), std::begin(ctrlpt));
            }

            switch(bez.getActive()) {
            case 0:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::BeginPoint, ctrlpt);
                break;

            case 1:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::Line, ctrlpt);
                break;

            case 2:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::QBezier, ctrlpt);
                break;

            case 3:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::Bezier, ctrlpt);
                break;

            default:
                // Error
                break;
            }
        }
    }

    //! Submit the buffered paths to the base scheme.
    //! This draws the paths directly via ProjSpatialScheme.
    void submit_direct() noexcept
    {
        for(auto& bez : m_bezv) {
            std::array<vertex_type,3> ctrlpt;
            if (bez.getActive() == 0) {
                ctrlpt[0] = bez.source();
            }
            else {
                std::copy(bez.begin()+1, bez.end(), std::begin(ctrlpt));
            }

            switch(bez.getActive()) {
            case 0:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::BeginPoint, ctrlpt);
                break;

            case 1:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::Line, ctrlpt);
                break;

            case 2:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::QBezier, ctrlpt);
                break;

            case 3:
                ProjSpatialScheme<T>::putPathElement(
                    bord2::PathElemType::Bezier, ctrlpt);
                break;

            default:
                // Error
                break;
            }
        }

        ProjSpatialScheme<T>::stroke();
    }

protected:
    void putPathElement(PathElement const& elem) override
    {
        switch(elem.type) {
        case bord2::PathElemType::BeginPoint:
            m_bezv.append(
                Bezier<Eigen::Vector3d,0>(elem.v[0]));
            m_lastpt = elem.v[0];
            break;

        case bord2::PathElemType::Line:
            m_bezv.append(
                Bezier<Eigen::Vector3d,1>(m_lastpt, elem.v[0]));
            m_lastpt = elem.v[0];
            break;

        case bord2::PathElemType::QBezier:
            m_bezv.append(
                Bezier<Eigen::Vector3d,1>(
                    m_lastpt, elem.v[0], elem.v[1]));
            m_lastpt = elem.v[1];
            break;

        case bord2::PathElemType::Bezier:
            m_bezv.append(
                Bezier<Eigen::Vector3d,1>(
                    m_lastpt, elem.v[0], elem.v[1], elem.v[2]));
            m_lastpt = elem.v[2];
            break;

        default:
            // Error
            break;
        }
    }
};


#include "StdAfx.h"
#include "OptApp.h"
#include <boost/filesystem.hpp>
#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include "vertigo/vertex_switchLinear.h"
#include "vertigo/edge_switchPrior.h"
#include "vertigo/edge_se3Switchable.h"

typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> >  SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
typedef std::tr1::unordered_map<int, g2o::HyperGraph::Vertex*>     VertexIDMap;
typedef std::pair<int, g2o::HyperGraph::Vertex*> VertexIDPair;
typedef std::set<g2o::HyperGraph::Edge*> EdgeSet;

COptApp::COptApp(void)
{
}

COptApp::~COptApp(void)
{
}

bool COptApp::Init()
{
	namespace fs = boost::filesystem;
	if ( fs::exists( fs::path( odometry_log_file_ ) ) ) {
		odometry_traj_.LoadFromFile( odometry_log_file_ );
		if ( fs::exists( fs::path( odometry_info_file_ ) ) ) {
			odometry_info_.LoadFromFile( odometry_info_file_ );
		}
	}
	if ( fs::exists( fs::path( loop_log_file_ ) ) ) {
		loop_traj_.LoadFromFile( loop_log_file_ );
		if ( fs::exists( fs::path( loop_info_file_ ) ) ) {
			loop_info_.LoadFromFile( loop_info_file_ );
		}
	}
	pose_traj_.data_.clear();
	pose_traj_.data_.push_back( FramedTransformation( 0, 0, 1, Eigen::Matrix4d::Identity() ) );
	for ( int i = 0; i < ( int )odometry_traj_.data_.size(); i++ ) {
		pose_traj_.data_.push_back( FramedTransformation( i + 1, i + 1, i + 2, pose_traj_.data_[ i ].transformation_ * odometry_traj_.data_[ i ].transformation_ ) );
	}
	return ( odometry_traj_.data_.size() > 0 );
}

void COptApp::OptimizeSwitchable()
{
	struct SwitchableEdge {
	public:
		VertexSwitchLinear * v_;
		EdgeSwitchPrior * ep_;
		EdgeSE3Switchable * e_;
		FramedTransformation * t_;
	};

    g2o::SparseOptimizer* optimizer;
	optimizer = new g2o::SparseOptimizer();
	optimizer->setVerbose(true);
	SlamBlockSolver * solver = NULL;
	SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
	linearSolver->setBlockOrdering(false);
	solver = new SlamBlockSolver(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* algo = new g2o::OptimizationAlgorithmLevenberg(solver);
	optimizer->setAlgorithm(algo);

	std::vector< SwitchableEdge > switch_edge;

	Eigen::Matrix< double, 6, 6 > default_information;
	default_information = Eigen::Matrix< double, 6, 6 >::Identity();

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = new g2o::VertexSE3();
		v->setId( i );
		v->setEstimate( Eigen2G2O( pose_traj_.data_[ i ].transformation_ ) );
		if ( i == 0 ) {
			v->setFixed( true );
		}
		optimizer->addVertex( v );

		if ( i > 0 ) {
			g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3();
			g2o_edge->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i - 1 ));
			g2o_edge->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i ));
			g2o_edge->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( odometry_traj_.data_[ i - 1 ].transformation_ ) ) );
			if ( odometry_info_.data_.size() > 0 ) {
				g2o_edge->setInformation( odometry_info_.data_[ i - 1 ].information_ );
			} else {
				g2o_edge->setInformation( default_information );
			}
			optimizer->addEdge( g2o_edge );
		}
	}

	for ( int i = 0; i < ( int )loop_traj_.data_.size(); i++ ) {
		FramedTransformation & t = loop_traj_.data_[ i ];

		SwitchableEdge edge;
		edge.t_ = &t;

		edge.v_ = new VertexSwitchLinear();
		edge.v_->setId( optimizer->vertices().size() );
		edge.v_->setEstimate( 1.0 );
		optimizer->addVertex( edge.v_ );

		edge.ep_ = new EdgeSwitchPrior();
		edge.ep_->vertices()[0] = edge.v_;
		edge.ep_->setMeasurement( 1.0 );
		edge.ep_->setInformation( Eigen::Matrix<double,1,1>::Identity() * weight_ );
		optimizer->addEdge( edge.ep_ );

		edge.e_ = new EdgeSE3Switchable();
		edge.e_->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id1_ ));
		edge.e_->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id2_ ));
		edge.e_->vertices()[2] = edge.v_;
		edge.e_->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( t.transformation_ ) ) );
		if ( loop_info_.data_.size() > 0 ) {
			edge.e_->setInformation( loop_info_.data_[ i ].information_ );
		} else {
			edge.e_->setInformation( default_information );
		}
		optimizer->addEdge( edge.e_ );
		switch_edge.push_back( edge );
	}

	optimizer->initializeOptimization();
	optimizer->optimize( max_iteration_ );

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = dynamic_cast< g2o::VertexSE3 * >( optimizer->vertex( i ) );
		pose_traj_.data_[ i ].transformation_ = G2O2Matrix4d( v->estimateAsSE3Quat() );
	}
	pose_traj_.SaveToFile( pose_log_file_ );

	loop_remain_traj_.data_.clear();
	for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
		SwitchableEdge & edge = switch_edge[ i ];
		if ( edge.v_->estimate() > 0.5 ) {
			loop_remain_traj_.data_.push_back( loop_traj_.data_[ i ] );
		}
	}
	loop_remain_traj_.SaveToFile( loop_remain_log_file_ );

	refine_traj_.data_.clear();
	for ( int i = 0; i < ( int )odometry_traj_.data_.size(); i++ ) {
		refine_traj_.data_.push_back( odometry_traj_.data_[ i ] );
	}
	for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
		SwitchableEdge & edge = switch_edge[ i ];
		if ( edge.v_->estimate() > 0.5 && loop_traj_.data_[ i ].id1_ + 1 < loop_traj_.data_[ i ].id2_ ) {
			refine_traj_.data_.push_back( loop_traj_.data_[ i ] );
		}
	}
	refine_traj_.SaveToFile( refine_log_file_ );
}

void COptApp::OptimizeEM()
{
	struct SwitchableEdge {
	public:
		double weight_;
		g2o::EdgeSE3 * e_;
		FramedTransformation * t_;
	};

	g2o::SparseOptimizer* optimizer;
	optimizer = new g2o::SparseOptimizer();
	optimizer->setVerbose(true);
	SlamBlockSolver * solver = NULL;
	SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
	linearSolver->setBlockOrdering(false);
	solver = new SlamBlockSolver(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* algo = new g2o::OptimizationAlgorithmLevenberg(solver);
	optimizer->setAlgorithm(algo);

	std::vector< SwitchableEdge > switch_edge;

	Eigen::Matrix< double, 6, 6 > default_information;
	default_information = Eigen::Matrix< double, 6, 6 >::Identity();

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = new g2o::VertexSE3();
		v->setId( i );
		v->setEstimate( Eigen2G2O( pose_traj_.data_[ i ].transformation_ ) );
		if ( i == 0 ) {
			v->setFixed( true );
		}
		optimizer->addVertex( v );

		if ( i > 0 ) {
			g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3();
			g2o_edge->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i - 1 ));
			g2o_edge->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i ));
			g2o_edge->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( odometry_traj_.data_[ i - 1 ].transformation_ ) ) );
			if ( odometry_info_.data_.size() > 0 ) {
				g2o_edge->setInformation( odometry_info_.data_[ i - 1 ].information_ );
			} else {
				g2o_edge->setInformation( default_information );
			}
			optimizer->addEdge( g2o_edge );
		}
	}

	for ( int i = 0; i < ( int )loop_traj_.data_.size(); i++ ) {
		FramedTransformation & t = loop_traj_.data_[ i ];
		SwitchableEdge edge;
		edge.t_ = &t;
		edge.weight_ = 0.0;

		edge.e_ = new g2o::EdgeSE3();
		edge.e_->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id1_ ));
		edge.e_->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id2_ ));
		edge.e_->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( t.transformation_ ) ) );
		if ( loop_info_.data_.size() > 0 ) {
			edge.e_->setInformation( loop_info_.data_[ i ].information_ );
		} else {
			edge.e_->setInformation( default_information );
		}
		optimizer->addEdge( edge.e_ );
		switch_edge.push_back( edge );
	}

	for ( int itr = 0; itr < max_iteration_; itr++ ) {
		// E step
		for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
			SwitchableEdge & edge = switch_edge[ i ];
			if ( loop_info_.data_.size() > 0 ) {
				edge.e_->setInformation( loop_info_.data_[ i ].information_ );
			} else {
				edge.e_->setInformation( default_information );
			}
			edge.e_->computeError();
			edge.weight_ = ( weight_ * weight_ ) / ( weight_ * weight_ + switch_edge[ i ].e_->chi2() );

			if ( loop_info_.data_.size() > 0 ) {
				edge.e_->setInformation( loop_info_.data_[ i ].information_ * sqrt( edge.weight_ ) );
			} else {
				edge.e_->setInformation( default_information * sqrt( edge.weight_ ) );
			}
		}

		// M step
		optimizer->initializeOptimization();
		optimizer->optimize( 1 );
	}

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = dynamic_cast< g2o::VertexSE3 * >( optimizer->vertex( i ) );
		pose_traj_.data_[ i ].transformation_ = G2O2Matrix4d( v->estimateAsSE3Quat() );
	}
	pose_traj_.SaveToFile( pose_log_file_ );

	loop_remain_traj_.data_.clear();
	for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
		SwitchableEdge & edge = switch_edge[ i ];
		if ( edge.weight_ > 0.25 ) {
			loop_remain_traj_.data_.push_back( loop_traj_.data_[ i ] );
		}
	}
	loop_remain_traj_.SaveToFile( loop_remain_log_file_ );
}


#include "Object.h"
#include "Camera.h"
#include "Light.h"
#include "read_json.h"
#include "write_ppm.h"
#include "viewing_ray.h"
#include "raycolor.h"
#include <Eigen/Core>
#include <vector>
#include <iostream>
#include <memory>
#include <limits>
#include <functional>

int main(int argc, char * argv[])
{
  Camera camera;
  std::vector< std::shared_ptr<Object> > objects;
  std::vector< std::shared_ptr<Light> > lights;
  // Read a camera and scene description from given .json file
  read_json(
    // argc<=1?"../data/sphere-and-plane.json":argv[1],
    argc<=1?"../src/creative.json":argv[1],
    camera,
    objects,
    lights);

  int width =  640;
  int height = 360;
  std::vector<unsigned char> rgb_image(3*width*height);
  // For each pixel (i,j)
  for(unsigned i=0; i<height; ++i)
  {
    for(unsigned j=0; j<width; ++j)
    {
      // Set background color
      Eigen::Vector3d rgb(0,0,0);

      // Compute viewing ray
      Ray ray;
      viewing_ray(camera,i,j,width,height,ray);

      // Shoot ray and collect color
      raycolor(ray,1.0,objects,lights,0,rgb);

      // Write double precision color into image
      auto clamp = [](double s) {
        return std::max(std::min(s,1.0),0.0);
      };
      rgb_image[0+3*(j+width*i)] = 255.0*clamp(rgb(0));
      rgb_image[1+3*(j+width*i)] = 255.0*clamp(rgb(1));
      rgb_image[2+3*(j+width*i)] = 255.0*clamp(rgb(2));

    }
  }

  write_ppm("rgb.ppm",rgb_image,width,height,3);
}


/*
 * Copyright 2009-2011 The VOTCA Development Team (http://www.votca.org)
 *
 * 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 <votca/tools/akimaspline.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <votca/tools/linalg.h>
#include <iostream>

namespace votca { namespace tools {

using namespace std;

void AkimaSpline::Interpolate(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in AkimaSpline::Interpolate : sizes of vectors x and y do not match");

    // Akima splines require at least 4 points
    if(x.size()<4)
        throw std::invalid_argument("error in AkimaSpline::Interpolate : vectors x and y have to contain at least 4 points");

    const int N = x.size();

    // adjust the grid
    _r.resize(N);

    // copy the grid points into f
    _r = x;

    // initialize vectors p1,p2,p3,p4 and t
    p0 = ub::zero_vector<double>(N);
    p1 = ub::zero_vector<double>(N);
    p2 = ub::zero_vector<double>(N);
    p3 = ub::zero_vector<double>(N);
    t = ub::zero_vector<double>(N);

    double m1,m2,m3,m4;

    double temp, g0, g1, g2, x1, x2, y1, y2, x4, x5, y4, y5, m5;

    // boundary conditions
    // >> determine t(0), t(1) and t(N-2), t(N-1)
    switch(_boundaries) {
        case splineNormal:
            // Akima method: estimation of two more points on each side using a
            // degree two polyomial
            // resulting slopes t(0), t(1), t(N-2), t(N-1) are directly calculated

            // left side: t(0), t(1)
            temp = (x(1)-x(0))/(x(2)-x(0));
            temp = temp*temp;
            g0 = y(0);
            g1 = ( (y(1)-y(0)) - temp*(y(2)-y(0)) ) / ( (x(1)-x(0)) - temp*(x(2)-x(0)) );
            g2 = ( (y(2)-y(0)) - g1*(x(2)-x(0)) ) / ( (x(2)-x(0))*(x(2)-x(0)) );
            x1 = x(0) - (x(2)-x(0));
            x2 = x(1) - (x(2)-x(0));
            y1 = g0 + g1*(x1-x(0)) + g2*(x1-x(0))*(x1-x(0));
            y2 = g0 + g1*(x2-x(0)) + g2*(x2-x(0))*(x2-x(0));
            m1 = (y2-y1)/(x2-x1);
            m2 = (y(0)-y2)/(x(0)-x2);
            m3 = (y(1)-y(0))/(x(1)-x(0));
            m4 = (y(2)-y(1))/(x(2)-x(1));
            t(0) = getSlope(m1,m2,m3,m4);
            m5 = (y(3)-y(2))/(x(3)-x(2));
            t(1) = getSlope(m2,m3,m4,m5);

            // right side: t(N-2), t(N-1)
            temp = (x(N-2)-x(N-1))/(x(N-3)-x(N-1));
            temp = temp*temp;
            g0 = y(N-1);
            g1 = ( (y(N-2)-y(N-1)) - temp*(y(N-3)-y(N-1)) ) / ( (x(N-2)-x(N-1)) - temp*(x(N-3)-x(N-1)) );
            g2 = ( (y(N-3)-y(N-1)) - g1*(x(N-3)-x(N-1)) ) / ( (x(N-3)-x(N-1))*(x(N-3)-x(N-1)) );
            x4 = x(N-2) + (x(N-1)-x(N-3));
            x5 = x(N-1) + (x(N-1)-x(N-3));
            y4 = g0 + g1*(x4-x(N-1)) + g2*(x4-x(N-1))*(x4-x(N-1));
            y5 = g0 + g1*(x5-x(N-1)) + g2*(x5-x(N-1))*(x5-x(N-1));
            m1 = (y(N-3)-y(N-4))/(x(N-3)-x(N-4));
            m2 = (y(N-2)-y(N-3))/(x(N-2)-x(N-3));
            m3 = (y(N-1)-y(N-2))/(x(N-1)-x(N-2));
            m4 = (y4-y(N-1))/(x4-x(N-1));
            m5 = (y5-y4)/(x5-x4);
            t(N-2) = getSlope(m1,m2,m3,m4);
            t(N-1) = getSlope(m2,m3,m4,m5);
            break;
        case splinePeriodic:
            // left: last two points determine the slopes t(0), t(1)
            m1 = (y(N-1)-y(N-2))/(x(N-1)-x(N-2));
            m2 = (y(0)-y(N-1))/(x(0)-x(N-1));
            m3 = (y(1)-y(0))/(x(1)-x(0));
            m4 = (y(2)-y(1))/(x(2)-x(1));
            m5 = (y(3)-y(2))/(x(3)-x(2));
            t(0) = getSlope(m1,m2,m3,m4);
            t(1) = getSlope(m2,m3,m4,m5);
            // right: first two points determine the slopes t(N-2), t(N-1)
            m1 = (y(N-3)-y(N-4))/(x(N-3)-x(N-4));
            m2 = (y(N-2)-y(N-3))/(x(N-2)-x(N-3));
            m3 = (y(N-1)-y(N-2))/(x(N-1)-x(N-2));
            m4 = (y(0)-y(N-1))/(x(0)-x(N-1));
            m5 = (y(1)-y(0))/(x(1)-x(0));
            t(N-2) = getSlope(m1,m2,m3,m4);
            t(N-1) = getSlope(m2,m3,m4,m5);
            break;
        case splineDerivativeZero:
	    throw std::runtime_error("erro in AkimaSpline::Interpolate: case splineDerivativeZero not implemented yet");
	    break;
    }

    // calculate t's for all inner points [2,N-3]
    for (int i=2; i<N-2; i++) {
        m1 = (y(i-1)-y(i-2))/(x(i-1)-x(i-2));
        m2 = (y(i)-y(i-1))/(x(i)-x(i-1));
        m3 = (y(i+1)-y(i))/(x(i+1)-x(i));
        m4 = (y(i+2)-y(i+1))/(x(i+2)-x(i+1));
        t(i) = getSlope(m1,m2,m3,m4);
    }

    // calculate p0,p1,p2,p3 for all intervals 0..(N-2), where interval
    // [x(i),x(i+1)] shall have number i (this means that the last interval
    // has number N-2)
    for (int i=0; i<N-1; i++) {
        p0(i) = y(i);
        p1(i) = t(i);
        p2(i) = ( 3.0*(y(i+1)-y(i))/(x(i+1)-x(i)) - 2.0*t(i) - t(i+1) ) / (x(i+1)-x(i));
        p3(i) = ( t(i) + t(i+1) - 2.0*(y(i+1)-y(i))/(x(i+1)-x(i)) ) / ( (x(i+1)-x(i))*(x(i+1)-x(i)) );
    }
}

void AkimaSpline::Fit(ub::vector<double> &x, ub::vector<double> &y)
{
    throw std::runtime_error("Akima fit not implemented.");
}

}}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file analyticpdfhestonengine.cpp
    \brief Analytic engine for arbitrary European payoffs under the Heston model
*/

#include <ql/math/functional.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/experimental/exoticoptions/analyticpdfhestonengine.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#include <boost/bind.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

#include <cmath>
#include <complex>

namespace QuantLib {

    namespace {
        struct HestonParams {
            Real v0, kappa, theta, sigma, rho;
        };

        HestonParams getHestonParams(
            const boost::shared_ptr<HestonModel>& model) {
            const HestonParams p = { model->v0(),    model->kappa(),
                                     model->theta(), model->sigma(),
                                     model->rho() };
            return p;
        }

        std::complex<Real> gamma(const HestonParams& p, Real p_x) {
            return std::complex<Real>(p.kappa, p.rho*p.sigma*p_x);
        }

        std::complex<Real> omega(const HestonParams& p, Real p_x) {
           const std::complex<Real> g = gamma(p, p_x);
           return std::sqrt(g*g
                  + p.sigma*p.sigma*std::complex<Real>(p_x*p_x, -p_x));
        }

        class CpxPv_Helper
            : public std::unary_function<Real, Real > {
          public:
            CpxPv_Helper(const HestonParams& p, Real x, Time t)
              : p_(p), t_(t), x_(x),
                c_inf_(std::min(10.0, std::max(0.0001,
                      std::sqrt(1.0-square<Real>()(p_.rho))/p_.sigma))
                      *(p_.v0 + p_.kappa*p_.theta*t))  {}

            Real operator()(Real x) const {
                return std::real(transformPhi(x));
            }

            Real p0(Real p_x) const {
                if (p_x < QL_EPSILON) {
                    return 0.0;
                }

                const Real u_x = std::max(QL_EPSILON, -std::log(p_x)/c_inf_);
                return std::real(phi(u_x)
                        *std::exp(std::complex<Real>(0.0, -2*u_x*x_))
                        /((p_x*c_inf_)*std::complex<Real>(0.0, u_x)));
            }

          private:
            std::complex<Real> transformPhi(Real x) const {
                if (x < QL_EPSILON) {
                    return std::complex<Real>(0.0, 0.0);
                }

                const Real u_x = -std::log(x)/c_inf_;
                return phi(u_x)/(x*c_inf_);
            }

            std::complex<Real> phi(Real p_x) const {
                const Real sigma2 = p_.sigma*p_.sigma;
                const std::complex<Real> g = gamma(p_, p_x);
                const std::complex<Real> o = omega(p_, p_x);
                const std::complex<Real> gamma = (g-o)/(g+o);

                return 2.0*std::exp(std::complex<Real>(0.0, p_x*x_)
                        - p_.v0*std::complex<Real>(p_x*p_x, -p_x)
                          /(g+o*(1.0+std::exp(-o*t_))/(1.0-std::exp(-o*t_)))
                         +p_.kappa*p_.theta/sigma2*(
                           (g-o)*t_ - 2.0*std::log((1.0-gamma*std::exp(-o*t_))
                                                               /(1.0-gamma))));
            }

            const HestonParams& p_;
            const Time t_;
            const Real x_, c_inf_;
        };
    }

    AnalyticPDFHestonEngine::AnalyticPDFHestonEngine(
        const boost::shared_ptr<HestonModel>& model,
        Real gaussLobattoEps,
        Size gaussLobattoIntegrationOrder)
    : gaussLobattoIntegrationOrder_(gaussLobattoIntegrationOrder),
      gaussLobattoEps_(gaussLobattoEps),
      model_(model) {  }

    void AnalyticPDFHestonEngine::calculate() const {
        // this is an European option pricer
        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Time t = process->time(arguments_.exercise->lastDate());

        const Real xMax = 8.0 * std::sqrt(process->theta()*t
            + (process->v0() - process->theta())
                *(1-std::exp(-process->kappa()*t))/process->kappa());

        results_.value = GaussLobattoIntegral(
            gaussLobattoIntegrationOrder_, gaussLobattoEps_)(
            boost::bind(&AnalyticPDFHestonEngine::weightedPayoff, this,_1, t),
                         -xMax, xMax);
    }

    Real AnalyticPDFHestonEngine::Pv(Real x_t, Time t) const {
        return GaussLobattoIntegral(
            gaussLobattoIntegrationOrder_, 0.1*gaussLobattoEps_)(
                CpxPv_Helper(getHestonParams(model_), x_t, t),
                0.0, 1.0)/M_TWOPI;
    }

    Real AnalyticPDFHestonEngine::cdf(Real s, Time t) const {
        const boost::shared_ptr<HestonProcess>& process = model_->process();
        const DiscountFactor d=  process->riskFreeRate()->discount(t)
                               / process->dividendYield()->discount(t);

        const Real s_t = process->s0()->value()/d;
        const Real x = std::log(s_t/s);

        return GaussLobattoIntegral(
            gaussLobattoIntegrationOrder_, gaussLobattoEps_)(
                boost::bind(&CpxPv_Helper::p0,
                    CpxPv_Helper(getHestonParams(model_), x, t), _1),
                0.0, 1.0)/M_TWOPI + 0.5;
    }

    Real AnalyticPDFHestonEngine::weightedPayoff(Real x_t, Time t) const {
        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Real s_0 = process->s0()->value();
        const DiscountFactor rD = process->riskFreeRate()->discount(t);
        const DiscountFactor dD = process->dividendYield()->discount(t);

        const Real s_t = s_0*std::exp(x_t)*dD/rD;
        const Real payoff = (*arguments_.payoff)(s_t);

        return (payoff != 0.0) ? payoff*Pv(x_t, t)*rD : 0.0;
    }
}


#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <unordered_map>
#include <functional>
#include <Eigen/Dense>
#include "../include/sample_network.h"
#include "../include/numerical_gradient.h"

namespace MyDL{

    using namespace Eigen;
    using std::shared_ptr;
    using std::make_shared;
    using std::string;
    using std::vector;
    using std::unordered_map;
    using std::cout;
    using std::endl;

    TwoLayerNet::TwoLayerNet(int input_size, int hidden_size, int output_size, double weight_init_std)
    {
        // 内部保持パラメータ
        _input_size = input_size;
        _hidden_size = hidden_size;
        _output_size = output_size;
        _weight_init_std = weight_init_std;

        // Affine Layer用のパラメータ -> スマートポインタで保持し、それをパラメータのリストに格納
        auto W1 = make_shared<MatrixXd>(input_size, hidden_size);
        auto W2 = make_shared<MatrixXd>(hidden_size, output_size);
        auto b1 = make_shared<MatrixXd>(1, hidden_size);
        auto b2 = make_shared<MatrixXd>(1, output_size);

        *W1 = weight_init_std * MatrixXd::Random(input_size, hidden_size);
        *W2 = weight_init_std * MatrixXd::Random(hidden_size, output_size);
        *b1 = MatrixXd::Zero(1, hidden_size);
        *b2 = MatrixXd::Zero(1, output_size);

        // Layer作成 -> スマートポインタで実装(コンストラクタを抜けた時に、実体が消されないようにするため)
        // 左辺の型はautoにしてはいけない(BaseLayerで統一し、コンテナに格納する ※ポリモーフィズムの実現)
        shared_ptr<BaseLayer> affine1    = make_shared<MyDL::Affine>(W1, b1);
        shared_ptr<BaseLayer> affine2    = make_shared<MyDL::Affine>(W2, b2);
        shared_ptr<BaseLayer> relu1      = make_shared<ReLU>();
        shared_ptr<BaseLayer> last_layer = make_shared<SoftmaxWithLoss>(); // predictではaffine2の出力、lossではlastlayerの出力を使うので分ける

        // 生のポインタを格納すると実体がスコープ外となり解放されてしまうので、shared_ptrで対処
        _layers["Affine1"] = affine1;
        _layers["ReLU1"] = relu1;
        _layers["Affine2"] = affine2;
        _last_layer = last_layer;

        // 各パラメータへのポインタを格納
        if (auto cast_affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            params["W1"] = cast_affine1->pW;
            params["b1"] = cast_affine1->pb;
        }
        if (auto cast_affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            params["W2"] = cast_affine2->pW;
            params["b2"] = cast_affine2->pb;
        }

        // unordered_mapでは追加順が保存されないので、別途順番通り名称を格納したコンテナを用意
        _layer_list.push_back("Affine1");
        _layer_list.push_back("BatchNorm"); // for batchnorm debug 21/03/21追加
        _layer_list.push_back("ReLU1");
        _layer_list.push_back("Affine2");

        // ------------------------------------------------
        // for batchnorm debug 21/03/21追加
        // ------------------------------------------------
        auto gamma = make_shared<MatrixXd>(1, hidden_size);
        auto beta = make_shared<MatrixXd>(1, hidden_size);
        *gamma = weight_init_std * MatrixXd::Random(1, hidden_size);
        *beta = MatrixXd::Zero(1, hidden_size);
        shared_ptr<BaseLayer> batch_norm = make_shared<BatchNorm>(gamma, beta);
        _layers["BatchNorm"] = batch_norm;
        if (auto cast_batchnorm = std::dynamic_pointer_cast<BatchNorm>(_layers["BatchNorm"]))
        {
            params["gamma"] = cast_batchnorm->pgamma;
            params["beta"]  = cast_batchnorm->pbeta;
        }
    }

    vector<MatrixXd> TwoLayerNet::predict(vector<MatrixXd> inputs)
    {
        // inputのバリデーションをしておくか？
        vector<MatrixXd> X = inputs;// 入力もvectorなので、そのまま受ければOK
        vector<MatrixXd> tmp_X;

        // mapのrange-forは内部的にstd::pairが返される
        for(auto layer : _layer_list)
        {
            // cout << layer << endl;
            tmp_X = _layers[layer]->forward(X);
            X.swap(tmp_X); // 中身入れ替え
        }
        return X;
    }

    vector<MatrixXd> TwoLayerNet::loss(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> pred_input, pred_out, loss_inputs, loss_output;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        loss_inputs = inputs;
        loss_inputs[0] = pred_out[0];

        loss_output = _last_layer->forward(loss_inputs);
        return loss_output;
    }

    double TwoLayerNet::accuracy(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> pred_input, pred_out;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        MatrixXd y, t;
        y = pred_out[0];
        t = inputs[1];
        double batch_size = t.rows();
        double accuracy = 0;

        MatrixXd::Index y_row, y_col, t_row, t_col;
        for (int i = 0; i < batch_size; i++)
        {
            y.row(i).maxCoeff(&y_row, &y_col);
            t.row(i).maxCoeff(&t_row, &t_col);

            accuracy += (double)(y_col == t_col);
        }

        return accuracy / batch_size;
    }

    unordered_map<string, MatrixXd> TwoLayerNet::gradient(vector<MatrixXd> inputs)
    {
        // Forward
        vector<MatrixXd> output;
        output = loss(inputs); // forward -> 逆伝播計算に必要な情報を各レイヤにキャッシュ

        // Backward
        vector<MatrixXd> dout, tmp_dout;
        dout.push_back(MatrixXd::Ones(1,1));

        dout = _last_layer->backward(dout);

        // 逆順ループ → Boostライブラリのboost::adaptors::reverse()を使う方がEasyではある
        for (auto it = _layer_list.rbegin(); it != _layer_list.rend(); it++)
        {
            string layer = *it;
            tmp_dout = _layers[layer]->backward(dout);
            dout.swap(tmp_dout);
        }

        unordered_map<string, MatrixXd> grads;
        // _layersに格納している変数はBaseLayerにアップキャストしているので、ダウンキャストが必要 -> nullptrのときは実行しないようにする
        if(shared_ptr<MyDL::Affine> affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            grads["W1"] = affine1->dW;
            grads["b1"] = affine1->db;
        }
        if(shared_ptr<MyDL::Affine> affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            grads["W2"] = affine2->dW;
            grads["b2"] = affine2->db;
        }

        // for batchnorm debug 21/03/21追加
        if(auto batchnorm = std::dynamic_pointer_cast<BatchNorm>(_layers["BatchNorm"]))
        {
            grads["gamma"] = batchnorm->dgamma;
            grads["beta"]  = batchnorm->dbeta;
        }

        return grads;
    }

    unordered_map<string, MatrixXd> TwoLayerNet::numerical_gradient(vector<MatrixXd> inputs)
    {
        // [&]は、スコープ外の変数を参照するというキャプチャー(ここではthisポインタを使うために指定)
        std::function<vector<MatrixXd>(MatrixXd)> loss_W = [this, &inputs](MatrixXd W) -> vector<MatrixXd> { return this->loss(inputs); };
        std::function<vector<MatrixXd>(VectorXd)> loss_W2 = [this, &inputs](VectorXd W) -> vector<MatrixXd> { return this->loss(inputs); };
        unordered_map<string, MatrixXd> grads;

        MatrixXd dW1, dW2, db1, db2;

        // 直接内部のレイヤのパラメータにアクセスするので、ダウンキャストが必要
        if (shared_ptr<MyDL::Affine> affine1 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine1"]))
        {
            dW1 = MyDL::numerical_gradient(loss_W, affine1->_W);
            db1 = MyDL::numerical_gradient(loss_W2, affine1->_b);
        }
        if (shared_ptr<MyDL::Affine> affine2 = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine2"]))
        {
            dW2 = MyDL::numerical_gradient(loss_W, affine2->_W);
            db2 = MyDL::numerical_gradient(loss_W2, affine2->_b);
        }

        grads["dW1"] = dW1;
        grads["dW2"] = dW2;
        grads["db1"] = db1;
        grads["db2"] = db2;

        return grads;
    }

    // -----------------------------------------------------------
    // MultiLayerNet: Weight Decay の検証用
    // 【21/04/06】
    // とりあえず動作することを目指すので、最初はreluで構成
    // 後ほどsigmoid含めて動くように変更。初期値も XavierとHeの両方を選択できるようにする。
    // -----------------------------------------------------------

    MultiLayerNet::MultiLayerNet(const int input_size,
                                 const vector<int> hidden_size,
                                 const int output_size,
                                 const double weight_decay_lambda)
    {
        _input_size = input_size;
        _hidden_size_list = hidden_size;
        _output_size = output_size;
        _weight_decay_lambda = weight_decay_lambda;

        // 各パラメータの初期化

        _layers["Affine1"] = make_shared<MyDL::Affine>(_input_size, _hidden_size_list[0]);
        _layers["ReLU1"] = make_shared<ReLU>();
        _layer_list.push_back("Affine1");
        _layer_list.push_back("ReLU1");
        for (int i=0; i<_hidden_size_list.size()-1; i++)
        {
            string tmp_num_str = std::to_string(i+2);
            _layers["Affine" + tmp_num_str] = make_shared<MyDL::Affine>(_hidden_size_list[i], _hidden_size_list[i + 1]);
            _layers["ReLU" + tmp_num_str] = make_shared<ReLU>();
            _layer_list.push_back("Affine" + tmp_num_str);
            _layer_list.push_back("ReLU" + tmp_num_str);
        }
        string tmp_num_str = std::to_string(_hidden_size_list.size()+1);
        _layers["Affine" + tmp_num_str] = make_shared<MyDL::Affine>(_hidden_size_list[_hidden_size_list.size() - 1], _output_size);
        _layers["ReLU" + tmp_num_str] = make_shared<ReLU>();
        _layer_list.push_back("Affine" + tmp_num_str);
        _layer_list.push_back("ReLU" + tmp_num_str);

        _last_layer = make_shared<SoftmaxWithLoss>(); // Loss Layer

        for (int layer_num = 1; layer_num <= _hidden_size_list.size()+1; layer_num++)
        {
            if (auto cast_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine" + std::to_string(layer_num)]))
            {
                params["W" + std::to_string(layer_num)] = cast_affine->pW;
                params["b" + std::to_string(layer_num)] = cast_affine->pb;
            }
        }

    }

    vector<MatrixXd> MultiLayerNet::predict(vector<MatrixXd> inputs)
    {
        vector<MatrixXd> X = inputs;
        vector<MatrixXd> tmp_X;

        for (auto layer : _layer_list)
        {
            tmp_X = _layers[layer]->forward(X);
            X.swap(tmp_X);
        }
        return X;
    }

    vector<MatrixXd> MultiLayerNet::loss(vector<MatrixXd> inputs, MatrixXd& t)
    {
        vector<MatrixXd> pred_input, pred_out, loss_inputs, loss_output;
        pred_input.push_back(inputs[0]);
        pred_out = predict(pred_input);

        loss_inputs.push_back(pred_out[0]);
        loss_inputs.push_back(t);

        loss_output = _last_layer->forward(loss_inputs);

        // あとは Weight Decay の項も計算してLossに加える
        double weight_decay = 0;
        for (auto param: params)
        {
            weight_decay += 0.5 * _weight_decay_lambda * (*(param.second)).sum();
        }

        loss_output[0](0) = loss_output[0](0) + weight_decay;

        return loss_output;
    }

    double MultiLayerNet::accuracy(vector<MatrixXd> inputs, MatrixXd& t)
    {
        vector<MatrixXd> pred_out;
        pred_out = predict(inputs);

        MatrixXd y;
        y = pred_out[0];
        double batch_size = t.rows();
        double accuracy = 0;

        MatrixXd::Index y_row, y_col, t_row, t_col;
        for (int i = 0; i < batch_size; i++)
        {
            y.row(i).maxCoeff(&y_row, &y_col);
            t.row(i).maxCoeff(&t_row, &t_col);

            accuracy += (double)(y_col == t_col);
        }

        return accuracy / batch_size;
    }

    unordered_map<string, MatrixXd> MultiLayerNet::gradient(vector<MatrixXd> inputs, MatrixXd& t)
    {
        // Forward
        vector<MatrixXd> output;
        output = loss(inputs, t);

        // Backward
        vector<MatrixXd> dout, tmp_dout;
        dout.push_back(MatrixXd::Ones(1,1));

        dout = _last_layer->backward(dout);

        for (auto it = _layer_list.rbegin(); it != _layer_list.rend(); it++)
        {
            string layer = *it;
            tmp_dout = _layers[layer]->backward(dout);
            dout.swap(tmp_dout);
        }

        unordered_map<string, MatrixXd> grads;

        for (int i = 1; i <= _hidden_size_list.size()+1; i++)
        {
            if(auto tmp_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine" + std::to_string(i)]))
            {
                grads["W" + std::to_string(i)] = tmp_affine->dW + _weight_decay_lambda * (*(tmp_affine->pW));
                grads["b" + std::to_string(i)] = tmp_affine->db;
            }
        }

        return grads;
    }

    // -----------------------------------------------------------
    // AffineLayer デバッグ用クラス → 参照先の更新確認
    // -----------------------------------------------------------
    DebugAffine::DebugAffine(int input_size, int output_size, double weight_init_std)
    {
        _input_size = input_size;
        _output_size = output_size;

        MatrixXd W = weight_init_std * MatrixXd::Random(input_size, output_size);
        MatrixXd b = MatrixXd::Zero(1, output_size);

        shared_ptr<BaseLayer> affine = std::make_shared<MyDL::Affine>(W, b);
        _layers["Affine"] = affine;

        if(shared_ptr<MyDL::Affine> tmp_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine"]))
        {
            params["W"] = tmp_affine->_W; // ここではコピーコンストラクタが走る
            params["b"] = tmp_affine->_b;
        }
    }

    void DebugAffine::PrintLayerParams(void)
    {
        if (shared_ptr<MyDL::Affine> tmp_affine = std::dynamic_pointer_cast<MyDL::Affine>(_layers["Affine"]))
        {
            cout << "--- parameter W ---" << endl;
            cout << tmp_affine->_W << endl;
            cout << "--- parameter b ---" << endl;
            cout << tmp_affine->_b << endl;
        }
    }

}


#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <math.h>
#include <time.h>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <math.h>
#include <time.h>
#include <fstream>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>
#include <linux/input-event-codes.h>

#include "origarm_ros/Command_Position.h"
#include "origarm_ros/Command_ABL.h"
#include "origarm_ros/modenumber.h"
#include "origarm_ros/segnumber.h"
#include "origarm_ros/keynumber.h"
#include "ros/package.h"
#include "myGlobalData.h"

#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))

using namespace std;

ifstream inFile;
string GivenDataFilePath = ros::package::getPath("origarm_ros") + "/predefined_param/";
string GivenDataFileName = "repeat_1_0.txt";

const int ms = 1000;  //1ms
int ts[] = {5*ms, 10*ms, 60*ms, 10*ms};

int mode_in;
float x_in, y_in, z_in;
float a_in[6], b_in[6], l_in[6];

int flag_start = 0;
int period = 175;
int repeat = 12;  // whole repeat times
int num = 0;     // smallest step number
int loop_no = 0; // to which step exactly

vector<vector<float>> s_A;
vector<vector<float>> s_B;
vector<vector<float>> s_L;

vector<vector<float>> state_A;
vector<vector<float>> state_B;
vector<vector<float>> state_L;

vector<float> alpha {0, 0, 0, 0, 0, 0};
vector<float> beta  {0, 0, 0, 0, 0, 0};
vector<float> length{g_length0, g_length0, g_length0, g_length0, g_length0, g_length0};

void keyCallback(const origarm_ros::keynumber &key)
{
	if (key.keycodePressed == KEY_B) // break and reset all command pressure to 0
	{
		printf("KEY_B pressed!\r\n");
		flag_start = 0;
		repeat = 12;
	}
	else if (key.keycodePressed == KEY_J) // same as saving button
	{
		printf("KEY_J pressed!\r\n");
		flag_start = 1;
	}
}

//generate single point by defining starting and ending point
static float lineardiffgenetraj(float ps, float pe, int step, int tstep)
{
	float pm = ps + step*(pe-ps)/(tstep-1);
	return pm;
}

//read state information from File
static void readFromFile()
{
	vector<float> s_a;
	vector<float> s_b;
	vector<float> s_l;
	vector<float> s_a1;
	vector<float> s_b1;
	vector<float> s_l1;

	inFile.open(GivenDataFilePath + GivenDataFileName, ios::in);

	if (!inFile)
	{
		printf("%s\n", "unable to open the file.");
		exit(1);
	}
	else
	{
		while (true)
		{
			inFile>>mode_in>>x_in>>y_in>>z_in>>a_in[0]>>b_in[0]>>l_in[0]>>a_in[1]>>b_in[1]>>l_in[1]>>a_in[2]>>b_in[2]>>l_in[2]>>a_in[3]>>b_in[3]>>l_in[3]>>a_in[4]>>b_in[4]>>l_in[4]>>a_in[5]>>b_in[5]>>l_in[5];

			if ( inFile.eof() )
			{
				break;
			}

			for (int i = 0; i < 6; i++)
			{
				s_a.push_back(a_in[i]);
				s_b.push_back(b_in[i]);
				s_l.push_back(l_in[i]);
			}

			if (s_a.size() < 6)
			{
				printf("%s\n", ".............");
				exit(1);
			}
			else
			{
				s_a1.assign(s_a.end()-6, s_a.end());
				s_b1.assign(s_b.end()-6, s_b.end());
				s_l1.assign(s_l.end()-6, s_l.end());

				s_A.push_back(s_a1);
				s_B.push_back(s_b1);
				s_L.push_back(s_l1);
			}
		}
	}

	inFile.close();
}

static vector<vector<float>> TrajGeneration(vector<vector<float>> & vect1, int tstep)
{
	int vectsize = vect1.size(); // return row size of 2d vector, size of timestamp read from file

	vector< vector<float> > state;
	vector<float> state_1;
	vector<float> state1;

	int t = 0;

	for (int i = 0; i < 6; i++)
	{
		for (int j = 0; j < vectsize-1; j++)
		{
			float ps = vect1[j][i];
			float pe = vect1[j+1][i];

			// printf("ps: %f, pe: %f\n", ps, pe);

			for (int k = 0; k < tstep; k++)// include both starting & ending point
			{
				float pm = lineardiffgenetraj(ps, pe, k, tstep);
				state1.push_back(pm);
				// printf("state1[%d]: %f\n", t, state1[t]);
				t ++;
			}
		}

		int newvectorsize = tstep*(vectsize-1);

		if (state1.size() < newvectorsize)
		{
			printf("%s\n", "!.............");
			exit(1);
		}
		else
		{
			state_1.assign(state1.end() - newvectorsize, state1.end());
			state.push_back(state_1);
		}
	}

	return state;
}

static void writeABLCommand()
{
	if (flag_start && repeat > 0)
	{
		if (num < state_A[0].size())
		{
			for (int i = 0; i < 6; i++)
			{
				alpha[i]  = state_A[i][num];
				beta[i]   = state_B[i][num];
				length[i] = state_L[i][num];
			}

			usleep(ts[loop_no]);
			num ++;
			if (num % period == 0)
			{
				loop_no ++;
			}
		}
		else
		{
			num = 0;
			loop_no = 0;
			repeat = repeat - 1;
		}
	}
	else
	{
		num = 0;
		loop_no = 0;

		for (int i = 0; i < 6; i++)
		{
			alpha[i]  = 0;
			beta[i]   = 0;
			length[i] = g_length0;
		}
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "demo_repeatability");
	ros::NodeHandle nh;
	ros::Rate r(100);     //Hz

	ros::Publisher  pub1 = nh.advertise<origarm_ros::Command_ABL>("Cmd_ABL_joy", 100);
	ros::Subscriber key_sub_ = nh.subscribe("key_number", 1, keyCallback);
	origarm_ros::Command_ABL Command_ABL_demo;

	readFromFile();

	// for (int i = 0; i < s_A.size(); i++)
	// {
	// 	for (int j = 0; j < s_A[0].size(); j++)
	// 	{
	// 		printf("s_A[%d][%d]: %f\n", i, j, s_A[i][j]);
	// 	}
	// }

	state_A = TrajGeneration(s_A, period);
	state_B = TrajGeneration(s_B, period);
	state_L = TrajGeneration(s_L, period);

	// for (int i = 0; i < state_A.size(); i++)
	// {
	// 	for (int j = 0; j < state_A[0].size(); j++)
	// 	{
	// 		printf("state_A[%d][%d]: %f\n", i, j, state_A[i][j]);
	// 	}
	// }

	while (ros::ok())
	{
		writeABLCommand();
		printf("flag_start: %d, num: %d, loop_no: %d, repeat: %d\r\n", flag_start, num, loop_no, repeat);
		for (int i = 0; i < 6; i++)
		{
			printf("alpha[%d]: %.4f, beta[%d]: %.4f, length[%d]: %.4f\r\n", i, alpha[i], i, beta[i], i, length[i]);
		}

		for (int i = 0; i < 6; i++)
		{
			Command_ABL_demo.segment[i].A = alpha[i];
			Command_ABL_demo.segment[i].B = beta[i];
			Command_ABL_demo.segment[i].L = length[i];
		}
		for (int i = 6; i < SEGNUM; i++)
		{
			Command_ABL_demo.segment[i].A = 0;
			Command_ABL_demo.segment[i].B = 0;
			Command_ABL_demo.segment[i].L = g_length0;
		}

		pub1.publish(Command_ABL_demo);

		ros::spinOnce(); //necessary for subscribe
		r.sleep();
	}

	inFile.close();
	return 0;
}


/*/
 * Use of this source code is governed by an MIT-style
 * license that can be found in the LICENSE file or at
 * https://opensource.org/licenses/MIT
/*/

#define EIGEN_MPL2_ONLY
#include <Eigen/Dense>
#undef EIGEN_MPL2_ONLY


/**
 * @file vecHarness.cpp
 * Test harness for vectorised beliefs using maxsum::DiscreteFunction objets.
 */

#include <limits>
#include <exception>
#include <cfloat>
#include "dec_brl/vpi.h"
#include "dec_brl/NormalGamma.h"
#include "DiscreteFunction.h"
#include "DomainIterator.h"
#include <iostream>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

// private module namespace
namespace
{

/**
 * Check that two doubles are equal within margin of error.
 */
bool equalWithinTol_m(double v1, double v2, double tol=0.0001)
{
   return tol > (std::abs(v1-v2)/std::min(v1,v2));
}

/**
 * Function used to check for consistency between values.
 */
bool isConsistent
(
 const dec_brl::dist::NormalGamma& dist1,
 const dec_brl::dist::NormalGamma& dist2
)
{
   bool  alphaOk = equalWithinTol_m(dist1.alpha,dist2.alpha);
   bool   betaOk = equalWithinTol_m(dist1.beta,dist2.beta);
   bool lambdaOk = equalWithinTol_m(dist1.lambda,dist2.lambda);
   bool      mOk = equalWithinTol_m(dist1.m,dist2.m);
   return alphaOk & betaOk & lambdaOk & mOk;
}

/**
 * Function used to check for consistency between scalar NormalGamma
 * and a specified element of a NormalGamma distribution defined over a
 * DiscreteFunction.
 */
bool isConsistent
(
 const dec_brl::dist::NormalGamma& otherDist,
 const dec_brl::dist::NormalGamma& scalarDist,
 const dec_brl::dist::NormalGamma_Tmpl<maxsum::DiscreteFunction>& vecDist,
 const int index
)
{
   const dec_brl::dist::NormalGamma* pCorrect = 0;
   for(int k=0; k<vecDist.m.domainSize(); ++k)
   {
      if(index==k)
      {
         pCorrect=&scalarDist;
      }
      else
      {
         pCorrect=&otherDist;
      }

      bool  alphaOk = equalWithinTol_m(pCorrect->alpha,vecDist.alpha(k));
      bool   betaOk = equalWithinTol_m(pCorrect->beta,vecDist.beta(k));
      bool lambdaOk = equalWithinTol_m(pCorrect->lambda,vecDist.lambda(k));
      bool      mOk = equalWithinTol_m(pCorrect->m,vecDist.m(k));

      if(!(alphaOk & betaOk & lambdaOk & mOk))
      {
         return false;
      }
   }
   return true;

} // isConsistent

} // private module namespace

/**
 * Main function for harness - executes all tests.
 */
int main()
{
   try
   {
      using namespace dec_brl;
      using namespace dec_brl::dist;
      std::cout << "Hello World!" << std::endl;

      //************************************************************************
      // Determine if this platform has its own representation of infinity,
      // and make a note in the log
      //************************************************************************
      if(std::numeric_limits<double>::has_infinity)
      {
         std::cout << "Platform has infinity" << std::endl;
      }
      else
      {
         std::cout << "Platform has no infinity, using max instead"
            << std::endl;
      }

      //************************************************************************
      // Construct a random number generator for a standard normal distribution.
      //************************************************************************
      boost::mt19937 rnd;  // uniform random number generator
      boost::normal_distribution<> normal; // normal distribution
      boost::variate_generator<boost::mt19937&,boost::normal_distribution<> >
         normrnd(rnd,normal); // generates normal distributed random variates.

      //************************************************************************
      // Register some variables for the creation of DiscreteFunction objects.
      //************************************************************************
      maxsum::registerVariable(1,5);
      maxsum::registerVariable(2,4);
      maxsum::registerVariable(3,2);

      //************************************************************************
      // Now we create a number parameter distributions:
      // scalarParams1 - will observe 1 observation per cycle
      // scalarParams2 - will observe 2 observations per cycle
      // vecParams - will equal scalarParams1 across its domain, except for
      // one element, which should equal scalarParams2.
      //************************************************************************
      std::cout << "Initialising parameter distributions" << std::endl;
      NormalGamma scalarParams1;
      NormalGamma scalarParams2;
      NormalGamma_Tmpl<maxsum::DiscreteFunction> vecParams;

      //************************************************************************
      // Try to expand domain of belief distribution
      //************************************************************************
      expand(vecParams,1);
      maxsum::VarID otherVars[] = {2,3};
      expand(vecParams,otherVars,otherVars+2);
      maxsum::ValIndex vecIndex = vecParams.m.domainSize()/2;

      std::cout << "selected index for dual update: " << vecIndex << std::endl;

      //************************************************************************
      // Repeatedly update the normal-gamma distribution with observations from
      // the standard normal, and validate VPI after each observation.
      //************************************************************************
      const int SAMPLE_SIZE = 60;
      std::vector<double> allObs1(SAMPLE_SIZE); // store obs for future ref
      std::vector<double> allObs2(SAMPLE_SIZE);
      double mu1 = 0.0; // sample mean for first obs only
      double mu2 = 0.0; // sample mean for all observations
      for(int it=0; it<SAMPLE_SIZE; ++it)
      {
         std::cout << "******************************************" << std::endl;
         std::cout << "ITERATION: " << it << std::endl;
         std::cout << "******************************************" << std::endl;

         //*********************************************************************
         // Sample two random variates from a standard normal distribution
         //*********************************************************************
         std::cout << "Generating new observations" << std::endl;
         double obs1 = normrnd();
         double obs2 = normrnd();
         allObs1[it] = obs1;
         allObs2[it] = obs2;
         mu1 += obs1;
         mu2 += obs2;

         //*********************************************************************
         // Update scalarParams1 using only the first observation, and
         // scalarParams2 using both observations. vecParams is updated so that
         // it equals scalarParams in all places, except one, where it equals
         // scalarParams2.
         //*********************************************************************
         std::cout << "Observing new observations" << std::endl;
         observe(scalarParams1,obs1);
         observe(scalarParams2,obs1);
         observe(scalarParams2,obs2);
         observe(vecParams,obs1);
         observe(vecParams,vecIndex,obs2);

         //*********************************************************************
         // Check hyperparameters for consistency
         //*********************************************************************
         std::cout << "Check hyperparameter consistency" << std::endl;
         for(int k=0; k<vecParams.m.domainSize(); ++k)
         {
            NormalGamma* pCorrect = &scalarParams1;
            if(k==vecIndex)
            {
               pCorrect = &scalarParams2;
            }

            if(!equalWithinTol_m(vecParams.alpha(k),pCorrect->alpha))
            {
               std::cout << "Incorrect at alpha location " << k << std::endl;
               return EXIT_FAILURE;
            }

            if(!equalWithinTol_m(vecParams.beta(k),pCorrect->beta))
            {
               std::cout << "Incorrect beta at location " << k << std::endl;
               return EXIT_FAILURE;
            }

            if(!equalWithinTol_m(vecParams.lambda(k),pCorrect->lambda))
            {
               std::cout << "Incorrect lambda at location " << k << std::endl;
               return EXIT_FAILURE;
            }

            if(!equalWithinTol_m(vecParams.m(k),pCorrect->m))
            {
               std::cout << "Incorrect mean at location " << k << std::endl;
               return EXIT_FAILURE;
            }
         }

      } // for loop

      //************************************************************************
      // Calculate the sufficient statistics for both sets of samples
      //************************************************************************
      std::cout << "Calculating sufficient statistics" << std::endl;
      mu1 /= SAMPLE_SIZE;
      mu2 /= SAMPLE_SIZE;
      double s1 = 0.0;
      double s2 = 0.0;
      for(int it=0; it<SAMPLE_SIZE; ++it)
      {
         double x1 = allObs1[it]-mu1;
         double x2 = allObs2[it]-mu2;
         s1 += x1*x1;
         s2 += x2*x2;
      }

      //************************************************************************
      // Repeat the updates using sufficient statistics, and check for
      // consistency.
      //************************************************************************
      std::cout << "Attempting Moment Update" << std::endl;
      NormalGamma scalarSingleUpdate1;
      NormalGamma scalarSingleUpdate2;
      NormalGamma_Tmpl<maxsum::DiscreteFunction> vecSingleUpdate;

      //************************************************************************
      // Try to expand domain of belief distribution
      //************************************************************************
      expand(vecSingleUpdate,1);
      expand(vecSingleUpdate,otherVars,otherVars+2);

      //************************************************************************
      // Do the update
      //************************************************************************
      observe(scalarSingleUpdate1,mu1,s1,SAMPLE_SIZE);
      observe(scalarSingleUpdate2,mu1,s1,SAMPLE_SIZE);
      observe(scalarSingleUpdate2,mu2,s2,SAMPLE_SIZE);
      observe(vecSingleUpdate,mu1,s1,SAMPLE_SIZE);
      observe(vecSingleUpdate,vecIndex,mu2,s2,SAMPLE_SIZE);

      //************************************************************************
      // Checking for consistency
      //************************************************************************
      std::cout << "Check for Consistency" << std::endl;
      if(!isConsistent(scalarParams1,scalarSingleUpdate1))
      {
         std::cout << "Params 1 - inconsistent moment update" << std::endl;
         return EXIT_FAILURE;
      }

      if(!isConsistent(scalarParams2,scalarSingleUpdate2))
      {
         std::cout << "Params 2 - inconsistent moment update" << std::endl;
         return EXIT_FAILURE;
      }

      if(isConsistent(scalarParams1,scalarSingleUpdate2))
      {
         std::cout << "Unexpected match between updates 1 and 2" << std::endl;
         return EXIT_FAILURE;
      }

      if(!isConsistent(scalarParams1,scalarParams2,vecSingleUpdate,vecIndex))
      {
         std::cout << "inconsistent vector moment update" << std::endl;
         return EXIT_FAILURE;
      }

   }
   catch(std::exception& e)
   {
      std::cout << "Caught error: " << e.what() << std::endl;
      return EXIT_FAILURE;
   }

   //***************************************************************************
   // If we get this far, everything is ok.
   //***************************************************************************
   std::cout << "Passed." << std::endl;
   return EXIT_SUCCESS;

} // function main


#include <sparse.h>
#include <sparse_fill.h>
#include <prox_math_policy.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(prod_mass_block);

BOOST_AUTO_TEST_CASE(mass_4x6_block_prod_test_case)
{
  typedef prox::MathPolicy<float>      math_policy;
  typedef math_policy::mass_block_type mass_block_type;
  typedef math_policy::block4x6_type   jacobian_block_type;

  jacobian_block_type jb;
  jacobian_block_type jb_result(sparse::zero_block<jacobian_block_type>());
  sparse::fill(jb, 1.0f);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  mass_block_type mb;
  sparse::fill(mb, 0.0f);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  mb[0] = -2.0f;

  prox::prod(jb,mb,jb_result);

  BOOST_CHECK_EQUAL( jb_result[0]  , -2  );
  BOOST_CHECK_EQUAL( jb_result[6]  , -14 );
  BOOST_CHECK_EQUAL( jb_result[12] , -26 );
  BOOST_CHECK_EQUAL( jb_result[18] , -38 );
  BOOST_CHECK_EQUAL( jb_result[1]  , -4  );
  BOOST_CHECK_EQUAL( jb_result[7]  , -16 );
  BOOST_CHECK_EQUAL( jb_result[13] , -28 );
  BOOST_CHECK_EQUAL( jb_result[19] , -40 );
  BOOST_CHECK_EQUAL( jb_result[2]  , -6  );
  BOOST_CHECK_EQUAL( jb_result[8]  , -18 );
  BOOST_CHECK_EQUAL( jb_result[14] , -30 );
  BOOST_CHECK_EQUAL( jb_result[20] , -42 );
  BOOST_CHECK_EQUAL( jb_result[3]  , 32  );
  BOOST_CHECK_EQUAL( jb_result[9]  , 68  );
  BOOST_CHECK_EQUAL( jb_result[15] , 104 );
  BOOST_CHECK_EQUAL( jb_result[21] , 140 );
  BOOST_CHECK_EQUAL( jb_result[4]  , 58  );
  BOOST_CHECK_EQUAL( jb_result[10] , 124 );
  BOOST_CHECK_EQUAL( jb_result[16] , 190 );
  BOOST_CHECK_EQUAL( jb_result[22] , 256 );
  BOOST_CHECK_EQUAL( jb_result[5]  , 73  );
  BOOST_CHECK_EQUAL( jb_result[11] , 157 );
  BOOST_CHECK_EQUAL( jb_result[17] , 241 );
  BOOST_CHECK_EQUAL( jb_result[23] , 325 );

}

BOOST_AUTO_TEST_SUITE_END();