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

#include <hayai.hpp>

#include <irritator/core.hpp>

#include <boost/ut.hpp>

#include <fmt/format.h>

#include <cstdio>

#include <fstream>

using namespace std;

struct file_output
{
    file_output(const char* file_path) noexcept
      : os(std::fopen(file_path, "w"))
    {}

    ~file_output() noexcept
    {
        if (os)
            std::fclose(os);
    }

    std::FILE* os = nullptr;
};

void
file_output_initialize(const irt::observer& obs, const irt::time /*t*/) noexcept
{
    if (!obs.user_data)
        return;

    auto* output = reinterpret_cast<file_output*>(obs.user_data);
    fmt::print(output->os, "t,{}\n", obs.name.c_str());
}

void
file_output_observe(const irt::observer& obs,
                    const irt::time t,
                    const irt::message& msg) noexcept
{
    if (!obs.user_data)
        return;

    auto* output = reinterpret_cast<file_output*>(obs.user_data);
    fmt::print(output->os, "{},{}\n", t, msg.real[0]);
}

struct neuron {
  irt::dynamics_id  sum;
  irt::dynamics_id  integrator;
  irt::dynamics_id  constant;
  irt::dynamics_id  cross;
  irt::dynamics_id  constant_cross;
};

struct neuron
make_neuron(irt::simulation* sim, long unsigned int i, double quantum) noexcept
{
  using namespace boost::ut;
  double tau_lif = 10;
  double Vr_lif = 0.0;
  double Vt_lif = 10.0;

  auto& sum_lif = sim->qss3_wsum_2_models.alloc();
  auto& integrator_lif = sim->qss3_integrator_models.alloc();
  auto& constant_lif = sim->constant_models.alloc();
  auto& constant_cross_lif = sim->constant_models.alloc();
  auto& cross_lif = sim->qss3_cross_models.alloc();

  sum_lif.default_input_coeffs[0] = -1.0/tau_lif;
  sum_lif.default_input_coeffs[1] = 20.0/tau_lif;

  constant_lif.default_value = 1.0;
  constant_cross_lif.default_value = Vr_lif;

  integrator_lif.default_X = 0.0;
  integrator_lif.default_dQ = quantum;

  cross_lif.default_threshold = Vt_lif;

  sim->alloc(sum_lif, sim->qss3_wsum_2_models.get_id(sum_lif));
  sim->alloc(integrator_lif, sim->qss3_integrator_models.get_id(integrator_lif));
  sim->alloc(constant_lif, sim->constant_models.get_id(constant_lif));
  sim->alloc(cross_lif, sim->qss3_cross_models.get_id(cross_lif));
  sim->alloc(constant_cross_lif, sim->constant_models.get_id(constant_cross_lif));

  struct neuron neuron_model = {sim->qss3_wsum_2_models.get_id(sum_lif),
                                sim->qss3_integrator_models.get_id(integrator_lif),
                                sim->constant_models.get_id(constant_lif),
                                sim->qss3_cross_models.get_id(cross_lif),
                                sim->constant_models.get_id(constant_cross_lif),
                                };

  // Connections
  expect(sim->connect(cross_lif.y[0], integrator_lif.x[1]) ==
        irt::status::success);
  expect(sim->connect(cross_lif.y[1], sum_lif.x[0]) ==
        irt::status::success);
  expect(sim->connect(integrator_lif.y[0],cross_lif.x[0]) ==
        irt::status::success);
  expect(sim->connect(integrator_lif.y[0],cross_lif.x[2]) ==
        irt::status::success);
  expect(sim->connect(constant_cross_lif.y[0],cross_lif.x[1]) ==
        irt::status::success);
  expect(sim->connect(constant_lif.y[0], sum_lif.x[1]) ==
        irt::status::success);
  expect(sim->connect(sum_lif.y[0], integrator_lif.x[0]) ==
        irt::status::success);
  return neuron_model;
}

void lif_benchmark(double simulation_duration, double quantum)
{
    using namespace boost::ut;

    irt::simulation sim;
    expect(irt::is_success(sim.init(2600lu, 40000lu)));

    struct neuron neuron_model = make_neuron(&sim,0.0,quantum);

    irt::time t = 0.0;
    std::string file_name = "output_lif_qss3_sd_"+
                            std::to_string(simulation_duration)+
                            "_q_"+std::to_string(quantum)+
                            ".csv";
    file_output fo_a(file_name.c_str());
    expect(fo_a.os != nullptr);

    auto& obs_a = sim.observers.alloc(0.01,
                                        "A",
                                        static_cast<void*>(&fo_a),
                                        &file_output_initialize,
                                        &file_output_observe,
                                        nullptr);
    sim.observe(sim.models.get(sim.qss3_integrator_models.get(neuron_model.integrator).id), obs_a);

    expect(irt::status::success == sim.initialize(t));

    do {

        irt::status st = sim.run(t);
        expect(st == irt::status::success);

    } while (t < simulation_duration);
}
void izhikevich_benchmark(double simulation_duration, double quantum, double a, double b, double c, double d, double I, double vini)
{
  using namespace boost::ut;
  irt::simulation sim;
  expect(irt::is_success(sim.init(1000lu, 1000lu)));
  auto& constant = sim.constant_models.alloc();
  auto& constant2 = sim.constant_models.alloc();
  auto& constant3 = sim.constant_models.alloc();
  auto& sum_a = sim.qss3_wsum_2_models.alloc();
  auto& sum_b = sim.qss3_wsum_2_models.alloc();
  auto& sum_c = sim.qss3_wsum_4_models.alloc();
  auto& sum_d = sim.qss3_wsum_2_models.alloc();
  auto& product = sim.qss3_multiplier_models.alloc();
  auto& integrator_a = sim.qss3_integrator_models.alloc();
  auto& integrator_b = sim.qss3_integrator_models.alloc();
  auto& cross = sim.qss3_cross_models.alloc();
  auto& cross2 = sim.qss3_cross_models.alloc();

  double vt = 30.0;

  constant.default_value = 1.0;
  constant2.default_value = c;
  constant3.default_value = I;

  cross.default_threshold = vt;
  cross2.default_threshold = vt;

  integrator_a.default_X = vini;
  integrator_a.default_dQ = quantum;

  integrator_b.default_X = vini;
  integrator_b.default_dQ = quantum;

  sum_a.default_input_coeffs[0] = 1.0;
  sum_a.default_input_coeffs[1] = -1.0;
  sum_b.default_input_coeffs[0] = -a;
  sum_b.default_input_coeffs[1] = a * b;
  sum_c.default_input_coeffs[0] = 0.04;
  sum_c.default_input_coeffs[1] = 5.0;
  sum_c.default_input_coeffs[2] = 140.0;
  sum_c.default_input_coeffs[3] = 1.0;
  sum_d.default_input_coeffs[0] = 1.0;
  sum_d.default_input_coeffs[1] = d;

  sim.alloc(constant3, sim.constant_models.get_id(constant3));
  sim.alloc(constant, sim.constant_models.get_id(constant));
  sim.alloc(constant2, sim.constant_models.get_id(constant2));
  sim.alloc(sum_a, sim.qss3_wsum_2_models.get_id(sum_a));
  sim.alloc(sum_b, sim.qss3_wsum_2_models.get_id(sum_b));
  sim.alloc(sum_c, sim.qss3_wsum_4_models.get_id(sum_c));
  sim.alloc(sum_d, sim.qss3_wsum_2_models.get_id(sum_d));
  sim.alloc(product, sim.qss3_multiplier_models.get_id(product));
  sim.alloc(integrator_a, sim.qss3_integrator_models.get_id(integrator_a));
  sim.alloc(integrator_b, sim.qss3_integrator_models.get_id(integrator_b));
  sim.alloc(cross, sim.qss3_cross_models.get_id(cross));
  sim.alloc(cross2, sim.qss3_cross_models.get_id(cross2));

  expect(sim.connect(integrator_a.y[0], cross.x[0]) ==
          irt::status::success);
  expect(sim.connect(constant2.y[0], cross.x[1]) == irt::status::success);
  expect(sim.connect(integrator_a.y[0], cross.x[2]) ==
          irt::status::success);

  expect(sim.connect(cross.y[1], product.x[0]) == irt::status::success);
  expect(sim.connect(cross.y[1], product.x[1]) == irt::status::success);
  expect(sim.connect(product.y[0], sum_c.x[0]) == irt::status::success);
  expect(sim.connect(cross.y[1], sum_c.x[1]) == irt::status::success);
  expect(sim.connect(cross.y[1], sum_b.x[1]) == irt::status::success);

  expect(sim.connect(constant.y[0], sum_c.x[2]) == irt::status::success);
  expect(sim.connect(constant3.y[0], sum_c.x[3]) == irt::status::success);

  expect(sim.connect(sum_c.y[0], sum_a.x[0]) == irt::status::success);
  // expect(sim.connect(integrator_b.y[0], sum_a.x[1]) ==
  // irt::status::success);
  expect(sim.connect(cross2.y[1], sum_a.x[1]) == irt::status::success);
  expect(sim.connect(sum_a.y[0], integrator_a.x[0]) ==
          irt::status::success);
  expect(sim.connect(cross.y[0], integrator_a.x[1]) ==
          irt::status::success);

  expect(sim.connect(cross2.y[1], sum_b.x[0]) == irt::status::success);
  expect(sim.connect(sum_b.y[0], integrator_b.x[0]) ==
          irt::status::success);

  expect(sim.connect(cross2.y[0], integrator_b.x[1]) ==
          irt::status::success);
  expect(sim.connect(integrator_a.y[0], cross2.x[0]) ==
          irt::status::success);
  expect(sim.connect(integrator_b.y[0], cross2.x[2]) ==
          irt::status::success);
  expect(sim.connect(sum_d.y[0], cross2.x[1]) == irt::status::success);
  expect(sim.connect(integrator_b.y[0], sum_d.x[0]) ==
          irt::status::success);
  expect(sim.connect(constant.y[0], sum_d.x[1]) == irt::status::success);

  std::string file_name = "output_izhikevitch_qss3_a_sd_"+
                          std::to_string(simulation_duration)+
                          "_q_"+std::to_string(quantum)+
                          "_a_"+std::to_string(a)+
                          "_b_"+std::to_string(b)+
                          "_c_"+std::to_string(c)+
                          "_d_"+std::to_string(d)+
                          ".csv";
  file_output fo_a(file_name.c_str());
  expect(fo_a.os != nullptr);

  auto& obs_a = sim.observers.alloc(0.01,
                                    "A",
                                    static_cast<void*>(&fo_a),
                                    &file_output_initialize,
                                    &file_output_observe,
                                    nullptr);
  file_name = "output_izhikevitch_qss3_b_sd_"+
                          std::to_string(simulation_duration)+
                          "_q_"+std::to_string(quantum)+
                          "_a_"+std::to_string(a)+
                          "_b_"+std::to_string(b)+
                          "_c_"+std::to_string(c)+
                          "_d_"+std::to_string(d)+
                          ".csv";
  file_output fo_b(file_name.c_str());
  expect(fo_b.os != nullptr);
  auto& obs_b = sim.observers.alloc(0.01,
                                    "B",
                                    static_cast<void*>(&fo_b),
                                    &file_output_initialize,
                                    &file_output_observe,
                                    nullptr);

  sim.observe(sim.models.get(integrator_a.id), obs_a);
  sim.observe(sim.models.get(integrator_b.id), obs_b);

  irt::time t = 0.0;

  expect(irt::status::success == sim.initialize(t));
  !expect(sim.sched.size() == 12_ul);

  do {
      irt::status st = sim.run(t);
      expect(st == irt::status::success);
  } while (t < simulation_duration);
};

BENCHMARK_P(LIF, QSS3, 10, 1,( double simulation_duration, double quantum))
{
  lif_benchmark(simulation_duration,quantum);
}
BENCHMARK_P(Izhikevich, QSS3, 1, 1,( double simulation_duration, double quantum, double a, double b, double c, double d, double I, double vini))
{
  izhikevich_benchmark(simulation_duration,quantum,a,b,c,d,I,vini);
}

BENCHMARK_P_INSTANCE(LIF, QSS3, (1000,1e-6));
// Regular spiking (RS)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.02,0.2,-65.0,8.0,10.0,0.0));
// Intrinsical bursting (IB)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.02,0.2,-55.0,4.0,10.0,0.0));
// Chattering spiking (CH)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.02,0.2,-50.0,2.0,10.0,0.0));
// Fast spiking (FS)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.1,0.2,-65.0,2.0,10.0,0.0));
// Thalamo-Cortical (TC)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.02,0.25,-65.0,0.05,10.0,-87.0));
// Rezonator (RZ)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.1,0.26,-65.0,2.0,10.0,-63.0));
// Low-threshold spiking (LTS)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.02,0.25,-65.0,2.0,10.0,-63.0));
// Problematic (P)
BENCHMARK_P_INSTANCE(Izhikevich, QSS3, (1000,1e-6,0.2,2,-56.0,-16.0,-99.0,0.0));

int
main()
{
      hayai::ConsoleOutputter consoleOutputter;

      hayai::Benchmarker::AddOutputter(consoleOutputter);
      hayai::Benchmarker::RunAllTests();

}


#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/modeling_utils.hpp>
#include <trajopt/kinematic_terms.hpp>
#include <trajopt/rave_utils.hpp>
#include <trajopt/utils.hpp>
#include <trajopt_utils/eigen_conversions.hpp>
#include <trajopt_utils/eigen_slicing.hpp>
#include <trajopt_utils/logging.hpp>
#include <trajopt_utils/stl_to_string.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <Eigen/Geometry>
#include <iostream>

using namespace std;
using namespace sco;
using namespace Eigen;
using namespace util;

namespace {

#if 0
Vector3d rotVec(const Matrix3d& m) {
  Quaterniond q; q = m;
  return Vector3d(q.x(), q.y(), q.z());
}
#endif
inline Vector3d rotVec(const OpenRAVE::Vector& q) {
  return Vector3d(q[1], q[2], q[3]);
}

#if 0
VectorXd concat(const VectorXd& a, const VectorXd& b) {
  VectorXd out(a.size()+b.size());
  out.topRows(a.size()) = a;
  out.middleRows(a.size(), b.size()) = b;
  return out;
}

template <typename T>
vector<T> concat(const vector<T>& a, const vector<T>& b) {
  vector<T> out;
  vector<int> x;
  out.insert(out.end(), a.begin(), a.end());
  out.insert(out.end(), b.begin(), b.end());
  return out;
}
#endif

}

namespace trajopt {

// CostPtr ConstructCost(VectorOfVectorPtr err_calc, const VarVector& vars, const VectorXd& coeffs, PenaltyType type, const string& name) {
//   return CostPtr(new CostFromErrFunc(err_calc), vars, coeffs, type, name);
// }

VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const {
  manip_->SetDOFValues(toDblVec(dof_vals));
  OR::Transform newpose = link_->GetTransform();

  OR::Transform pose_err = pose_inv_ * newpose;
  VectorXd err = concat(rotVec(pose_err.rot), toVector3d(pose_err.trans));
  return err;
}

#if 0
CartPoseCost::CartPoseCost(const VarVector& vars, const OR::Transform& pose, RobotAndDOFPtr manip, KinBody::LinkPtr link, const VectorXd& coeffs) :
    CostFromErrFunc(VectorOfVectorPtr(new CartPoseErrCalculator(pose, manip, link)), vars, coeffs, ABS, "CartPose")
{}
CartPoseConstraint::CartPoseConstraint(const VarVector& vars, const OR::Transform& pose,
    RobotAndDOFPtr manip, KinBody::LinkPtr link, const VectorXd& coeffs) :
    ConstraintFromFunc(VectorOfVectorPtr(new CartPoseErrCalculator(pose, manip, link)), vars, coeffs, EQ, "CartPose")
{}
#endif

void CartPoseErrorPlotter::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  CartPoseErrCalculator* calc = static_cast<CartPoseErrCalculator*>(m_calc.get());
  DblVec dof_vals = getDblVec(x, m_vars);
  calc->manip_->SetDOFValues(dof_vals);
  OR::Transform target = calc->pose_inv_.inverse(), cur = calc->link_->GetTransform();
  PlotAxes(env, cur, .05,  handles);
  PlotAxes(env, target, .05,  handles);
  handles.push_back(env.drawarrow(cur.trans, target.trans, .005, OR::Vector(1,0,1,1)));
}

#if 0
struct CartPositionErrCalculator {
  Vector3d pt_world_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  CartPositionErrCalculator(const Vector3d& pt_world, RobotAndDOFPtr manip, OR::KinBody::LinkPtr link) :
  pt_world_(pt_world),
  manip_(manip),
  link_(link)
  {}
  VectorXd operator()(const VectorXd& dof_vals) {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();
    return pt_world_ - toVector3d(newpose.trans);
  }
};
#endif

MatrixXd CartVelJacCalculator::operator()(const VectorXd& dof_vals) const {
  int n_dof = manip_->GetDOF();
  MatrixXd out(6, 2*n_dof);
  manip_->SetDOFValues(toDblVec(dof_vals.topRows(n_dof)));
  OR::Transform pose0 = link_->GetTransform();
  MatrixXd jac0 = manip_->PositionJacobian(link_->GetIndex(), pose0.trans);
  manip_->SetDOFValues(toDblVec(dof_vals.bottomRows(n_dof)));
  OR::Transform pose1 = link_->GetTransform();
  MatrixXd jac1 = manip_->PositionJacobian(link_->GetIndex(), pose1.trans);
  out.block(0,0,3,n_dof) = -jac0;
  out.block(0,n_dof,3,n_dof) = jac1;
  out.block(3,0,3,n_dof) = jac0;
  out.block(3,n_dof,3,n_dof) = -jac1;
  return out;
}

VectorXd CartVelCalculator::operator()(const VectorXd& dof_vals) const {
  int n_dof = manip_->GetDOF();
  manip_->SetDOFValues(toDblVec(dof_vals.topRows(n_dof)));
  OR::Transform pose0 = link_->GetTransform();
  manip_->SetDOFValues(toDblVec(dof_vals.bottomRows(n_dof)));
  OR::Transform pose1 = link_->GetTransform();
  VectorXd out(6);
  out.topRows(3) = toVector3d(pose1.trans - pose0.trans - OR::Vector(limit_,limit_,limit_));
  out.bottomRows(3) = toVector3d( - pose1.trans + pose0.trans - OR::Vector(limit_, limit_, limit_));
  return out;
}

#if 0
CartVelConstraint::CartVelConstraint(const VarVector& step0vars, const VarVector& step1vars, RobotAndDOFPtr manip, KinBody::LinkPtr link, double distlimit) :
        ConstraintFromFunc(VectorOfVectorPtr(new CartVelCalculator(manip, link, distlimit)),
             MatrixOfVectorPtr(new CartVelJacCalculator(manip, link, distlimit)), concat(step0vars, step1vars), VectorXd::Ones(0), INEQ, "CartVel")
{} // TODO coeffs
#endif

#if 0
struct UpErrorCalculator {
  Vector3d dir_local_;
  Vector3d goal_dir_world_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  MatrixXd perp_basis_; // 2x3 matrix perpendicular to goal_dir_world
  UpErrorCalculator(const Vector3d& dir_local, const Vector3d& goal_dir_world, RobotAndDOFPtr manip, KinBody::LinkPtr link) :
    dir_local_(dir_local),
    goal_dir_world_(goal_dir_world),
    manip_(manip),
    link_(link)
  {
    Vector3d perp0 = goal_dir_world_.cross(Vector3d::Random()).normalized();
    Vector3d perp1 = goal_dir_world_.cross(perp0);
    perp_basis_.resize(2,3);
    perp_basis_.row(0) = perp0.transpose();
    perp_basis_.row(1) = perp1.transpose();
  }
  VectorXd operator()(const VectorXd& dof_vals) {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();
    return perp_basis_*(toRot(newpose.rot) * dir_local_ - goal_dir_world_);
  }
};
#endif
}


//==============================================================================
//         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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_ABS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_ABS_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/abs.hpp>
#include <boost/simd/sdk/math.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( abs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< arithmetic_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_unsigned<A0>::type utype;

      utype mask = a0 >> (sizeof(result_type)*8 - 1);
      return (a0 + mask) ^ mask;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< single_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      #ifdef BOOST_SIMD_HAS_FABSF
       return ::fabsf(a0);
      #else
       return (a0 > 0) ? a0 : -a0;
      #endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< double_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
       return ::fabs(a0);
    }
  };
  BOOST_DISPATCH_IMPLEMENT          ( abs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< unsigned_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( abs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< bool_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0;
    }
  };
} } }

#endif


#define STENCILA_FRAME_CPP

#include <fstream>

#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
//#define BOOST_DISABLE_ASSERTS
#include <boost/multi_array.hpp>

#include <stencila/exception.hpp>
#include <stencila/frame.hpp>
#include <stencila/string.hpp>

namespace Stencila {

Frame::Frame(void):
	data_(new Data){
	resize_(0,0);
}

Frame::Frame(const Frame& frame):
	data_(new Data(*frame.data_)),
	labels_(frame.labels_){
	resize_(frame.rows(),frame.columns());
}

Frame::Frame(const std::vector<std::string>& labels, unsigned int rows):
	data_(new Data),
	labels_(labels){
	resize_(rows,labels_.size());
}

Frame::Frame(unsigned int rows, const std::vector<std::string>& labels):
	data_(new Data),
	labels_(labels){
	resize_(rows,labels_.size());
}

Frame::Frame(const std::vector<std::string>& labels, const std::vector<double>& values):
	data_(new Data),
	labels_(labels){
	unsigned int cols = labels.size();
	unsigned int rows = values.size()/labels.size();
	resize_(rows,cols);
	for(unsigned int row=0;row<rows;row++){
		for(unsigned int col=0;col<cols;col++){
			operator()(row,col) = values[row*cols+col];
		}
	}
}

Frame::~Frame(void){
	delete data_;
};

unsigned int Frame::rows(void) const {
	return data_->shape()[0];
}

unsigned int Frame::columns(void) const {
	return data_->shape()[1];
}

bool Frame::empty(void) const {
	return rows()==0;
}

std::vector<std::string> Frame::labels(void) const {
	return labels_;
}

std::string Frame::label(unsigned int index) const {
	return labels_[index];
}

int Frame::label(const std::string& label) const {
	auto iter = std::find(labels_.begin(),labels_.end(),label);
	if(iter==labels_.end()) return -1;
	else return iter-labels_.begin();
}

bool Frame::has(const std::string& label) const {
	return Frame::label(label)>=0;
}

double& Frame::operator()(unsigned int row, unsigned int column){
	return (*data_)(boost::array<Data::index,2>{{row,column}});
}

const double& Frame::operator()(unsigned int row, unsigned int column) const {
	return (*data_)(boost::array<Data::index,2>{{row,column}});
}

double& Frame::operator()(unsigned int row, const std::string& label) {
	return operator()(row,this->label(label));
}

const double& Frame::operator()(unsigned int row, const std::string& label) const {
	return operator()(row,this->label(label));
}

std::vector<double> Frame::row(unsigned int row) const {
	return std::vector<double>();
}

std::vector<double> Frame::column(unsigned int column) const {
	return std::vector<double>();
}

std::vector<double> Frame::column(const std::string& label) const {
	return column(Frame::label(label));
}

Frame Frame::slice(unsigned int row) const {
	Frame frame(1,labels());
	for(unsigned int col=0;col<columns();col++) frame(0,col) = operator()(row,col);
	return frame;
}

Frame& Frame::add(const std::string& label, const double& value){
	labels_.push_back(label);
	delta_(0,1);
	unsigned int column = columns() - 1;
	for(unsigned int row=0;row<rows();row++) (*data_)[row][column] = value;
	return *this;
}

Frame& Frame::append(unsigned int rows){
	delta_(rows,0);
	return *this;
}

Frame& Frame::append(const std::vector<double>& values){
	auto cols = columns();
	if(values.size()!=cols){
		STENCILA_THROW(Exception,str(boost::format(
			"Error attempting to append a row with <%i> columns to a frame with <%i> columns"
		)%values.size()%cols));
	}
	delta_(1,0);
	auto row = rows()-1;
	for(unsigned int col=0;col<cols;col++) operator()(row,col) = values[col];
	return *this;
}

Frame& Frame::append(const std::vector<std::string>& values){
	std::vector<double> numbers(values.size());
	for(unsigned int i=0;i<values.size();i++){
		auto string = values[i];
		double number;
		try {
			number = boost::lexical_cast<double>(string);
		} catch(...){
			STENCILA_THROW(Exception,"Error attempting to convert string <"+string+"> to number");
		}
		numbers[i] = number;
	}
	return append(numbers);
}

Frame& Frame::append(const Frame& frame){
	if(columns()==0){
		labels_ = frame.labels();
		resize_(0,labels_.size());
	}
	else if(frame.columns()!=columns()){
		STENCILA_THROW(Exception,str(boost::format(
			"Error attempting to append a frame with <%i> columns to a frame with <%s> columns"
		)%frame.columns()%columns()));
	}
	unsigned int rows_old = rows();
	delta_(frame.rows(),0);
	unsigned int rows_new = rows();
	(*data_)[boost::indices
		[Data::index_range(rows_old,rows_new)]
		[Data::index_range(0,columns())]
	] = *frame.data_;
	return *this;
}

Frame& Frame::clear(void){
	resize_(0,0);
	return *this;
}

Frame& Frame::read(std::istream& stream, const std::string& separator) {
	// Clear this frame
	clear();
	// Get labels from header and use to intialise
	std::string header;
	std::getline(stream,header);
	std::vector<std::string> labels;
	boost::split(labels,header,boost::is_any_of(separator));
	labels_ = labels;
	resize_(0,labels_.size());
	// Get each line
	std::string line;
	int count = 0;
	while(std::getline(stream,line)){
		try {
			count++;
			// Skip lines that are all whitespace
			// (this primarily is to prevent errors caused by extra empty lines at end of files)
			if(std::all_of(line.begin(),line.end(),isspace)) continue;
			// Split into values
			std::vector<std::string> values;
			boost::split(values,line,boost::is_any_of(separator));
			append(values);
		} catch (const std::exception& error){
			STENCILA_THROW(Exception,"Error reading line.\n  number: "+string(count)+"\n  content: "+line.substr(0,20)+"...\n  error: "+error.what());
		}
	}
	return *this;
}

Frame& Frame::read(const std::string path, const std::string& separator) {
	std::ifstream file(path);
	return read(file,separator);
}

const Frame& Frame::write(std::ostream& stream,const std::string& separator) const {
	auto rows = Frame::rows();
	auto cols = Frame::columns();
	for(unsigned int col=0;col<cols;col++){
		stream<<label(col);
		if(col!=cols-1) stream<<"\t";
	}
	stream<<"\n";
	for(unsigned int row=0;row<rows;row++){
		for(unsigned int col=0;col<cols;col++){
			stream<<operator()(row,col);
			if(col!=cols-1) stream<<"\t";
		}
		stream<<"\n";
	}
	return *this;
}

const Frame& Frame::write(const std::string path,const std::string& separator) const {
	std::ofstream file(path);
	return write(file,separator);
}

void Frame::resize_(unsigned int rows, unsigned int columns){
	Data::extent_gen extents;
	data_->resize(extents[rows][columns]);
}

void Frame::delta_(int rows, int columns){
	auto shape = data_->shape();
	Data::extent_gen extents;
	data_->resize(extents[shape[0]+rows][shape[1]+columns]);
}

}

std::ostream& operator<<(std::ostream& stream, const Stencila::Frame& frame){
	frame.write(stream);
	return stream;
}


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */
#include <gtest/gtest.h>
#include <armadillo>
#include <stdexcept>
#include <time.h>
#include <sys/time.h>
#include "ZeromerDiff.h"
#include "KeyClassifier.h"
#include "Utils.h"

using namespace std;

TEST(KeyClassifier_Test, ProjectionTest) {
  Col<double> well;
  well << 1 << 1;
  Col<double> incorporation;
  incorporation << 1 << 0;
  double p = KeyClassifier::GetProjection(incorporation, well);
  ASSERT_NEAR(p, 1, .0001);// 1.110721
  well <<  0.2193781 <<  1.9257150 <<  -0.0256586 <<  0.3911019;
  incorporation <<   0.7616795 <<  -0.8993396 << -0.1143299 << -1.3601566;
  p = KeyClassifier::GetProjection(well, incorporation);
  ASSERT_NEAR(-2.0938, p , .0001);

}

TEST(KeyClassifier_Test, Classify_Test) {
  //                                    0 1 2 3 4 5 6 7
  //                                    T A C G T A C G
    vector<int> libKey = char2Vec<int>("1 0 1 0 0 1 0", ' ');
    vector<int> tfKey = char2Vec<int> ("0 1 0 0 1 0 1", ' ');
    vector<KeySeq> keys;
    KeySeq lKey, tKey;
    lKey.name = "lib";
    lKey.flows = libKey;
    lKey.zeroFlows.set_size(4);
    lKey.zeroFlows << 1 << 3 << 4 << 6 ;
    keys.push_back(lKey);
    tKey.name = "tf";
    tKey.flows = tfKey;
    tKey.zeroFlows.set_size(4);
    tKey.zeroFlows << 0 << 2 << 3 << 5;
    keys.push_back(tKey);

    ZeromerDiff<double> bg;
    Mat<double> wellFlows;
    wellFlows.load("traces.txt",raw_ascii);
    wellFlows = trans(wellFlows);
    Mat<double> refFlows;
    refFlows.load("reference.txt", raw_ascii);
    refFlows = trans(refFlows);
    Col<double> time(refFlows.n_rows);
    for (size_t i = 0; i < time.n_rows; i++) {
      time[i] = i;
    }
    KeyFit fit;
    Mat<double> predicted(wellFlows.n_rows, wellFlows.n_cols);
    vector<float> reference;
    vector<float> signal;
		//    KeyClassifier::PrintVec(wellFlows);
		//    KeyClassifier::PrintVec(refFlows);
    //KeyClassifier::Classify(keys, bg, wellFlows, refFlows, reference, signal, time, 4, fit, predicted);
    KeyClassifier kClass;
    kClass.Classify(keys, bg, wellFlows, refFlows, time, 4, fit, predicted);
    Mat<double> diff = wellFlows - predicted;
    // cout << "Diff is: " << endl;
    // diff.raw_print();
    uvec zeroFlows(2);
    zeroFlows(0) = 0;
    zeroFlows(1) = 1;
    //    cout << "Index is: " <<  fit.keyIndex << " is: " << fit.snr << endl;
    Col<double> param;
    Mat<double> subW = wellFlows.submat(0,3,49,4);
    Mat<double> subR = refFlows.submat(0,3,49,4);
    bg.FitZeromer(subW, subR,zeroFlows, time, param);
    Col<double> z;
    bg.PredictZeromer(refFlows.col(1), time, param, z);
    Col<double> d = wellFlows.col(1) - z;
		//    cout << "diff is: " << endl;
    //    d.raw_print();
    bg.PredictZeromer(refFlows.col(3), time, param, z);
    d = wellFlows.col(3) - z;
		//    cout << "diff is: " << endl;
    //    d.raw_print();
}


#ifndef CX_DQMC_REPLICA_GS_HPP
#define CX_DQMC_REPLICA_GS_HPP
#include <boost/mpi.hpp>

#include <libdqmc/la.hpp>
#include <libdqmc/cx_dqmc_greens_general.hpp>
#include <libdqmc/cx_dqmc_greens_replica_gs.hpp>
#include <libdqmc/cx_dqmc_greens_replica_renyi_gs.hpp>
#include <libdqmc/global.hpp>
#include <libdqmc/prefixed_out.hpp>
#include <libdqmc/cx_updates.hpp>
#include <libdqmc/tools.hpp>
#include <libdqmc/temper_package.hpp>
#include <libdqmc/cx_hopping.hpp>
#include <libdqmc/cx_density.hpp>
#include <libdqmc/cx_checkerboard.hpp>
#include <libdqmc/cx_compare_hopping_checkerboard.hpp>
// #include <libdqmc/cx_checkerboard_manual.hpp>
// #include <libdqmc/cx_measurements.hpp>
#include <complex>
// ALPS INCLUDES
#include <alps/lattice.h>
#include <alps/parapack/worker.h>
#include <alps/alea.h>
#include <alps/osiris.h>
#include <alps/osiris/dump.h>
#include <alps/expression.h>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

// Standard includes
#include <mpi.h>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <fstream>
#include <cstdlib>
#include <list>
#include <vector>
#include <ctime>
#include <climits>
#include <limits>
#include <cmath>
#include <numeric>
#include <map>
#include <exception>

#include <boost/multi_array.hpp>
#include <boost/filesystem.hpp>

using namespace std;

class cx_dqmc_replica_gs : public alps::parapack::mc_worker {

public:

    ///@{
    ~cx_dqmc_replica_gs() { }

    cx_dqmc_replica_gs(const alps::Parameters &);
    cx_dqmc_replica_gs(boost::mpi::communicator const& comm, const alps::Parameters &);

    void start(const alps::Parameters& params);
    double work_done(alps::ObservableSet& obs);
    bool is_thermalized() const { return steps.is_thermalized(); } ;
    void init_observables(alps::Parameters const &, alps::ObservableSet& obs);
    void run(alps::ObservableSet& obs);
    double progress() const { return steps.progress(); } ;

    alps::mc_steps steps;
    dqmc::parameters cx_dqmc_params;
    alps::RealObservable dummy;
    cx_dqmc::workspace * ws;
    cx_dqmc::workspace * sqr_ws;
    cx_dqmc::workspace * r_ws;
    cx_dqmc::workspace * r_ws_0;

    temper_package outgoing, incoming;

    /// The auxiliary spin arrays
    boost::multi_array<double, 3 > aux_spins;
    boost::multi_array<double, 3 > aux_spins_0;
    boost::multi_array<double, 3 > aux_spins_1;
    boost::multi_array<double, 3 > aux_spins_temp;

    int particles;
    int rank;
    double gamma;
    double T;
    double beta;
    double delta_tau;
    int slices;
    double hopping, hopping_perp;
    double interaction;
    int sweeps_measurement;

    double lambda;
    double gamma_table[2];

    // int bonds;
    int slice_steps;
    int start_slice;
    int end_slice;
    int volume;

    uint connected_0, connected_1, disconnected_0, disconnected_1;
    uint n_A_0, n_A_1, n_B_0, n_B_1;
    int max_flips;
    bool first_run;
    cx_dqmc::abstract_greens * gs_0_0;
    cx_dqmc::abstract_greens * gs_0_1;
    cx_dqmc::abstract_greens * gs_0_0_gen;
    cx_dqmc::abstract_greens * gs_0;
    cx_dqmc::abstract_greens * gs_1;

    int state;
    double log_det_ratio;
    pdouble_t det_up_0, det_down_0, det_up_1, det_down_1;
    pdouble_t det_up_0_0, det_down_0_0, det_up_0_1, det_down_0_1;
    pdouble_t last_det_0, last_det_1;
    cx_double sign_0_0, avg_sign_0_0;
    cx_double sign_0_1, avg_sign_0_1;
    cx_double sign_0, avg_sign_0;
    cx_double sign_1, avg_sign_1;

    pdouble_t log_ratio, ratio;
    pdouble_t local_moment, double_occupancy;
    int meas_counter;
    bool flipped;
    int test_runs;
    double alpha;
    double alpha_0;
    double alpha_1;

    bool state_0_done;
    bool state_1_done;

    bool grover;

    void switch_state(alps::ObservableSet& obs);
    bool gs_sweep(alps::ObservableSet& obs);
    bool gs_sweep_continuously_slice(alps::ObservableSet& obs);
    bool gs_sweep_continuously_chunk(alps::ObservableSet& obs);
    bool gs_sweep_continuously_complete(alps::ObservableSet& obs);
    double gs_delayed_spinful_flip(alps::ObservableSet& obs, int slice, bool at_once = true);
    double gs_delayed_continuous_spinful_flip(alps::ObservableSet& obs, int slice, vector<double>& switch_probs);
    double gs_simple_spinless_flip(alps::ObservableSet& obs, int slice);
    double gs_delayed_spinless_flip(alps::ObservableSet& obs, int slice);
    double gs_delayed_continuous_spinless_flip(alps::ObservableSet& obs, int slice, vector<double>& switch_probs);
    double gs_attractive_spinless_flip(alps::ObservableSet& obs, int slice);
    double gs_delayed_attractive_spinless_flip(alps::ObservableSet& obs, int slice);
    int gs_spinful_naive_flip(alps::ObservableSet& obs, int slice);
    bool package_ok;
    void TiePackage();
    void ComputeRatio();
    void Decide();
    void HandlePackage();

    void measure(alps::ObservableSet& obs);

    void randomize_spins() {
	for (uint s = 0; s < cx_dqmc_params.slices; s++) {
	    for (int i = 0; i < cx_dqmc_params.num_aux_spins; i++) {
		uniform_01() > 0.5 ?
		    aux_spins[0][s][i] = -1:
		    aux_spins[0][s][i] = 1;
		aux_spins_0[0][s][i] = aux_spins[0][s][i];
		aux_spins_1[0][s][i] = aux_spins[0][s][i];

		aux_spins[1][s][i] = aux_spins[0][s][i];;
		aux_spins_0[1][s][i] = aux_spins[1][s][i];
		aux_spins_1[1][s][i] = aux_spins[1][s][i];

	    }
	}
    };

    void ordered_spins() {
	for (uint s = 0; s < cx_dqmc_params.slices; s++) {
	    for (int i = 0; i < cx_dqmc_params.num_aux_spins; i++) {
		i % 2 == 0 ?
		    aux_spins[0][s][i] = -1:
		    aux_spins[0][s][i] = 1;

		aux_spins_0[0][s][i] = aux_spins[0][s][i];
		aux_spins_1[0][s][i] = aux_spins[0][s][i];

		aux_spins[1][s][i] = aux_spins[0][s][i];;
		aux_spins_0[1][s][i] = aux_spins[1][s][i];
		aux_spins_1[1][s][i] = aux_spins[1][s][i];

	    }
	}
    };

    void copy_aux_spins(boost::multi_array<double,3>& in,
			boost::multi_array<double, 3>& out) {
	for (int r = 0; r < 2; r++) {
	    for (int s = 0; s < cx_dqmc_params.slices; s++) {
		for (int l = 0; l < cx_dqmc_params.num_aux_spins; l++) {
		    out[r][s][l] = in[r][s][l];
		}
	    }
	}
    }

    std::string TimeString() {
	using namespace std;

	time_t timestamp;
	tm *now;
	timestamp = time(0);
	now = localtime(&timestamp);

	stringstream time_string;

	time_string << "[" << now->tm_year + 1900 << "-";
	if (now->tm_mon + 1 < 10) time_string << "0";
	time_string << now->tm_mon+1 << "-";
	if (now->tm_mday < 10) time_string << "0";
	time_string << now->tm_mday << " ";
	if (now->tm_hour < 10) time_string << "0";
	time_string << now->tm_hour << ":";
	if (now->tm_min < 10) time_string << "0";
	time_string << now->tm_min << ":";
	if (now->tm_sec < 10) time_string << "0";
	time_string << now->tm_sec << "]   ";

	return time_string.str();
    }

    void copy_to_gs_0() {
	for (int i = 0; i < gs_0_0->chunks; ++i) {
	    gs_0->u_stack[i] = gs_0_0->u_stack[i];
	    gs_0->d_stack[i] = gs_0_0->d_stack[i];
	    gs_0->t_stack[i] = gs_0_0->t_stack[i];
	}
	for (int i = 0; i < gs_0_1->chunks; ++i) {
	    gs_0->u_stack[i + gs_0_0->chunks] = gs_0_1->u_stack[i];
	    gs_0->d_stack[i + gs_0_0->chunks] = gs_0_1->d_stack[i];
	    gs_0->t_stack[i + gs_0_0->chunks] = gs_0_1->t_stack[i];
	}
	gs_0->idx = 0;
	gs_0->current_slice = 0;
	gs_0->direction = 1;
	gs_0->dir_switch = true;
	gs_0->propagate(dummy);
    }

    void copy_to_gs_1() {
	for (int i = 0; i < gs_0_0->chunks; ++i) {
	    gs_1->u_stack[i] = gs_0_0->u_stack[i];
	    gs_1->d_stack[i] = gs_0_0->d_stack[i];
	    gs_1->t_stack[i] = gs_0_0->t_stack[i];
	}
	for (int i = 0; i < gs_0_1->chunks; ++i) {
	    gs_1->u_stack[i + gs_0_0->chunks] = gs_0_1->u_stack[i];
	    gs_1->d_stack[i + gs_0_0->chunks] = gs_0_1->d_stack[i];
	    gs_1->t_stack[i + gs_0_0->chunks] = gs_0_1->t_stack[i];
	}
	gs_1->idx = 0;
	gs_1->current_slice = 0;
	gs_1->direction = 1;
	gs_1->dir_switch = true;
	gs_1->propagate(dummy);
    }

    void copy_to_gs_0x() {
	for (int i = 0; i < gs_0_0->chunks; ++i) {
	    gs_0_0->u_stack[i] = gs_1->u_stack[i];
	    gs_0_0->d_stack[i] = gs_1->d_stack[i];
	    gs_0_0->t_stack[i] = gs_1->t_stack[i];
	}
	for (int i = 0; i < gs_0_1->chunks; ++i) {
	    gs_0_1->u_stack[i] = gs_1->u_stack[i + gs_0_0->chunks];
	    gs_0_1->d_stack[i] = gs_1->d_stack[i + gs_0_0->chunks];
	    gs_0_1->t_stack[i] = gs_1->t_stack[i + gs_0_0->chunks];
	}
	gs_0_0->idx = 0;
	gs_0_0->current_slice = 0;
	gs_0_0->direction = 1;
	gs_0_0->dir_switch = true;
	gs_0_0->propagate(dummy);

	gs_0_1->idx = 0;
	gs_0_1->current_slice = 0;
	gs_0_1->direction = 1;
	gs_0_1->dir_switch = true;
	gs_0_1->propagate(dummy);
    }

    void load(alps::IDump& dump) {
	print_memory_usage();
	dump >> steps >> state;
	print_memory_usage();

	for (int r = 0; r < 2; r++) {
	    for (int s = 0; s < cx_dqmc_params.slices; s++) {
		for (int l = 0; l < cx_dqmc_params.num_aux_spins; l++) {
		    dump >> aux_spins_0[r][s][l];
		    dump >> aux_spins_1[r][s][l];
		}
	    }
	}

	if (state == 0) {
	    for (int a = 0; a < 2; a++) {
		for (uint s = 0; s < cx_dqmc_params.slices; s++) {
		    for (int i = 0; i < cx_dqmc_params.num_aux_spins; i++) {
			aux_spins[a][s][i] = aux_spins_0[a][s][i];
		    }
		}
	    }
	} else {
	    for (int a = 0; a < 2; a++) {
		for (uint s = 0; s < cx_dqmc_params.slices; s++) {
		    for (int i = 0; i < cx_dqmc_params.num_aux_spins; i++) {
			aux_spins[a][s][i] = aux_spins_1[a][s][i];
		    }
		}
	    }
	}

	out << "Monte Carlo Steps " << steps() << endl;
	print_memory_usage();

	gs_0_0->build_stack();
	gs_0_1->build_stack();
	gs_1->build_stack();
	copy_to_gs_1();
	// gs_1->propagate(dummy);

	if (cx_dqmc_params.partial_cuts == true) {
	    gs_0->build_stack();
	    copy_to_gs_0();
	    // gs_0->propagate(dummy);
	}

	out << "Retesting sweep stability - state is " << state << endl;
	{
	    boost::timer::auto_cpu_timer t;
	    for (uint i = 0; i < cx_dqmc_params.slices; i++) {
		gs_0_0->propagate(dummy);
		gs_0_1->propagate(dummy);
	    }
	}

	{
	    boost::timer::auto_cpu_timer t;
	    for (uint i = 0; i < cx_dqmc_params.slices; i++) {
		cx_dqmc_params.n_A = cx_dqmc_params.n_A_1;
		cx_dqmc_params.n_B = cx_dqmc_params.n_B_1;
		gs_1->propagate(dummy);
		gs_1->propagate(dummy);
	    }
	}

	if (cx_dqmc_params.partial_cuts == true) {
	    {
		cx_dqmc_params.n_A = cx_dqmc_params.n_A_0;
		cx_dqmc_params.n_B = cx_dqmc_params.n_B_0;

		boost::timer::auto_cpu_timer t;
		for (uint i = 0; i < 2 * cx_dqmc_params.slices; i++) {
		    gs_0->propagate(dummy);
		    gs_0->propagate(dummy);
		}
	    }
	}

	if (state == 0 && cx_dqmc_params.partial_cuts == true) {
	    cx_dqmc_params.n_A = cx_dqmc_params.n_A_0;
	    cx_dqmc_params.n_B = cx_dqmc_params.n_B_0;
	} else if (state == 1 && cx_dqmc_params.partial_cuts == true) {
	    cx_dqmc_params.n_A = cx_dqmc_params.n_A_1;
	    cx_dqmc_params.n_B = cx_dqmc_params.n_B_1;
	}
    }

    void save(alps::ODump& dump) const {
	dump << steps << state;

	for (int r = 0; r < 2; r++) {
	    for (int s = 0; s < cx_dqmc_params.slices; s++) {
		for (int l = 0; l < cx_dqmc_params.num_aux_spins; l++) {
		    dump << aux_spins_0[r][s][l];
		    dump << aux_spins_1[r][s][l];
		}
	    }
	}
    }

    void calculate_weights(bool recalculate = true) {
	if (recalculate == true) {
	    gs_0_0->build_stack();
	    gs_0_1->build_stack();
	    copy_to_gs_1();

	    if (cx_dqmc_params.partial_cuts == true) {
		copy_to_gs_0();
	    }
	}

	{
	    if (state == 0 && cx_dqmc_params.partial_cuts == false) {
		if (gs_0_0->get_slice() != 0)
		    dqmc::tools::abort("Called calculate_weights on the wrong slice of gs_0_0");
		if (gs_0_1->get_slice() != 0)
		    dqmc::tools::abort("Called calculate_weights on the wrong slice of gs_0_1");

		copy_to_gs_1();
		gs_1->log_weight();
	    } else if (state == 0 && cx_dqmc_params.partial_cuts == true) {
		if (gs_0->get_slice() != 0)
		    dqmc::tools::abort("Called measure on the wrong slice of gs_0");

		gs_0->log_weight();
		cx_dqmc_params.n_A = cx_dqmc_params.n_A_1;
		cx_dqmc_params.n_B = cx_dqmc_params.n_B_1;
		for (int i = 0; i < gs_0->chunks; ++i) {
		    gs_1->u_stack[i] = gs_0->u_stack[i];
		    gs_1->d_stack[i] = gs_0->d_stack[i];
		    gs_1->t_stack[i] = gs_0->t_stack[i];
		}
		gs_1->idx = 0;
		gs_1->current_slice = 0;
		gs_1->direction = 1;
		gs_1->dir_switch = true;
		gs_1->propagate(dummy);
		gs_1->log_weight();
		cx_dqmc_params.n_A = cx_dqmc_params.n_A_0;
		cx_dqmc_params.n_B = cx_dqmc_params.n_B_0;
	    } else if (state == 1) {
		if (gs_1->get_slice() != 0)
		    dqmc::tools::abort("Called measure on the wrong slice of gs_1");

		gs_1->log_weight();

		if (cx_dqmc_params.partial_cuts == true) {
		    cx_dqmc_params.n_A = cx_dqmc_params.n_A_0;
		    cx_dqmc_params.n_B = cx_dqmc_params.n_B_0;
		    for (int i = 0; i < gs_1->chunks; ++i) {
			gs_0->u_stack[i] = gs_1->u_stack[i];
			gs_0->d_stack[i] = gs_1->d_stack[i];
			gs_0->t_stack[i] = gs_1->t_stack[i];
		    }
		    gs_0->idx = 0;
		    gs_0->current_slice = 0;
		    gs_0->direction = 1;
		    gs_0->dir_switch = true;
		    gs_0->propagate(dummy);
		    gs_0->log_weight();
		    cx_dqmc_params.n_A = cx_dqmc_params.n_A_1;
		    cx_dqmc_params.n_B = cx_dqmc_params.n_B_1;
		}
		else {
		    copy_to_gs_0x();
		    gs_0_0->log_weight();
		    gs_0_1->log_weight();
		}
	    }
	}

	if (cx_dqmc_params.model_id <= cx_dqmc_params.CX_SPINFUL_HUBBARD_ALTERNATIVE) {
	    if (cx_dqmc_params.partial_cuts == false) {
		sign_0_0 = gs_0_0->det_sign * gs_0_0->det_sign * gs_0_0->phase;
		sign_0_1 = gs_0_1->det_sign * gs_0_1->det_sign * gs_0_1->phase;
		sign_0 = sign_0_0 * sign_0_1;
		sign_1 = gs_1->det_sign * gs_1->det_sign * gs_1->phase;
		det_up_0_0 = gs_0_0->get_log_det();
		det_up_0_1 = gs_0_1->get_log_det();
		det_up_0 = 2 * (det_up_0_0 + det_up_0_1);
		det_up_1 = 2 * gs_1->get_log_det();
	    } else {
		sign_0 = gs_0->det_sign * gs_0->det_sign * gs_0->phase;
		sign_1 = gs_1->det_sign * gs_1->det_sign * gs_1->phase;
		det_up_0 = 2 * gs_0->get_log_det();
		det_up_1 = 2 * gs_1->get_log_det();
	    }
	} else {
	    if (cx_dqmc_params.partial_cuts == false) {
		sign_0_0 = gs_0_0->det_sign * gs_0_0->phase;
		sign_0_1 = gs_0_1->det_sign * gs_0_1->phase;
		sign_0 = sign_0_0 * sign_0_1;
		sign_1 = gs_1->det_sign * gs_1->phase;
		det_up_0_0 = gs_0_0->get_log_det();
		det_up_0_1 = gs_0_1->get_log_det();
		det_up_0 = (det_up_0_0 + det_up_0_1);
		det_up_1 = gs_1->get_log_det();
	    } else {
		sign_0 = gs_0->det_sign * gs_0->phase;
		sign_1 = gs_1->det_sign * gs_1->phase;
		det_up_0 = gs_0->get_log_det();
		det_up_1 = gs_1->get_log_det();
	    }
	}
    }

    inline void print_memory_usage() {
	using namespace std;
	if (cx_dqmc_params.rank < 1) {
	    try {
		double vm, rss;
		dqmc::tools::process_mem_usage(vm, rss);
		out << "VM: " << vm << setw(5) << "" << "RSS: " << rss << endl;
	    } catch(std::exception const& ex) {
		out << "Determining memory usage failed\t" << ex.what() << endl;
	    }
	}
    }

    boost::mpi::communicator world;
    PrefixedOut out;
    PrefixedOut err;
};

#endif


#include "engine.hpp"
#include "box2dmessages.hpp"
#include "visco_elastic.hpp"
#include "generic/is_finite.hpp"
#include "spatial/2d/neighborhood_spatial_hashing.hpp"

#include <Eigen/Core>
#include <Eigen/Dense>

#include <boost/log/trivial.hpp>

namespace GooBalls {
namespace d2 {
namespace Physics {
using namespace Spatial;

Engine::Engine() {
    m_fluidSolver = std::make_unique<ViscoElastic>();
}

void Engine::fluidSolver(std::unique_ptr<FluidSolver>&& solver){
    m_fluidSolver = std::move(solver);
}

void Engine::initScene(Scene& scene){
    BOOST_LOG_TRIVIAL(trace) << "Physics Engine: initializing scene";
    for(auto& mesh : scene.meshes){
        mesh.prepare(scene.fluid->h());
        mesh.update_from_rigid_body();
    }
    meshToFluid(scene);
    m_fluidSolver->initFluid(scene);
    scene.world.SetGravity(b2Vec2(scene.gravity[0], scene.gravity[1]));
    BOOST_LOG_TRIVIAL(trace) << "Physics Engine: Scene initialized";
}

void Engine::meshToFluid(Scene& scene) const {
    int boundaryParticles = 0;
    for(const auto& mesh : scene.meshes){
        boundaryParticles += mesh.particles_position_local().rows();
    }
    if(boundaryParticles <= 0){
        return;
    }
    // transfer rigid body particles to fluid
    auto& boundary = scene.fluid->boundary_position();
    auto& volume = scene.fluid->boundary_volume();
    auto& velocity = scene.fluid->boundary_velocity();
    auto& mass = scene.fluid->boundary_mass();
    auto& velocityCorrection = scene.fluid->boundary_velocity_correction_coefficient();
    boundary.resize(boundaryParticles, Eigen::NoChange);
    volume.resize(boundaryParticles, Eigen::NoChange);
    velocity.resize(boundaryParticles, Eigen::NoChange);
    mass.resize(boundaryParticles, Eigen::NoChange);
    velocityCorrection.resize(boundaryParticles, Eigen::NoChange);
    int s = 0;
    for(size_t i = 0; i < scene.meshes.size(); ++i){
        auto& mesh = scene.meshes[i];
        const auto& local = mesh.particles_position_local();
        boundary.block(s, 0, local.rows(), 2) = (local * mesh.rotation().transpose()).rowwise() + mesh.translation();//(local * mesh.rotation().transpose()).rowwise() + mesh.translation(); // TODO: check order of arguments
        volume.block(s, 0, local.rows(), 1) = mesh.particles_volume();
        velocity.block(s, 0, local.rows(), 2) = mesh.particles_velocity();
        mass.block(s, 0, local.rows(), 1) = mesh.particles_mass();
        velocityCorrection.block(s, 0, local.rows(), 1).array() = mesh.particles_velocity_correction_coefficient();
        s += local.rows();
        /*
        if(mesh.particles_velocity().maxCoeff() > 50){
            BOOST_LOG_TRIVIAL(info) << "R" << i << ", Max rigid particle speed: " << mesh.particles_velocity().maxCoeff();
        }
        */
    }
}

void Engine::fluidToMesh(Scene& scene) const {
    if(scene.fluid->boundary_position().rows() <= 0){
        return;
    }
    int s = 0;
    for(size_t i = 0; i < scene.meshes.size(); ++i){
        auto& mesh = scene.meshes[i];
        const auto& local = mesh.particles_position_local();
        const auto& force = scene.fluid->boundary_force().block(s, 0, local.rows(), 2);
        const auto& pos = scene.fluid->boundary_position().block(s, 0, local.rows(), 2);
        s += local.rows();

        if(mesh.body->GetMass() == 0.0){
            //BOOST_LOG_TRIVIAL(info) << "Skipping R" << i;
            continue;
        }

        /*
        for(int i = 0; i < part.rows(); ++i){
            mesh.body->ApplyForce(b2Vec2(part(i,0)/pos.rows(), part(i,1)/pos.rows()), b2Vec2(pos(i,0), pos(i,1)), true);
        }
        //*/
        TranslationVector Frigid = force.colwise().mean();
        /*
        auto acc = force.array().colwise()/ms.array();
        if(Frigid.maxCoeff() > 100*1000){
            BOOST_LOG_TRIVIAL(info) << "R" << i << ", Frigid: " << Frigid[0] << ", " << Frigid[1];
        }
        if(acc.maxCoeff() > 1000*1000){
            BOOST_LOG_TRIVIAL(info) << "R" << i << ", Facc: " << acc.maxCoeff();
        }
        */
        mesh.body->ApplyForce(b2Vec2(Frigid[0], Frigid[1]), mesh.body->GetWorldCenter(), true);
        TranslationVector cog = TranslationVector(mesh.body->GetWorldCenter().x, mesh.body->GetWorldCenter().y);
        //cog = pos.colwise.mean();
        Coordinates2d centered = (pos.rowwise() - cog);

        /*
         * Cross product: a x b = (a2 b3 - a3b2, a3b1 - a1b3, a1b2 - a2b1)
         * = (a2 * 0 - 0 * b2, 0 * b1 - a1 * b, a1 b2 - a2 b1)
        */

        Float torque = (centered.col(0)*Frigid[1] - centered.col(1) * Frigid[0]).sum();
        mesh.body->ApplyTorque(torque, true);
    }
}

void Engine::advance(Scene& scene, TimeStep dt) {
    BOOST_LOG_TRIVIAL(trace) << "Physics Engine: advancing scene by " << dt;
    assert(is_finite(scene.fluid->particles_position()));
    for(auto& mesh : scene.meshes){
        if(mesh.body->GetType() != b2_kinematicBody){
            continue;
        }
        auto v = mesh.body->GetLinearVelocity();
        v = v + dt * mesh.linearAcceleration;
        auto t = mesh.body->GetAngularVelocity();
        t = t + dt * mesh.angularAcceleration;
        mesh.body->SetLinearVelocity(v);
        mesh.body->SetAngularVelocity(t);
    }
    scene.world.Step(dt, velocity_iterations, position_iterations);
    // The rigid bodies moved: use their transform to compute the local meshes
    for(auto& mesh : scene.meshes){
        mesh.update_from_rigid_body();
    }
    meshToFluid(scene);
    // solve fluid
    m_fluidSolver->advance(scene, dt);
    assert(scene.fluid->boundary_position().rows() == scene.fluid->boundary_force().rows());
    assert(is_finite(scene.fluid->particles_position()));

    fluidToMesh(scene);
}

void Engine::BeginContact(b2Contact* contact) {
    auto msg = std::shared_ptr<Box2DBeginContactMessage>();
    msg->contact = contact;
    sendMessage(msg);
}
void Engine::EndContact(b2Contact* contact){
    auto msg = std::shared_ptr<Box2DEndContactMessage>();
    msg->contact = contact;
    sendMessage(msg);
}
void Engine::PreSolve(b2Contact* contact, const b2Manifold* oldManifold){
    auto msg = std::shared_ptr<Box2DPreSolveMessage>();
    msg->contact = contact;
    msg->oldManifold = oldManifold;
    sendMessage(msg);
}
void Engine::PostSolve(b2Contact* contact, const b2ContactImpulse* impulse){
    auto msg = std::shared_ptr<Box2DPostSolveMessage>();
    msg->contact = contact;
    msg->impulse = impulse;
}

} // Physics
} // d2
} // GooBalls


#pragma once

#include "ga/ga.h"
#include "tamm/tamm.hpp"
#include <Eigen/Dense>

namespace tamm {

template<typename T>
inline void jacobi(ExecutionContext& ec, Tensor<T>& d_r, Tensor<T>& d_t,
                   T shift, bool transpose, std::vector<double>& evl_sorted,
                   const TAMM_SIZE& n_occ_alpha, const TAMM_SIZE& n_occ_beta) {
    // EXPECTS(transpose == false);
    block_for(ec, d_r(), [&](IndexVector blockid) {
        const TAMM_SIZE rsize = d_r.block_size(blockid);
        std::vector<T> rbuf(rsize);
        d_r.get(blockid, rbuf);

        const TAMM_SIZE tsize = d_t.block_size(blockid);

        std::vector<T> tbuf(tsize);

        auto& rtiss      = d_r.tiled_index_spaces();
        auto rblock_dims = d_r.block_dims(blockid);

        TAMM_SIZE noab = n_occ_alpha+n_occ_beta;
        std::vector<double> p_evl_sorted_occ(noab);
        std::vector<double> p_evl_sorted_virt(evl_sorted.size()-noab);
        std::copy(evl_sorted.begin(), evl_sorted.begin() + noab,
                  p_evl_sorted_occ.begin());
        std::copy(evl_sorted.begin() + noab, evl_sorted.end(),
                  p_evl_sorted_virt.begin());

        if(d_r.num_modes() == 2) {
            auto ioff  = rtiss[0].tile_offset(blockid[0]);
            auto joff  = rtiss[1].tile_offset(blockid[1]);
            auto isize = rblock_dims[0];
            auto jsize = rblock_dims[1];

            if(!transpose) {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (-p_evl_sorted_virt[ioff + i] +
                                     p_evl_sorted_occ[joff + j] + shift);
                    }
                }
            } else {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (p_evl_sorted_occ[ioff + i] -
                                     p_evl_sorted_virt[joff + j] + shift);
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else if(d_r.num_modes() == 4) {

            auto rblock_offset = d_r.block_offsets(blockid);

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (-p_evl_sorted_virt[ioff[0] + i0] -
                                   p_evl_sorted_virt[ioff[1] + i1] +
                                   p_evl_sorted_occ[ioff[2] + i2] +
                                   p_evl_sorted_occ[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            } else {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (p_evl_sorted_occ[ioff[0] + i0] +
                                   p_evl_sorted_occ[ioff[1] + i1] -
                                   p_evl_sorted_virt[ioff[2] + i2] -
                                   p_evl_sorted_virt[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else {
            assert(0); // @todo implement
        }
    });
    // GA_Sync();
}

template<typename T>
inline void jacobi_cs(ExecutionContext& ec, Tensor<T>& d_r, Tensor<T>& d_t,
                   T shift, bool transpose, std::vector<double>& evl_sorted,
                   const TAMM_SIZE& n_occ_alpha, const TAMM_SIZE& n_vir_alpha,
                   const bool not_spin_orbital=false) {
    // EXPECTS(transpose == false);
    block_for(ec, d_r(), [&](IndexVector blockid) {
        const TAMM_SIZE rsize = d_r.block_size(blockid);
        std::vector<T> rbuf(rsize);
        d_r.get(blockid, rbuf);

        const TAMM_SIZE tsize = d_t.block_size(blockid);

        std::vector<T> tbuf(tsize);

        auto& rtiss      = d_r.tiled_index_spaces();
        auto rblock_dims = d_r.block_dims(blockid);

        TAMM_SIZE noa  = n_occ_alpha;
        TAMM_SIZE noab = n_occ_alpha + n_occ_alpha;
        TAMM_SIZE nva  = n_vir_alpha;
        std::vector<double> p_evl_sorted_occ(noa);
        std::vector<double> p_evl_sorted_virt(nva);
        std::copy(evl_sorted.begin(), evl_sorted.begin() + noa,
                  p_evl_sorted_occ.begin());
        if(not_spin_orbital)
            std::copy(evl_sorted.begin() + noa, evl_sorted.begin() + noa + nva,
                        p_evl_sorted_virt.begin());
        else
            std::copy(evl_sorted.begin() + noab, evl_sorted.begin() + noab + nva,
                  p_evl_sorted_virt.begin());

        if(d_r.num_modes() == 2) {
            auto ioff  = rtiss[0].tile_offset(blockid[0]);
            auto joff  = rtiss[1].tile_offset(blockid[1]);
            auto isize = rblock_dims[0];
            auto jsize = rblock_dims[1];

            if(!transpose) {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (-p_evl_sorted_virt[ioff + i] +
                                     p_evl_sorted_occ[joff + j] + shift);
                    }
                }
            } else {
                for(auto i = 0U, c = 0U; i < isize; i++) {
                    for(auto j = 0U; j < jsize; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (p_evl_sorted_occ[ioff + i] -
                                     p_evl_sorted_virt[joff + j] + shift);
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else if(d_r.num_modes() == 4) {

            auto rblock_offset = d_r.block_offsets(blockid);

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (-p_evl_sorted_virt[ioff[0] + i0] -
                                   p_evl_sorted_virt[ioff[1] + i1] +
                                   p_evl_sorted_occ[ioff[2] + i2] +
                                   p_evl_sorted_occ[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            } else {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (p_evl_sorted_occ[ioff[0] + i0] +
                                   p_evl_sorted_occ[ioff[1] + i1] -
                                   p_evl_sorted_virt[ioff[2] + i2] -
                                   p_evl_sorted_virt[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            }
            d_t.add(blockid, tbuf);
        } else {
            assert(0); // @todo implement
        }
    });
    // GA_Sync();
}

template<typename T>
inline void jacobi_eom(ExecutionContext& ec, LabeledTensor<T> d_r_lt, LabeledTensor<T> d_t_lt,
                      T shift, bool transpose, std::vector<double>& evl_sorted, const TAMM_SIZE n_occ_alpha, const TAMM_SIZE n_occ_beta) {
    // EXPECTS(transpose == false);
     Tensor<T> d_r = d_r_lt.tensor();
     Tensor<T> d_t = d_t_lt.tensor();

    block_for(ec, d_r_lt, [&](IndexVector bid) {
        IndexVector blockid   = internal::translate_blockid(bid, d_r_lt);

        const TAMM_SIZE rsize = d_r.block_size(blockid);
        std::vector<T> rbuf(rsize);
        d_r.get(blockid, rbuf);

        const TAMM_SIZE tsize = d_t.block_size(blockid);

        std::vector<T> tbuf(tsize);

        // auto& rtiss      = d_r.tiled_index_spaces();
        auto rblock_dims = d_r.block_dims(blockid);
        auto rblock_offset = d_r.block_offsets(blockid);

        TAMM_SIZE noab = n_occ_alpha+n_occ_beta;
        std::vector<double> p_evl_sorted_occ(noab);
        std::vector<double> p_evl_sorted_virt(evl_sorted.size()-noab);
        std::copy(evl_sorted.begin(), evl_sorted.begin() + noab,
                  p_evl_sorted_occ.begin());
        std::copy(evl_sorted.begin() + noab, evl_sorted.end(),
                  p_evl_sorted_virt.begin());

        if(d_r.num_modes() == 3) {

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i = 0U, c = 0U; i < isize[0]; i++) {
                    for(auto j = 0U; j < isize[1]; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (-p_evl_sorted_virt[ioff[0] + i] +
                                     p_evl_sorted_occ[ioff[1] + j] + shift);
                    }
                }
            } else {
                for(auto i = 0U, c = 0U; i < isize[0]; i++) {
                    for(auto j = 0U; j < isize[1]; j++, c++) {
                        tbuf[c] =
                          rbuf[c] / (p_evl_sorted_occ[ioff[0] + i] -
                                     p_evl_sorted_virt[ioff[1] + j] + shift);
                    }
                }
            }
            // auto last_id = rtiss[2].translate(blockid[2], d_t.tiled_index_spaces()[2]);
            // blockid[2] = last_id;
            d_t.add(blockid, tbuf);
        } else if(d_r.num_modes() == 5) {

            std::vector<size_t> ioff;
            for(auto x : rblock_offset) { ioff.push_back(x); }
            std::vector<size_t> isize;
            for(auto x : rblock_dims) { isize.push_back(x); }

            if(!transpose) {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (-p_evl_sorted_virt[ioff[0] + i0] -
                                   p_evl_sorted_virt[ioff[1] + i1] +
                                   p_evl_sorted_occ[ioff[2] + i2] +
                                   p_evl_sorted_occ[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            } else {
                for(auto i0 = 0U, c = 0U; i0 < isize[0]; i0++) {
                    for(auto i1 = 0U; i1 < isize[1]; i1++) {
                        for(auto i2 = 0U; i2 < isize[2]; i2++) {
                            for(auto i3 = 0U; i3 < isize[3]; i3++, c++) {
                                tbuf[c] =
                                  rbuf[c] /
                                  (p_evl_sorted_occ[ioff[0] + i0] +
                                   p_evl_sorted_occ[ioff[1] + i1] -
                                   p_evl_sorted_virt[ioff[2] + i2] -
                                   p_evl_sorted_virt[ioff[3] + i3] + shift);
                            }
                        }
                    }
                }
            }
            // auto last_id = rtiss[4].translate(blockid[4], d_t.tiled_index_spaces()[4]);
            // blockid[4] = last_id;
            d_t.add(blockid, tbuf);
        } else {
            assert(0); // @todo implement
        }
    });
    // GA_Sync();
}

/**
 * @brief dot product between data held in two labeled tensors. Corresponding
 * elements are multiplied.
 *
 * This routine ignores permutation symmetry, and associated symmetrizatin
 * factors
 *
 * @tparam T Type of elements in both tensors
 * @param ec Execution context in which this function is invoked
 * @param lta Labeled tensor A
 * @param ltb labeled Tensor B
 * @return dot product A . B
 */
template<typename T>
inline T ddot(ExecutionContext& ec, LabeledTensor<T> lta,
              LabeledTensor<T> ltb) {
    T ret = 0;
    block_for(ec.pg(), lta, [&](IndexVector blockid) {
        Tensor<T> atensor     = lta.tensor();
        const TAMM_SIZE asize = atensor.block_size(blockid);
        std::vector<T> abuf(asize);

        Tensor<T> btensor     = ltb.tensor();
        const TAMM_SIZE bsize = btensor.block_size(blockid);
        std::vector<T> bbuf(bsize);

        const size_t sz = asize;
        for(size_t i = 0; i < sz; i++) { ret += abuf[i] * bbuf[i]; }
    });
    return ret;
}

/**
 * @brief DIIS routine
 * @tparam T Type of element in each tensor
 * @param ec Execution context in which this function invoked
 * @param[in] d_rs Vector of R tensors
 * @param[in] d_ts Vector of T tensors
 * @param[out] d_t Vector of T tensors produced by DIIS
 * @pre d_rs.size() == d_ts.size()
 * @pre 0<=i<d_rs.size(): d_rs[i].size() == d_t.size()
 * @pre 0<=i<d_ts.size(): d_ts[i].size() == d_t.size()
 */
template<typename T>
inline void diis(ExecutionContext& ec,
                 std::vector<std::vector<Tensor<T>>>& d_rs,
                 std::vector<std::vector<Tensor<T>>>& d_ts,
                 std::vector<Tensor<T>> d_t) {

    EXPECTS(d_t.size() == d_rs.size());
    size_t ntensors = d_t.size();
    EXPECTS(ntensors > 0);
    size_t ndiis = d_rs[0].size();
    EXPECTS(ndiis > 0);
    for(auto i = 0U; i < ntensors; i++) { EXPECTS(d_rs[i].size() == ndiis); }

    using Matrix =
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    using Vector =
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    Matrix A = Matrix::Zero(ndiis + 1, ndiis + 1);
    Vector b = Vector::Zero(ndiis + 1, 1);
    for(auto k = 0U; k < ntensors; k++) {
        for(auto i = 0U; i < ndiis; i++) {
            for(auto j = i; j < ndiis; j++) {
                Tensor<T> d_r1{};
                Tensor<T>::allocate(&ec,d_r1);
                Tensor<T>& t1 = d_rs[k].at(i);
                Tensor<T>& t2 = d_rs[k].at(j);
                //A(i, j) += ddot(ec, (*d_rs[k]->at(i))(), (*d_rs[k]->at(j))());
                Scheduler{ec}(d_r1() = t1() * t2()).execute();

                A(i,j) += get_scalar(d_r1);
                Tensor<T>::deallocate(d_r1);
            }
        }
    }

    for(auto i = 0U; i < ndiis; i++) {
        for(auto j = i; j < ndiis; j++) { A(j, i) = A(i, j); }
    }
    for(auto i = 0U; i < ndiis; i++) {
        A(i, ndiis) = -1.0;
        A(ndiis, i) = -1.0;
    }

    b(ndiis, 0) = -1;

    // Solve AX = B
    // call dgesv(diis+1,1,a,maxdiis+1,iwork,b,maxdiis+1,info)
    // Vector x = A.colPivHouseholderQr().solve(b);
    Vector x = A.lu().solve(b);

    auto sch = Scheduler{ec};
    for(auto k = 0U; k < ntensors; k++) {
        Tensor<T>& dt = d_t[k];
        sch(dt() = 0);
        for(auto j = 0U; j < ndiis; j++) {
            auto& tb = d_ts[k].at(j);
            sch(dt() += x(j, 0) * tb());
        }
    }
    // GA_Sync();
    sch.execute();
    // GA_Sync();
}

} // namespace tamm


#include <boost/math/complex.hpp>


#include "cluster.h"

#include "agency_rating.h"
#include "util.h"

#include <boost/range/algorithm/nth_element.hpp>
#include <Eigen/Core>

#include <cassert>
#include <cmath>
#include <set>
#include <vector>

void TNewsCluster::AddDocument(const TDbDocument& document) {
    Documents.push_back(std::move(document));
    FreshestTimestamp = std::max(FreshestTimestamp, static_cast<uint64_t>(Documents.back().FetchTime));
}

uint64_t TNewsCluster::GetTimestamp(float percentile) const {
    assert(!Documents.empty());
    std::vector<uint64_t> clusterTimestamps;
    clusterTimestamps.reserve(Documents.size());
    for (const TDbDocument& doc : Documents) {
        clusterTimestamps.push_back(doc.FetchTime);
    }
    size_t index = static_cast<size_t>(std::floor(percentile * (clusterTimestamps.size() - 1)));
    boost::range::nth_element(clusterTimestamps, clusterTimestamps.begin() + index);
    return clusterTimestamps[index];
}

void TNewsCluster::Summarize(const TAgencyRating& agencyRating) {
    assert(GetSize() != 0);
    const auto embeddingKey = (GetLanguage() == tg::LN_RU ? tg::EK_FASTTEXT_TITLE : tg::EK_FASTTEXT_CLASSIC);
    const size_t embeddingSize = Documents.back().Embeddings.at(embeddingKey).size();
    Eigen::MatrixXf points(GetSize(), embeddingSize);
    for (size_t i = 0; i < GetSize(); i++) {
        auto embedding = Documents[i].Embeddings.at(embeddingKey);
        Eigen::Map<Eigen::VectorXf, Eigen::Unaligned> eigenVector(embedding.data(), embedding.size());
        points.row(i) = eigenVector / eigenVector.norm();
    }
    Eigen::MatrixXf docsCosine = points * points.transpose();

    std::vector<double> weights;
    weights.reserve(GetSize());
    uint64_t freshestTimestamp = GetFreshestTimestamp();
    for (size_t i = 0; i < GetSize(); ++i) {
        const TDbDocument& doc = Documents[i];
        double docRelevance = docsCosine.row(i).mean();
        int64_t timeDiff = static_cast<int64_t>(doc.FetchTime) - static_cast<int64_t>(freshestTimestamp);
        double timeMultiplier = Sigmoid(static_cast<double>(timeDiff) / 3600.0 + 12.0);
        double agencyScore = agencyRating.ScoreUrl(doc.Url);
        double weight = (agencyScore + docRelevance) * timeMultiplier;
        if (doc.Nasty) {
            weight *= 0.5;
        }
        weights.push_back(weight);
    }
    SortByWeights(weights);
}

void TNewsCluster::CalcFeatures(
    const TAlexaAgencyRating& alexaRating,
    const std::vector<TDbDocument>& docs)
{
    Features.reserve(3*4*6 + 2*4*6);
    const char* codes[] = {"US", "GB", "IN", "RU", "CA", "AU"};
    const double decays[] = {1800., 3600., 7200., 86400.};
    const double shifts[] = {1., 1.3, 1.6};
    for (double shift : shifts) {
        for (double decay : decays) {
            auto slice = CalcImportance(alexaRating, docs, tg::LN_EN, RT_LOG, shift, decay);
            Features.push_back(slice.Importance);
            if (decay != 86400.) {
                continue;
            }
            for (const std::string& code : codes) {
                Features.push_back(slice.WeightedCountryShare[code]);
            }
        }
    }
    for (ERatingType type : {RT_RAW, RT_ONE}) {
        for (double decay : decays) {
            auto slice = CalcImportance(alexaRating, docs, tg::LN_EN, type, 0.0, decay);
            Features.push_back(slice.Importance);
            if (decay != 86400.) {
                continue;
            }
            for (const std::string& code : codes) {
                Features.push_back(slice.WeightedCountryShare[code]);
            }
        }
    }
}

TSliceFeatures TNewsCluster::CalcImportance(
    const TAlexaAgencyRating& alexaRating,
    const std::vector<TDbDocument>& docs,
    tg::ELanguage language,
    ERatingType type,
    double shift,
    double decay)
{
    double count = 0;
    double wCount = 0;
    TSliceFeatures slice;

    const char* codes[] = {"US", "GB", "IN", "RU", "CA", "AU"};
    for (const std::string& code : codes) {
        slice.CountryShare[code] = 0;
        slice.WeightedCountryShare[code] = 0;
    }

    slice.DocWeights.reserve(GetSize());
    for (const TDbDocument& doc : Documents) {
        double agencyWeight = alexaRating.ScoreUrl(doc.Host, language, type, shift);
        slice.DocWeights.push_back(agencyWeight);

        const std::string& docHost = doc.Host;;
        for (const std::string& code : codes) {
            double share = alexaRating.GetCountryShare(docHost, code);
            slice.CountryShare[code] += share;
            slice.WeightedCountryShare[code] += share * agencyWeight;
        }
        count += 1;
        wCount += agencyWeight;
    }

    for (const std::string& code : codes) {
        if (count > 0) {
            slice.CountryShare[code] /= count;
        }
        if (wCount > 0) {
            slice.WeightedCountryShare[code] /= wCount;
        }
    }

    for (size_t i = 0; i < docs.size(); ++i) {
        const TDbDocument& startDoc = docs[i];
        int32_t startTime = startDoc.FetchTime;
        double rank = 0.;

        std::set<std::string> seenHosts;
        for (size_t j = i; j < docs.size(); ++j) {
            const TDbDocument& doc = docs[j];
            const std::string& docHost = doc.Host;
            if (seenHosts.insert(docHost).second) {
                double agencyWeight = alexaRating.ScoreUrl(docHost, language, type, shift);
                double docTimestampRemapped = static_cast<double>(startTime - static_cast<int32_t>(doc.FetchTime)) / decay;
                double timeMultiplier = Sigmoid(std::max(docTimestampRemapped, -15.));
                double score = agencyWeight * timeMultiplier;
                rank += score;
            }
        }
        if (rank > slice.Importance) {
            slice.Importance = rank;
            slice.BestTimestamp = startDoc.FetchTime;
        }
    }
    return slice;
}

void TNewsCluster::CalcImportance(const TAlexaAgencyRating& alexaRating) {
    auto docs = GetDocuments();
    std::stable_sort(docs.begin(), docs.end(), [](const TDbDocument& p1, const TDbDocument& p2) {
        if (p1.FetchTime != p2.FetchTime) {
            return p1.FetchTime < p2.FetchTime;
        }
        return p1.Url < p2.Url;
    });
    CalcFeatures(alexaRating, docs);
    TSliceFeatures slice = CalcImportance(alexaRating, docs, tg::LN_EN, RT_LOG, 1., 3600);
    BestTimestamp = slice.BestTimestamp;
    Importance = slice.Importance;
    DocWeights = slice.DocWeights;
    CountryShare = slice.CountryShare;
    WeightedCountryShare = slice.WeightedCountryShare;
}

void TNewsCluster::CalcCategory() {
    std::vector<size_t> categoryCount(tg::ECategory_ARRAYSIZE);
    for (const TDbDocument& doc : Documents) {
        tg::ECategory docCategory = doc.Category;
        assert(doc.IsNews());
        categoryCount[static_cast<size_t>(docCategory)] += 1;
    }
    auto it = std::max_element(categoryCount.begin(), categoryCount.end());
    Category = static_cast<tg::ECategory>(std::distance(categoryCount.begin(), it));
}

void TNewsCluster::SortByWeights(const std::vector<double>& weights) {
    std::vector<std::pair<TDbDocument, double>> weightedDocs;
    weightedDocs.reserve(Documents.size());
    for (size_t i = 0; i < Documents.size(); i++) {
        weightedDocs.emplace_back(std::move(Documents[i]), weights[i]);
    }
    Documents.clear();
    std::stable_sort(weightedDocs.begin(), weightedDocs.end(), [](
        const std::pair<TDbDocument, double>& a,
        const std::pair<TDbDocument, double>& b)
    {
        if (std::abs(a.second - b.second) < 0.000001) {
            return a.first.Title < b.first.Title;
        }
        return a.second > b.second;
    });
    for (const auto& [doc, _] : weightedDocs) {
        AddDocument(doc);
    }
}

bool TNewsCluster::operator<(const TNewsCluster& other) const {
    if (FreshestTimestamp == other.FreshestTimestamp) {
        return Id < other.Id;
    }
    return FreshestTimestamp < other.FreshestTimestamp;
}

bool TNewsCluster::Compare(const TNewsCluster& cluster, uint64_t timestamp) {
    return cluster.FreshestTimestamp < timestamp;
}


/*
 * test_gladys.cpp
 *
 * Test the Graph Library for Autonomous and Dynamic Systems
 *
 * author:  Pierrick Koch <pierrick.koch@laas.fr>
 * created: 2013-07-01
 * license: BSD
 */
#define BOOST_TEST_MODULE const_string test
#include <boost/test/included/unit_test.hpp>

#include <string>
#include <sstream>

#include "gdalwrap/gdal.hpp"
#include "gladys/gladys.hpp"
#include "gladys/nav_graph.hpp"

BOOST_AUTO_TEST_SUITE( gladys )

BOOST_AUTO_TEST_CASE( test_raster_to_graph )
{
    std::string region_path = "/tmp/test_gladys_raster_to_graph.tif";
    std::string weight_path = "/tmp/test_gladys_raster_to_graph_nav.tif";
    std::string robotm_path = "/tmp/robot.json";
    std::string graphv_path = "/tmp/test_gladys_raster_to_graph_nav.dot";

    // create a robot model (JSON configuration file)
    std::ofstream robot_cfg(robotm_path);
    robot_cfg<<"{\"robot\":{\"mass\":1.0,\"radius\":1.0,\"velocity\":1.0}}";
    robot_cfg.close();

    // create a region map (GeoTiff image)
    gdalwrap::gdal region;
    region.set_size(4, 9, 9);
    // name bands
    region.names = {"NO_3D_CLASS", "FLAT", "OBSTACLE", "ROUGH"};
    // add an obstacle at the center of the map
    region.bands[1].assign(9*9, 1);
    for ( int i=1 ; i < 9 ; i++ ) {
        region.bands[1][i+5*9] = 0.2 ;
        region.bands[2][i+5*9] = 0.8 ;
    }

    region.save(region_path);

    // create a navigation graph from the map
    weight_map wm(region_path, robotm_path);
    nav_graph ng(wm);
    std::ostringstream oss_graphviz;
    ng.write_graphviz(oss_graphviz);
    ng.write_graphviz(graphv_path); // for debug
    ng.save(weight_path);

    BOOST_TEST_MESSAGE( "oss_graphviz.size() = " << oss_graphviz.str().size() );
    BOOST_CHECK_EQUAL( oss_graphviz.str().size() , 8335 );

    point_xy_t p1 = {1, 1};
    point_xy_t p2 = {5, 9};
    path_t path = ng.astar_search(p1, p2);
    // g++ >= 4.7 or clang >= 3.1 : support initializer_list
    BOOST_TEST_MESSAGE( "path: " + to_string(path) );
    BOOST_CHECK_EQUAL( path.size() , 14 );
    point_xy_t check_point = {1.0, 6.5} ;
    BOOST_CHECK_EQUAL( path[7][0], check_point[0] );
    BOOST_CHECK_EQUAL( path[7][1], check_point[1] );

    point_xy_t p3 = {7, 9};
    points_t goals = {p1, p2, p3};
    std::vector<double> costs = ng.single_source_all_costs(p1, goals);
    BOOST_CHECK_EQUAL( costs[0], ng.astar_search(points_t({p1}), points_t({goals[0]})).cost);
    BOOST_CHECK_EQUAL( costs[1], ng.astar_search(points_t({p1}), points_t({goals[1]})).cost);
    BOOST_CHECK_EQUAL( costs[2], ng.astar_search(points_t({p1}), points_t({goals[2]})).cost);

    robot_cfg.open(robotm_path);
    robot_cfg   << "{"
    << "\"robot\":{\"mass\":1.0,\"radius\":2.0,\"velocity\":1.0},"
    << "\"sensor\":{\"range\":20.0,\"fov\":6.28,"
    <<   "\"pose\":{\"x\":0.1,\"y\":0.2,\"z\":0.7,\"t\":0.0}"
    << "},"
    << "\"antenna\":{\"range\":5.0,\"fov\":6.28,"
    <<   "\"pose\":{\"x\":0.1,\"y\":0.2,\"z\":0.7,\"t\":0.0}"
    << "}"
    << "}" ;
    robot_cfg.close();

    std::string dtm_path = "/tmp/test_gladys_dtm.tif";
    gdalwrap::gdal dtm;
    dtm.set_size(2, 9, 9);
    dtm.names = {"Z_MIN", "Z_MAX"};
    dtm.save(dtm_path);
    gladys obj(region_path, dtm_path, robotm_path);
    points_t start = {p1};
    points_t goal  = {p2};
    path_cost_util_t pcu = obj.navigation(start, goal);
    BOOST_TEST_MESSAGE( "pcu path: " + to_string(pcu.path) );
    BOOST_CHECK_EQUAL( path.size() , pcu.path.size() );

    // DTM from test_visibility
    dtm.clear();
    dtm.set_size(2, 9, 9);
    // add a small wall in the middle of the map
    dtm.names[0] = "Z_MAX";
    dtm.get_band("Z_MAX").assign(9*9, 0.5);
    for (int i=0 ; i<9 ; i++ )
    dtm.get_band("Z_MAX")[ 5 + i*9 ] = 1.3;
    // two special points to observe + the observer point
    dtm.get_band("Z_MAX")[ 8 + 0*9 ] = 1.9;
    dtm.get_band("Z_MAX")[ 8 + 8*9 ] = 1.1;
    dtm.get_band("Z_MAX")[ 0 + 5*9 ] = 0.6;
    // add a small band of never-observed points
    dtm.names[1] = "N_POINTS";
    dtm.get_band("N_POINTS").assign(9*9, 5.);
    for (int i=0 ; i<9 ; i++ )
    dtm.get_band("N_POINTS")[ 3 + i*9 ] = 0.0;
    // one special point to observe
    dtm.get_band("N_POINTS")[ 8 + 5*9 ] = 0.0;

    dtm.save(dtm_path);

    gladys g(region_path, dtm_path, robotm_path);

    point_xy_t pS = {0, 5};
    point_xy_t pT = {8, 0};

    //Test different range for sensor and antenna
    bool b = g.is_visible(pS, pT);
    BOOST_CHECK_EQUAL(b, true);

    b = g.can_communicate(pS, pT);
    BOOST_CHECK_EQUAL(b, false);
}

BOOST_AUTO_TEST_SUITE_END();


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120716    A. Ronse          File created.
 *
 *    References
 *      [1] J.R. Clynch, Introduction to Earth Gravity, March 2006,
 *          http://www.gmat.unsw.edu.au/snap/gps/clynch_pdfs/intgrav.pdf
 *      [2] WolframAlpha: computational knowledge engine, coriolis calculator
 *          http://www.wolframalpha.com/input/?i=coriolis+effect&a=*C.coriolis+effect-_*Formula-
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/function.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/ReferenceFrames/apparentAccelerationModel.h"

namespace tudat
{
namespace unit_tests
{

using reference_frames::computeApparentAcceleration;
using reference_frames::ApparentAccelerationModel;

BOOST_AUTO_TEST_SUITE( test_reference_frame_transformations )

// Summary of tests.
// Test 1: Check apparent acceleration of static object at Earth equator (=only centrifugal).
// Test 2: Check apparent acceleration of moving object at Earth pole (=only Coriolis).
// Test 3: Check apparent acceleration of moving object in an accelerating, rotational reference
//         frame.
// Test 4: Check apparent acceleration of moving object in inertial reference frame (=0).
// Test 5: Test class implementation.

//! Test apparent acceleration of static object at Earth equator (=only centrifugal).
// Reference data from [1] (page 5), recomputed using Excel for higher precision.
BOOST_AUTO_TEST_CASE( testCentrifugalAccelerationEarth )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationRate( 0.0, 0.0, 7.29212351699e-5 );
    const Eigen::Vector3d rotationAcceleration = Eigen::Vector3d::Zero( );

    // Specify state.
    const Eigen::Vector3d position( 6378000.0, 0.0, 0.0 );
    const Eigen::Vector3d velocity = Eigen::Vector3d::Zero( );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( 0.0339150567038567, 0.0, 0.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( apparentAcceleration, expectedAcceleration, 1.0e-12 );
}

//! Test apparent acceleration of moving object at Earth pole (=only Coriolis).
// Reference data from pen/paper analysis, checked using [2].
BOOST_AUTO_TEST_CASE( testCoriolisAccelerationAtEarthPoles )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationRate( 0.0, 0.0, 7.29212351699e-5 );
    const Eigen::Vector3d rotationAcceleration = Eigen::Vector3d::Zero( );

    // Specify state.
    const Eigen::Vector3d position( 0.0, 0.0, 6378000.0 );
    const Eigen::Vector3d velocity( 10.0, 0.0, 0.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( 0.0, -0.0014584247033981, 0.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( apparentAcceleration, expectedAcceleration, 1.0e-13 );
}

//! Test apparent acceleration of moving object in a general non-inertial reference frame.
// Checked with computations performed in MATLAB.
BOOST_AUTO_TEST_CASE( testApparentAccelerationGeneral )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration( 1.0, 2.0, 3.0 );
    const Eigen::Vector3d rotationRate( 0.1, 0.2, 0.3 );
    const Eigen::Vector3d rotationAcceleration( 0.03, 0.02, 0.01 );

    // Specify state.
    const Eigen::Vector3d position( 100.0, 300.0, 500.0 );
    const Eigen::Vector3d velocity( 10.0, 40.0, 25.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( -2.0, 9.0, -10.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( apparentAcceleration, expectedAcceleration, 1.0e-14 );
}

//! Test apparent acceleration of moving object in an inertial reference frame (=0).
BOOST_AUTO_TEST_CASE( testApparentAccelerationStatic )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationRate = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationAcceleration = Eigen::Vector3d::Zero( );

    // Specify state.
    const Eigen::Vector3d position( 100.0, 300.0, 500.0 );
    const Eigen::Vector3d velocity( 10.0, 40.0, 25.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( 0.0, 0.0, 0.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_BASE( apparentAcceleration, expectedAcceleration );
}

//! Test class implementation of apparent acceleration model.
BOOST_AUTO_TEST_CASE( testApparentAccelerationClass )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration( 1.0, 2.0, 3.0 );
    const Eigen::Vector3d rotationRate( 0.1, 0.2, 0.3 );
    const Eigen::Vector3d rotationAcceleration( 0.03, 0.02, 0.01 );
    const Eigen::Vector3d position( 100.0, 300.0, 500.0 );
    const Eigen::Vector3d velocity( 10.0, 40.0, 25.0 );

    // Create functions returning the parameters defined above.
    boost::function< Eigen::Vector3d( ) > accelerationFunction =
            boost::lambda::constant( referenceFrameAcceleration );
    boost::function< Eigen::Vector3d( ) > rotationRateFunction =
            boost::lambda::constant( rotationRate );
    boost::function< Eigen::Vector3d( ) > rotationAccelerationFunction =
            boost::lambda::constant( rotationAcceleration );
    boost::function< Eigen::Vector3d( ) > positionFunction =
            boost::lambda::constant( position );
    boost::function< Eigen::Vector3d( ) > velocityFunction =
            boost::lambda::constant( velocity );

    // Create object of apparentAcceleraitonModel class; pass functions in constructor
    ApparentAccelerationModel apparentAccelerationModel(
           accelerationFunction, rotationRateFunction, rotationAccelerationFunction,
           positionFunction, velocityFunction );
    const Eigen::Vector3d expectedAcceleration( -2.0, 9.0, -10.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d computedAcceleration = apparentAccelerationModel.getAcceleration( );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedAcceleration,
                                       expectedAcceleration, 1.0e-14 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//
// Created by odderikf on 11/8/18.
//

#ifndef OVING12_HUFFMAN_TREE_HPP
#define OVING12_HUFFMAN_TREE_HPP

#include <vector>
#include <zconf.h>
#include <boost/shared_ptr.hpp>
#include <fstream>

class huffman_tree {
public:
    class tree{
    public:
        tree *parent = nullptr;
        tree *left = nullptr;
        tree *right = nullptr;
        uint64_t value;
        char *character = nullptr;
        explicit tree(uint64_t value) : value(value){};
        ~tree(){
            delete left;
            delete right;
            delete character;
        }
    };

    std::vector<bool> findpath(const unsigned char &);
    std::vector<tree*> characters;
    tree* root;
    std::vector<std::vector<bool>> paths;

    explicit huffman_tree(std::vector<uint64_t> counts);
    ~huffman_tree(){
        delete root;
    }

    enum direction{
        LEFT = 0,
        RIGHT = 1
    };
};

#endif //OVING12_HUFFMAN_TREE_HPP


#include <gtest/gtest.h>
#include <iostream>
#include <chrono>
#include <boost/multiprecision/gmp.hpp>
#include "bigint.h"

template <typename BIGINT>
void addition_bench() {
    std::srand(std::time(nullptr));
    size_t n_reps = 5;
    auto start = std::chrono::high_resolution_clock::now();
    for (int length = 1; length < 100; length += 10) {
        std::string s1;
        for (int i = 0; i < length; i++) {
            s1.push_back((std::rand() % 10) +'0');
        }
        std::string s2;
        for (int i = 0; i < length; i++) {
            s2.push_back((std::rand() % 10) +'0');
        }
        if (s1[0] == '0') {
            s1[0] = ((std::rand() % 9 + 1) +'0');
        }
        if (s2[0] == '0') {
            s2[0] = ((std::rand() % 9 + 1) +'0');
        }
        BIGINT bigint1{s1};
        BIGINT bigint2{s2};
        for (int i = 0; i < n_reps; i++) {
            bigint1 + bigint2;
        }
    }
    auto finish = std::chrono::high_resolution_clock::now();
    auto us = std::chrono::duration_cast<std::chrono::microseconds>(finish-start);
    std::cout << "Finished in " << us.count() << " us."  << std::endl;
}

template <typename BIGINT>
void subtraction_bench() {
    std::srand(std::time(nullptr));
    size_t n_reps = 5;
    auto start = std::chrono::high_resolution_clock::now();
    for (int length = 1; length < 100; length += 10) {
        std::string s1;
        for (int i = 0; i < length; i++) {
            s1.push_back((std::rand() % 10) +'0');
        }
        std::string s2;
        for (int i = 0; i < length; i++) {
            s2.push_back((std::rand() % 10) +'0');
        }
        if (s1[0] == '0') {
            s1[0] = ((std::rand() % 9 + 1) +'0');
        }
        if (s2[0] == '0') {
            s2[0] = ((std::rand() % 9 + 1) +'0');
        }
        BIGINT bigint1{s1};
        BIGINT bigint2{s2};
        for (int i = 0; i < n_reps; i++) {
            bigint1 + bigint2;
        }
    }
    auto finish = std::chrono::high_resolution_clock::now();
    auto us = std::chrono::duration_cast<std::chrono::microseconds>(finish-start);
    std::cout << "Finished in " << us.count() << " us."  << std::endl;
}

template <typename BIGINT>
void multiplication_bench() {
    std::srand(std::time(nullptr));
    size_t n_reps = 5;
    auto start = std::chrono::high_resolution_clock::now();
    for (int length = 1; length < 100; length += 10) {
        std::string s1;
        for (int i = 0; i < length; i++) {
            s1.push_back((std::rand() % 10) +'0');
        }
        std::string s2;
        for (int i = 0; i < length; i++) {
            s2.push_back((std::rand() % 10) +'0');
        }
        if (s1[0] == '0') {
            s1[0] = ((std::rand() % 9 + 1) +'0');
        }
        if (s2[0] == '0') {
            s2[0] = ((std::rand() % 9 + 1) +'0');
        }
        BIGINT bigint1{s1};
        BIGINT bigint2{s2};
        for (int i = 0; i < n_reps; i++) {
            bigint1 * bigint2;
        }
    }
    auto finish = std::chrono::high_resolution_clock::now();
    auto us = std::chrono::duration_cast<std::chrono::microseconds>(finish-start);
    std::cout << "Finished in " << us.count() << " us."  << std::endl;
}

template <typename BIGINT>
void division_bench() {
    std::srand(std::time(nullptr));
    size_t n_reps = 5;
    auto start = std::chrono::high_resolution_clock::now();
    for (int length = 1; length < 100; length += 10) {
        std::string s1;
        for (int i = 0; i < length; i++) {
            s1.push_back((std::rand() % 10) +'0');
        }
        std::string s2;
        for (int i = 0; i < length; i++) {
            s2.push_back((std::rand() % 10) +'0');
        }
        if (s1[0] == '0') {
            s1[0] = ((std::rand() % 9 + 1) +'0');
        }
        if (s2[0] == '0') {
            s2[0] = ((std::rand() % 9 + 1) +'0');
        }
        BIGINT bigint1{s1};
        BIGINT bigint2{s2};
        for (int i = 0; i < n_reps; i++) {
            bigint1 / bigint2;
        }
    }
    auto finish = std::chrono::high_resolution_clock::now();
    auto us = std::chrono::duration_cast<std::chrono::microseconds>(finish-start);
    std::cout << "Finished in " << us.count() << " us."  << std::endl;
}

TEST (benchmark, benchmark_addition){
    std::cout << "Boost GMP wrapper performance: " << std::endl;
    addition_bench<boost::multiprecision::mpz_int>();
    std::cout << "BigIntCpp wrapper performance: " << std::endl;
    addition_bench<BigInt>();
    EXPECT_EQ(1,1);
}

TEST (benchmark, benchmark_subtraction){
    std::cout << "Boost GMP wrapper performance: " << std::endl;
    subtraction_bench<boost::multiprecision::mpz_int>();
    std::cout << "BigIntCpp wrapper performance: " << std::endl;
    subtraction_bench<BigInt>();
    EXPECT_EQ(1,1);
}

TEST (benchmark, benchmark_multiplication){
    std::cout << "Boost GMP wrapper performance: " << std::endl;
    multiplication_bench<boost::multiprecision::mpz_int>();
    std::cout << "BigIntCpp wrapper performance: " << std::endl;
    multiplication_bench<BigInt>();
    EXPECT_EQ(1,1);
}

TEST (benchmark, benchmark_division){
    std::cout << "Boost GMP wrapper performance: " << std::endl;
    division_bench<boost::multiprecision::mpz_int>();
    std::cout << "BigIntCpp wrapper performance: " << std::endl;
    division_bench<BigInt>();
    EXPECT_EQ(1,1);
}