/*
 * AbstractEquationAdjoint.cpp
 *
 *  Created on: 16.11.2018
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/patterns.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/types.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <iostream>
#include <map>
#include <string>

#include <base/Norm.h>
#include <forward/AbstractEquationAdjoint.h>

namespace wavepi {
namespace forward {

template <int dim>
void AbstractEquationAdjoint<dim>::next_mesh(size_t source_idx, size_t target_idx) {
  if (source_idx == mesh->length()) {
    dof_handler = mesh->get_dof_handler(target_idx);

    system_rhs_u.reinit(dof_handler->n_dofs());
    system_rhs_v.reinit(dof_handler->n_dofs());

    tmp_u.reinit(dof_handler->n_dofs());
    tmp_v.reinit(dof_handler->n_dofs());
  } else
    dof_handler =
        mesh->transfer(source_idx, target_idx, {&system_rhs_u, &system_rhs_v, &tmp_u, &tmp_v, &tmp_R_adjoint});

  sparsity_pattern = mesh->get_sparsity_pattern(target_idx);
  constraints      = mesh->get_constraint_matrix(target_idx);

  matrix_A.reinit(*sparsity_pattern);
  matrix_B.reinit(*sparsity_pattern);
  matrix_C.reinit(*sparsity_pattern);

  system_matrix.reinit(*sparsity_pattern);

  rhs.reinit(dof_handler->n_dofs());

  solution_u.reinit(dof_handler->n_dofs());
  solution_v.reinit(dof_handler->n_dofs());
}

template <int dim>
void AbstractEquationAdjoint<dim>::cleanup() {
  matrix_A.clear();
  matrix_B.clear();
  matrix_C.clear();

  solution_u.reinit(0);
  solution_v.reinit(0);

  system_rhs_u.reinit(0);
  system_rhs_v.reinit(0);

  tmp_u.reinit(0);
  tmp_v.reinit(0);

  rhs.reinit(0);
}

template <int dim>
void AbstractEquationAdjoint<dim>::next_step(size_t time_idx) {
  LogStream::Prefix p("next_step");

  double time = mesh->get_time(time_idx);
  right_hand_side->set_time(time);
}

template <int dim>
void AbstractEquationAdjoint<dim>::assemble(size_t i) {
  double time = mesh->get_time(i);
  right_hand_side->set_time(time);

  // this helps only a bit because each of the operations is already parallelized
  Threads::TaskGroup<void> task_group;
  task_group += Threads::new_task(&AbstractEquationAdjoint<dim>::assemble_matrices, *this, i);
  task_group += Threads::new_task(&RightHandSide<dim>::create_right_hand_side, *right_hand_side, *dof_handler,
                                  mesh->get_quadrature(), rhs);
  task_group.join_all();
}

template <int dim>
void AbstractEquationAdjoint<dim>::assemble_u_pre(size_t i) {
  // grid has not been changed yet,
  // matrix_* contain the matrices of the *last* time step.

  if (i == mesh->length() - 1) {
    /* i == N
     *
     * (M_N^2)^t (u_N, v_N)^t = (g_N, 0)^t
     *
     * g_N = ((M_N^2)^t)_11 u_N + ((M_N^2)^t)_12 v_N
     *     = [k_N^2 C^N + θ k_N B^N + θ^2 A^N] u_N + (1-θ) A^N v_N
     *     = [k_N^2 C^N + θ k_N B^N + θ^2 A^N] u_N
     */
  } else if (i == 0) {
    /*
     * (u_0, v_0)^t = (g_0, 0)^t - (M_{i+1}^1)^t (u_1, v_1)^t
     *
     * u_0 = g_0 + [-θ(1-θ) A^i + k_{i+1}(k_{i+1} C^{i+1} + θ B^{i+1})] u_1
     *              - (1-θ) A^i v_1
     */

    double time_step_last = mesh->get_time(i + 1) - mesh->get_time(i);

    Vector<double> tmp = solution_u;
    tmp *= 1 / (time_step_last * time_step_last);
    matrix_C.vmult(system_rhs_u, tmp);

    tmp *= time_step_last * theta;
    matrix_B.vmult_add(system_rhs_u, tmp);

    tmp_u.equ(-1.0 * theta * (1 - theta), solution_u);
    tmp_u.add(-1.0 * (1 - theta), solution_v);
  } else {
    /*
     * (M_i^2)^t (u_i, v_i)^t = (g_i, 0)^t - (M_{i+1}^1)^t (u_{i+1}, v_{i+1})^t
     *
     * ((M_i^2)^t)_11 u_i = g_i - (M_{i+1}^1)^t_11 u_{i+1} - (M_{i+1}^1)^t_12 v_{i+1} - ((M_i^2)^t)_12 v_i
     * ╰──────┬─────╯     = g_i + [-θ(1-θ) A^i + k_{i+1}(k_{i+1} C^{i+1} + θ B^{i+1})] u_{i+1}
     *        │             - (1-θ) A^i v_{i+1} - θ A^i v_i
     *        │
     *        ╰‒‒‒  =  [k_i^2 C^i + θ k_i B^i + θ^2 A^i]
     */

    double time_step_last = mesh->get_time(i + 1) - mesh->get_time(i);

    Vector<double> tmp = solution_u;
    tmp *= 1 / (time_step_last * time_step_last);
    matrix_C.vmult(system_rhs_u, tmp);

    tmp *= time_step_last * theta;
    matrix_B.vmult_add(system_rhs_u, tmp);

    tmp_u.equ(-1.0 * theta * (1 - theta), solution_u);
    tmp_u.add(-1.0 * (1 - theta), solution_v);
  }
}

template <int dim>
void AbstractEquationAdjoint<dim>::assemble_u(size_t i) {
  if (i == mesh->length() - 1) {
    /* i == N
     *
     * (M_N^2)^t (u_N, v_N)^t = (g_N, 0)^t
     *
     * g_N = ((M_N^2)^t)_11 u_N + ((M_N^2)^t)_12 v_N
     *     = [k_N^2 C^N + θ k_N B^N + θ^2 A^N] u_N + (1-θ) A^N v_N
     *     = [k_N^2 C^N + θ k_N B^N + θ^2 A^N] u_N
     */

    double time_step = mesh->get_time(i) - mesh->get_time(i - 1);

    system_rhs_u = rhs;

    system_matrix = 0.0;  // important because it still holds the matrix for v !!
    system_matrix.add(1.0 / (time_step * time_step), matrix_C);
    system_matrix.add(theta / time_step, matrix_B);
    system_matrix.add(theta * theta, matrix_A);
  } else if (i == 0) {
    /*
     * (u₀, v₀)^t = (g₀, 0)^t - (M_{i+1}¹)^t (u₁, v₁)^t
     *
     * u₀ = g₀ + [-θ(1-θ) D^{0,1}M⁻¹A⁰ + k₁(k₁ C¹ + θ B¹)] u₁
     *              - (1-θ) D^{0,1}M⁻¹A⁰ v₁
     * tmp_u = -θ(1-θ) u₁ - (1-θ) v₁
     *
     *        + some D intermediate transposes, but not applied to v_i !
     */

    Vector<double> tmp1(tmp_u.size());
    Vector<double> tmp2(tmp_u.size());

    // M⁻¹D^{i,i+1} before multiplying with A^i, then add to system_rhs_u
    vmult_D_intermediate_transpose(*mesh->get_mass_matrix(i), tmp1, tmp_u, this->solver_tolerance);

    tmp1.add(-theta, solution_v);
    matrix_A.vmult(tmp2, tmp1);
    system_rhs_u.add(1.0, tmp2);

    system_rhs_u += rhs;

    system_matrix = IdentityMatrix(solution_u.size());
  } else {
    /*
     * (M_i^2)^t (u_i, v_i)^t = (g_i, 0)^t - (M_{i+1}^1)^t (u_{i+1}, v_{i+1})^t
     *
     * ((M_i^2)^t)_11 u_i = g_i - (M_{i+1}^1)^t_11 u_{i+1} - (M_{i+1}^1)^t_12 v_{i+1} - ((M_i^2)^t)_12 v_i
     * ╰──────┬─────╯     = g_i + [-θ(1-θ) A^i + k_{i+1}(k_{i+1} C^{i+1} + θ B^{i+1})] u_{i+1}
     *        │             - (1-θ) A^i v_{i+1} - θ A^i v_i
     *        │
     *        ╰‒‒‒  =  [k_i^2 C^i + θ k_i B^i + θ^2 A^i]
     *
     *        + some D intermediate transposes, but not applied to v_i !
     */

    Vector<double> tmp1(tmp_v.size());
    Vector<double> tmp2(tmp_v.size());

    // M⁻¹D^{i,i+1} before multiplying with A^i, then add to system_rhs_u
    vmult_D_intermediate_transpose(*mesh->get_mass_matrix(i), tmp1, tmp_u, this->solver_tolerance);

    tmp1.add(-theta, solution_v);
    matrix_A.vmult(tmp2, tmp1);
    system_rhs_u.add(1.0, tmp2);

    system_rhs_u += rhs;

    double time_step = mesh->get_time(i) - mesh->get_time(i - 1);
    system_matrix    = 0.0;  // important because it still holds the matrix for v !!
    system_matrix.add(1.0 / (time_step * time_step), matrix_C);
    system_matrix.add(theta / time_step, matrix_B);
    system_matrix.add(theta * theta, matrix_A);
  }

  // needed, because hanging node constraints are not already built into the sparsity pattern
  constraints->condense(system_matrix, system_rhs_u);

  apply_boundary_conditions_u(mesh->get_time(i));
}

template <int dim>
void AbstractEquationAdjoint<dim>::assemble_v_pre(size_t i) {
  // grid has not been changed yet,
  // matrix_* contain the matrices of the *last* time step.

  if (i == mesh->length() - 1) {
    /*
     * (M_N^2)^t (u_N, v_N)^t = (g_N, 0)^t
     *
     * 0 = ((M_N^2)^t)_21 u_N + ((M_N^2)^t)_22 v_N
     *     \------------/       \------------/
     *           = 0                 /= 0
     *
     *     => v_N = 0
     */
  } else if (i == 0) {
    /*
     * (u_0, v_0)^t = (g_0, 0)^t - (M_{i+1}^1)^t (u_1, v_1)^t
     *
     * v_0 = - ((M_{i+1}^1)^t)_21 u_1 - ((M_{i+1}^1)^t)_22 v_1
     *     = [θ(k_{i+1} C^i - (1-θ) B^i) + (1-θ) (k_{i+1} C^{i+1} + θ B^{i+1})] u_1
     *       + [k_{i+1} C^i - (1-θ) B^i)] v_1
     */

    double time_step_last = mesh->get_time(1) - mesh->get_time(0);

    Vector<double> tmp = solution_u;
    tmp *= (1 - theta) / time_step_last;
    matrix_C.vmult(system_rhs_v, tmp);

    tmp *= time_step_last * theta;
    matrix_B.vmult_add(system_rhs_v, tmp);

    tmp_v.equ(theta, solution_u);
    tmp_v += solution_v;
  } else {
    /*
     * (M_i^2)^t (u_i, v_i)^t = (g_i, 0)^t - (M_{i+1}^1)^t (u_{i+1}, v_{i+1})^t
     *
     * ((M_i^2)^t)_22 v_i = - (M_{i+1}^1)^t_21 u_{i+1} - (M_{i+1}^1)^t_22 v_{i+1} - ((M_i^2)^t)_21 u_i
     * ╰──────┬─────╯     = [θ(k_{i+1} C^i - (1-θ) B^i) + (1-θ) (k_{i+1} C^{i+1} + θ B^{i+1})] u_{i+1}
     *        │             + [k_{i+1} C^i - (1-θ) B^i)] v_{i+1}
     *        │
     *        ╰‒‒‒  =  [k_i C^i + θ B^i]
     */

    double time_step_last = mesh->get_time(i + 1) - mesh->get_time(i);

    Vector<double> tmp = solution_u;
    tmp *= (1 - theta) / time_step_last;
    matrix_C.vmult(system_rhs_v, tmp);

    tmp *= time_step_last * theta;
    matrix_B.vmult_add(system_rhs_v, tmp);

    tmp_v.equ(theta, solution_u);
    tmp_v += solution_v;
  }
}

template <int dim>
void AbstractEquationAdjoint<dim>::assemble_v(size_t i) {
  if (i == mesh->length() - 1) {
    /*
     * (M_N^2)^t (u_N, v_N)^t = (g_N, 0)^t
     *
     * 0 = ((M_N^2)^t)_21 u_N + ((M_N^2)^t)_22 v_N
     *     \------------/       \------------/
     *           = 0                 /= 0
     *
     *     => v_N = 0
     */

    system_rhs_v  = 0.0;
    system_matrix = IdentityMatrix(solution_v.size());
  } else if (i == 0) {
    /*
     * (u_0, v_0)^t = (g_0, 0)^t - (M_{i+1}^1)^t (u_1, v_1)^t
     *
     * v_0 = - ((M_{i+1}^1)^t)_21 u_1 - ((M_{i+1}^1)^t)_22 v_1
     *     = [θ(k_{i+1} C^i - (1-θ) B^i) + (1-θ) (k_{i+1} C^{i+1} + θ B^{i+1})] u_1
     *       + [k_{i+1} C^i - (1-θ) B^i)] v_1
     *
     *        + some D intermediate transposes
     */

    double time_step_last = mesh->get_time(1) - mesh->get_time(0);

    Vector<double> tmp1(tmp_v.size());
    Vector<double> tmp2(tmp_v.size());

    // C^{0,1} instead of C^0
    vmult_C_intermediate(tmp1, tmp_v);
    system_rhs_v.add(1.0 / time_step_last, tmp1);

    // M⁻¹D^{0,1} before multiplying with B^0, then add to system_rhs_v
    vmult_D_intermediate_transpose(*mesh->get_mass_matrix(i), tmp1, tmp_v, this->solver_tolerance);
    matrix_B.vmult(tmp2, tmp1);
    system_rhs_v.add(-1 * (1 - theta), tmp2);

    system_matrix = IdentityMatrix(solution_u.size());
  } else {
    /*
     * (M_i^2)^t (u_i, v_i)^t = (g_i, 0)^t - (M_{i+1}^1)^t (u_{i+1}, v_{i+1})^t
     *
     * ((M_i^2)^t)_22 v_i = - (M_{i+1}^1)^t_21 u_{i+1} - (M_{i+1}^1)^t_22 v_{i+1} - ((M_i^2)^t)_21 u_i
     * ╰──────┬─────╯     = [θ(k_{i+1} C^i - (1-θ) B^i) + (1-θ) (k_{i+1} C^{i+1} + θ B^{i+1})] u_{i+1}
     *        │             + [k_{i+1} C^i - (1-θ) B^i)] v_{i+1}
     *        │
     *        ╰‒‒‒  =  [k_i C^i + θ B^i]
     *
     *        + some D intermediate transposes
     */

    double time_step_last = mesh->get_time(i + 1) - mesh->get_time(i);

    Vector<double> tmp1(tmp_v.size());
    Vector<double> tmp2(tmp_v.size());

    // C^{i,i+1} instead of C^i
    vmult_C_intermediate(tmp1, tmp_v);
    system_rhs_v.add(1.0 / time_step_last, tmp1);

    // M⁻¹D^{i,i+1} before multiplying with B^i, then add to system_rhs_v
    vmult_D_intermediate_transpose(*mesh->get_mass_matrix(i), tmp1, tmp_v, this->solver_tolerance);
    matrix_B.vmult(tmp2, tmp1);
    system_rhs_v.add(-1 * (1 - theta), tmp2);

    // system_matrix <- 0 not needed because matrix was reinited due to possible mesh change
    double time_step = mesh->get_time(i) - mesh->get_time(i - 1);
    system_matrix.add(1.0 / time_step, matrix_C);
    system_matrix.add(theta, matrix_B);
  }

  // needed, because hanging node constraints are not already built into the sparsity pattern
  constraints->condense(system_matrix, system_rhs_v);

  apply_boundary_conditions_v(mesh->get_time(i));
}

template <int dim>
void AbstractEquationAdjoint<dim>::solve_u() {
  LogStream::Prefix p("solve_u");

  double norm_rhs = system_rhs_u.l2_norm();

  SolverControl solver_control(this->solver_max_iter, this->solver_tolerance * norm_rhs);
  SolverCG<> cg(solver_control);

  // Fewer (~half) iterations using preconditioner, but at least in 2D this is still not worth the effort
  // PreconditionSSOR<SparseMatrix<double> > precondition;
  // precondition.initialize (system_matrix, PreconditionSSOR<SparseMatrix<double> >::AdditionalData(.6));
  PreconditionIdentity precondition = PreconditionIdentity();

  cg.solve(system_matrix, solution_u, system_rhs_u, precondition);
  constraints->distribute(solution_u);

  std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));
  deallog << "Steps: " << solver_control.last_step();
  deallog << ", ‖res‖ = " << solver_control.last_value();
  deallog << ", ‖rhs‖ = " << norm_rhs << std::endl;

  deallog.flags(f);
}

template <int dim>
void AbstractEquationAdjoint<dim>::solve_v() {
  LogStream::Prefix p("solve_v");

  double norm_rhs = system_rhs_v.l2_norm();

  SolverControl solver_control(this->solver_max_iter, this->solver_tolerance * norm_rhs);
  SolverCG<> cg(solver_control);

  // See the comment in solve_u about preconditioning
  PreconditionIdentity precondition = PreconditionIdentity();

  cg.solve(system_matrix, solution_v, system_rhs_v, precondition);
  constraints->distribute(solution_v);

  std::ios::fmtflags f(deallog.flags(std::ios_base::scientific));

  deallog << "Steps: " << solver_control.last_step();
  deallog << ", ‖res‖ = " << solver_control.last_value();
  deallog << ", ‖rhs‖ = " << norm_rhs << std::endl;

  deallog.flags(f);
}

template <int dim>
DiscretizedFunction<dim> AbstractEquationAdjoint<dim>::run(std::shared_ptr<RightHandSide<dim>> right_hand_side) {
  LogStream::Prefix p("AbstractEqAdj");
  Assert(mesh->length() >= 2, ExcInternalError());

  Timer timer, assembly_timer;
  timer.start();

  // this is going to be the result
  DiscretizedFunction<dim> res(mesh);

  // save handle to rhs function so that `assemble` and `next_step` can use it
  this->right_hand_side = right_hand_side;

  for (size_t j = 0; j < mesh->length(); j++) {
    size_t i = mesh->length() - 1 - j;

    LogStream::Prefix pp("step-" + Utilities::int_to_string(j, 4));
    double time = mesh->get_time(i);

    // u -> u_old, same for v and matrices
    next_step(i);

    // assembling that needs to take place on the old grid
    assemble_v_pre(i);
    assemble_u_pre(i);

    // set dof_handler to mesh for this time step,
    // interpolate to new mesh (if j != 0)
    next_mesh(i + 1, i);

    // assemble new matrices
    assembly_timer.start();
    assemble(i);
    assembly_timer.stop();

    // finish assembling of rhs_v
    // and solve for $v^i$
    assemble_v(i);
    solve_v();

    // finish assembling of rhs_u
    // and solve for $u^i$
    assemble_u(i);
    solve_u();

    /* apply R^t */

    if (i < mesh->length() - 1) {
      Vector<double> tmp(solution_u.size());
      vmult_D_intermediate_transpose(*mesh->get_mass_matrix(i), tmp, tmp_R_adjoint, this->solver_tolerance);

      res[i].add(1.0, tmp);
    }

    if (i > 0) {
      tmp_R_adjoint.reinit(solution_u.size());
      tmp_R_adjoint.equ(theta * (1 - theta), solution_u);
      tmp_R_adjoint.add(1 - theta, solution_v);

      // tmp_R_adjoint has to be transferred to grid i-1 first!
      // res[i - 1] += " vmult_D_intermediate_transpose(tmp_R_adjoint) ";

      res[i].add(theta * theta, solution_u);
      res[i].add(theta, solution_v);
    }

    std::ios::fmtflags f(deallog.flags(std::ios_base::fixed));
    deallog << "t=" << time << std::scientific << ", ";
    deallog << "‖u‖=" << solution_u.l2_norm() << ", ‖v‖=" << solution_v.l2_norm() << std::endl;
    deallog.flags(f);
  }

  timer.stop();
  std::ios::fmtflags f(deallog.flags(std::ios_base::fixed));
  deallog << "solved adjoint PDE in " << timer.wall_time() << "s (setup " << assembly_timer.wall_time() << "s)"
          << std::endl;
  deallog.flags(f);

  cleanup();

  return res;
}

template class AbstractEquationAdjoint<1>;
template class AbstractEquationAdjoint<2>;
template class AbstractEquationAdjoint<3>;

} /* namespace forward */
} /* namespace wavepi */


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

#include <boost/simd/toolbox/arithmetic/functions/idivceil.hpp>
#include <boost/simd/include/functions/simd/iceil.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/dispatch/meta/strip.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivceil_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return iceil(tofloat(a0)/tofloat(a1)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivceil_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<unsigned_<A0>,X>))((simd_<unsigned_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return rdivide(a0+a1-One<A0>(), a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivceil_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<int16_<A0>,X>))((simd_<int16_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type              stype;
      typedef typename dispatch::meta::upgrade<stype>::type   itype;
      typedef simd::native<itype,X>                          ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::fusion::tie(a0l, a0h) = split(a0);
      boost::fusion::tie(a1l, a1h) = split(a1);
      return bitwise_cast<A0>(group(idivceil(a0l, a1l),
                                    idivceil(a0h, a1h)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivceil_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<int8_<A0>,X>))((simd_<int8_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type          itype;
      typedef simd::native<itype, X>                      ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::fusion::tie(a0l, a0h) = split(a0);
      boost::fusion::tie(a1l, a1h) = split(a1);
      return bitwise_cast<A0>(group(idivceil(a0l, a1l),
                               idivceil(a0h, a1h) ));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivceil_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return iceil(a0/a1); }
  };
} } }

#endif


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2013 - 2014 MetaScale SAS
//
//          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_SIMD_VMX_ALTIVEC_REC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_VMX_ALTIVEC_REC_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/arithmetic/functions/rec.hpp>
#include <boost/simd/include/functions/simd/fast_rec.hpp>
#include <boost/simd/include/functions/simd/copysign.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/dispatch/attributes.hpp>

#if !defined( BOOST_SIMD_NO_INFINITIES )
#include <boost/simd/include/functions/simd/is_inf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( rec_
                                   , boost::simd::tag::vmx_
                                   , (A0)
                                   , (( simd_ < floating_<A0>
                                              , boost::simd::tag::vmx_
                                              >
                                     ))
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type estimate = fast_rec(a0);

      // fix rec(+/-0)
      estimate = if_else( is_eqz(a0)
                        , copysign(Inf<result_type>(),a0)
                        , estimate
                        );

      // fix rec(+/-inf)
      #if !defined( BOOST_SIMD_NO_INFINITIES )
      estimate = if_else( is_inf(a0)
                        , copysign(Zero<result_type>(),a0)
                        , estimate
                        );
      #endif

      return estimate;
    }
  };
} } }
#endif
#endif


/**
 * \ file TS9ToneFilter.cpp
 */

#include <ATK/EQ/PedalToneStackFilter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

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

constexpr gsl::index PROCESSSIZE = 1024*1026;
constexpr gsl::index SAMPLINGRATE = 1024*64;

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_tone_test )
{
  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_tone(.5);
  BOOST_CHECK_EQUAL(filter.get_tone(), .5);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_throw_1_test )
{
  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_tone(1.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_throw_0_test )
{
  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_tone(-0.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha0_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.95511927));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha0_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 1.16067427));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha0_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.14668446));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha1_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.95349053));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha1_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.95619999));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha1_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 1.03180903));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha05_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(0.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.95386498));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha05_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(0.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.97251369));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_TS9ToneCoefficients_alpha05_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::TS9ToneCoefficients<double> > filter;
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_tone(0.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.59163051));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/monad/bind_mcd.hpp>
#include <boost/hana/monad/flatten_mcd.hpp>
#include <boost/hana/monad/laws.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/monad.hpp>
using namespace boost::hana;

template <template <typename ...> class mcd>
void test() {
    constexpr auto monad = detail::minimal::monad<mcd>;
    BOOST_HANA_CONSTEXPR_ASSERT(Monad::laws::check(
        monad(1),
        2,
        [=](auto x) { return monad(x + 1); },
        [=](auto x) { return monad(x * 2); }
    ));
}

int main() {
    test<Monad::flatten_mcd>();
    test<Monad::bind_mcd>();
}


#ifndef __BUILDING_FOOTPRINT_EXTRACTION_HPP__
#define __BUILDING_FOOTPRINT_EXTRACTION_HPP__

/************** application-specific types ****************/

#include "core/geometry.hpp"
#include <boost/variant.hpp>
typedef boost::variant<Rectangle_2,Circle_2> object;
//typedef Rectangle_2 object;

#include "core/global_reconstruction_unary_energy.hpp"
#include "rjmcmc/image/gradient_functor.hpp"
#include "rjmcmc/image/oriented.hpp"
typedef oriented<gradient_image_t> oriented_gradient_view;

//typedef boost::gil::gray16s_image_t ndvi_image_t;
typedef rjmcmc::any_image_t ndvi_image_t;
typedef oriented<ndvi_image_t>     oriented_ndvi_view;
typedef global_reconstruction_unary_energy<oriented_gradient_view,oriented_ndvi_view> unary_energy;

#include "rjmcmc/mpp/energy/intersection_area_binary_energy.hpp"
typedef intersection_area_binary_energy<>      binary_energy;

#include "rjmcmc/mpp/rectilinear_searchspace.hpp"
typedef rectilinear_searchspace<object> searchspace;

#include "rjmcmc/rjmcmc/kernel/transform_kernels.hpp"
typedef rjmcmc::generator<searchspace>          generator_kernel;

#include "rjmcmc/geometry/kernels/rectangle_scaled_edge_kernel.hpp"
#include "rjmcmc/geometry/kernels/rectangle_rotation_scaled_corner_kernel.hpp"
#include "rjmcmc/geometry/kernels/circle_translation_kernel.hpp"
typedef geometry::rectangle_scaled_edge_kernel<K> modifier_kernel1;
typedef geometry::rectangle_rotation_scaled_corner_kernel<K> modifier_kernel2;
typedef geometry::circle_translation_kernel<K> modifier_kernel3;

/************** rjmcmc library types ****************/

//#include "simulated_annealing/schedule/logarithmic_schedule.hpp"
//#include "simulated_annealing/schedule/step_schedule.hpp"
//typedef simulated_annealing::step_schedule<rjmcmc::logarithmic_schedule<double> > schedule;
#include "rjmcmc/simulated_annealing/schedule/geometric_schedule.hpp"
typedef simulated_annealing::geometric_schedule<double> schedule;

#include "rjmcmc/simulated_annealing/end_test/max_iteration_end_test.hpp"
typedef simulated_annealing::max_iteration_end_test                           end_test;

//#include "mpp/configuration/vector_configuration.hpp"
//typedef marked_point_process::vector_configuration<object, unary_energy, binary_energy> configuration;
#include "rjmcmc/mpp/configuration/graph_configuration.hpp"
typedef marked_point_process::graph_configuration<object, unary_energy, binary_energy> configuration;

#include "rjmcmc/rjmcmc/distribution/poisson_distribution.hpp"
typedef rjmcmc::poisson_distribution                           distribution;
//#include "mpp/distribution/uniform_distribution.hpp"
//typedef marked_point_process::uniform_distribution                           distribution;

#include "rjmcmc/rjmcmc/sampler/sampler.hpp"
#include "rjmcmc/mpp/kernel/kernel.hpp"
typedef mpp::uniform_birth_kernel<generator_kernel>           birth_kernel;
typedef mpp::uniform_death_kernel                             death_kernel;
typedef rjmcmc::binary_kernel<birth_kernel,death_kernel>      birth_death_kernel;

typedef mpp::modification_kernel<modifier_kernel1>            modification_kernel1;
typedef mpp::modification_kernel<modifier_kernel2>            modification_kernel2;
typedef mpp::modification_kernel<modifier_kernel3>            modification_kernel3;

//#include "rjmcmc/sampler/dueck_scheuer_sampler.hpp"
//typedef rjmcmc::dueck_scheuer_acceptance acceptance;
//#include "rjmcmc/sampler/franz_hoffmann_sampler.hpp"
//typedef rjmcmc::franz_hoffmann_acceptance acceptance;
#include "rjmcmc/rjmcmc/acceptance/metropolis_acceptance.hpp"
typedef rjmcmc::metropolis_acceptance acceptance;

#include "rjmcmc/mpp/direct_sampler.hpp"
typedef marked_point_process::direct_sampler<distribution,generator_kernel> d_sampler;

typedef rjmcmc::sampler<d_sampler,acceptance,birth_death_kernel
        ,modification_kernel1
        ,modification_kernel2
        ,modification_kernel3
        > sampler;

/************** main ****************/

Iso_rectangle_2 get_bbox(const param *p) {
    int x0 = p->get<int>("xmin");
    int x1 = p->get<int>("xmax");
    int y0 = p->get<int>("ymin");
    int y1 = p->get<int>("ymax");
    if(x0>x1) std::swap(x0,x1);
    if(y0>y1) std::swap(y0,y1);
    return Iso_rectangle_2(x0,y0,x1,y1);
}

void set_bbox(param *p, const Iso_rectangle_2& r) {
    p->set("xmin",(int) r.min().x());
    p->set("ymin",(int) r.min().y());
    p->set("xmax",(int) r.max().x());
    p->set("ymax",(int) r.max().y());
}

void create_configuration(param *p, const oriented_gradient_view& grad, const oriented_ndvi_view& ndvi, configuration *&c) {
    // energies
    unary_energy e1( grad, ndvi,
                     p->get<double>("energy"),
                     p->get<double>("ponderation_grad"),
                     p->get<double>("ponderation_ndvi")
                     );

    binary_energy e2(
            p->get<double>("surface")
            );

    // empty initial configuration
    c = new configuration(e1,e2);
}

void create_sampler(param *p, sampler *&s) {
    Iso_rectangle_2 r = get_bbox(p);

    searchspace valid;
    valid.min(Rectangle_2(r.min(),K::Vector_2(0,0) ,1/p->get<double>("maxrectangleratio")));
    valid.max(Rectangle_2(r.max(),(r.max()-r.min()),  p->get<double>("maxrectangleratio")));
    valid.min(Circle_2(r.min(),0));
    valid.max(Circle_2(r.max(),p->get<double>("maxcircleradius")));

    generator_kernel birth(valid);

    birth_kernel        kbirth(birth);
    death_kernel        kdeath;

    birth_death_kernel kbirthdeath(
            kbirth, kdeath,
            p->get<double>("pbirth"),
            p->get<double>("pdeath")
            );

    modifier_kernel1     modif1;
    modifier_kernel2     modif2;
    modifier_kernel3     modif3;

    distribution cs(p->get<double>("poisson"));

    acceptance a;
    d_sampler ds( cs, birth );
    s = new sampler( ds, a, kbirthdeath
                     , mpp::make_modification_kernel(modif1,0.2)
                     , mpp::make_modification_kernel(modif2,0.2)
                     , mpp::make_modification_kernel(modif3,0.2)
                     );
    //	s = new sampler( cs, birth );
}

void create_schedule(param *p, schedule *&t)
{
    t = new schedule(
            p->get<double>("temp"),
            p->get<double>("deccoef")
            );
    //  t = new schedule( 1000, rjmcmc::logarithmic_schedule(p->get<double>("temp")) );
}

void create_end_test(param *p, end_test *&e)
{
    e = new end_test(
            p->get<int>("nbiter")
            );
}

template<typename Visitor>
void init_visitor(param *p, Visitor& v)
{
    v.init(
            p->get<int>("nbdump"),
            p->get<int>("nbsave")
            );
}

#endif // __BUILDING_FOOTPRINT_EXTRACTION_HPP__


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_TEST_TEST_NUMERIC_HPP
#define BOOST_HANA_TEST_TEST_NUMERIC_HPP

#include <boost/hana/core/datatype.hpp>
#include <boost/hana/core/operators.hpp>

// instances
#include <boost/hana/comparable.hpp>
#include <boost/hana/enumerable.hpp>
#include <boost/hana/group.hpp>
#include <boost/hana/integral_domain.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/monoid.hpp>
#include <boost/hana/orderable.hpp>
#include <boost/hana/ring.hpp>

namespace boost { namespace hana {
    namespace test {
        struct numeric_type : operators::adl {
            constexpr explicit numeric_type(int v) : value(v) { }
            int value;
            //! @todo Find a way to make this explicit.
            constexpr operator bool() const { return value; }
        };

        using Numeric = datatype_t<numeric_type>;

        struct _numeric {
            constexpr numeric_type operator()(int x) const {
                return numeric_type{x};
            }
        };
        constexpr _numeric numeric{};
    }

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //
    // Define either one to select which MCD is used:
    //  BOOST_HANA_TEST_COMPARABLE_EQUAL_MCD
    //  BOOST_HANA_TEST_COMPARABLE_ORDERABLE_MCD
    //
    // If neither is defined, the MCD used is unspecified.
    //////////////////////////////////////////////////////////////////////////
#if defined(BOOST_HANA_TEST_COMPARABLE_EQUAL_MCD)
    template <>
    struct equal_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value == y.value); }
    };
#else
    template <>
    struct equal_impl<test::Numeric, test::Numeric>
        : Orderable::equal_impl<test::Numeric, test::Numeric>
    { };
#endif

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct less_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value < y.value); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Enumerable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct succ_impl<test::Numeric> {
        template <typename N>
        static constexpr auto apply(N n)
        { return test::numeric(n.value + 1); }
    };

    template <>
    struct pred_impl<test::Numeric> {
        template <typename N>
        static constexpr auto apply(N n)
        { return test::numeric(n.value - 1); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct eval_if_impl<test::Numeric> {
        template <typename C, typename T, typename E>
        static constexpr auto apply(C c, T t, E e) {
            auto id = [](auto x) { return x; };
            return c.value ? t(id) : e(id);
        }
    };

    template <>
    struct not_impl<test::Numeric> {
        template <typename X>
        static constexpr auto apply(X x)
        { return test::numeric(!x.value); }
    };

    template <>
    struct while_impl<test::Numeric> {
        template <typename Pred, typename State, typename F>
        static constexpr auto apply(Pred pred, State state, F f)
            -> decltype(true ? f(state) : state)
        {
            if (pred(state))
                return while_(pred, f(state), f);
            else
                return state;
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct plus_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value + y.value); }
    };

    template <>
    struct zero_impl<test::Numeric> {
        static constexpr auto apply()
        { return test::numeric(0); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Group
    //
    // Define either one to select which MCD is used:
    //  BOOST_HANA_TEST_GROUP_NEGATE_MCD
    //  BOOST_HANA_TEST_GROUP_MINUS_MCD
    //
    // If neither is defined, the MCD used is unspecified.
    //////////////////////////////////////////////////////////////////////////
#if defined(BOOST_HANA_TEST_GROUP_NEGATE_MCD)
    template <>
    struct negate_impl<test::Numeric> {
        template <typename X>
        static constexpr auto apply(X x)
        { return test::numeric(-x.value); }
    };
#else
    template <>
    struct minus_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value - y.value); }
    };
#endif

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct mult_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value * y.value); }
    };

    template <>
    struct one_impl<test::Numeric> {
        static constexpr auto apply()
        { return test::numeric(1); }
    };

    //////////////////////////////////////////////////////////////////////////
    // IntegralDomain
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct quot_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value / y.value); }
    };

    template <>
    struct rem_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value % y.value); }
    };
}} // end namespace boost::hana

#endif //! BOOST_HANA_TEST_TEST_NUMERIC_HPP


// Copyright 2018 Martin Krasser. All Rights Reserved.
// Modifications copyright (C) 2018 Uber Technologies, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// =============================================================================

#include "bayesian_optimization.h"

#include <cmath>
#include <iostream>
#include <numeric>

#include <Eigen/LU>
#include "LBFGS.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;

namespace horovod {
namespace common {

const double NORM_PDF_C = std::sqrt(2 * M_PI);

void GetSufficientStats(std::vector<double>& v, double* mu, double* sigma) {
  double sum = std::accumulate(v.begin(), v.end(), 0.0);
  *mu = sum / v.size();

  std::vector<double> diff(v.size());
  std::transform(v.begin(), v.end(), diff.begin(), [mu](double& x) { return x - *mu; });
  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
  *sigma = std::sqrt(sq_sum / v.size());
}

// Returns a list of distributions that generate real values uniformly and random between the bounds.
std::vector<std::uniform_real_distribution<>> GetDistributions(std::vector<std::pair<double, double>> bounds) {
  std::vector<std::uniform_real_distribution<>> dists;
  for (const std::pair<double, double>& bound : bounds) {
    dists.push_back(std::uniform_real_distribution<>(bound.first, bound.second));
  }
  return dists;
}

BayesianOptimization::BayesianOptimization(std::vector<std::pair<double, double>> bounds, double alpha, double xi)
    : d_(bounds.size()),
      bounds_(bounds),
      xi_(xi),
      dists_(GetDistributions(bounds)),
      gpr_(GaussianProcessRegressor(alpha)) {}

void BayesianOptimization::AddSample(const Eigen::VectorXd& x, double y) {
  x_samples_.push_back(x);
  y_samples_.push_back(y);
}

VectorXd BayesianOptimization::NextSample(bool normalize) {
  double mu = 0.0;
  double sigma = 1.0;
  if (normalize && y_samples_.size() >= 3) {
    GetSufficientStats(y_samples_, &mu, &sigma);
  }

  // Matrices are immutable and must be regenerated each time a new sample is added.
  MatrixXd x_sample(x_samples_.size(), d_);
  for (unsigned int i = 0; i < x_samples_.size(); ++i) {
    x_sample.row(i) = x_samples_[i];
  }

  MatrixXd y_sample(y_samples_.size(), 1);
  for (unsigned int i = 0; i < y_samples_.size(); ++i) {
    double norm_score = (y_samples_[i] - mu) / sigma;

    VectorXd y_i(1);
    y_i(0) = norm_score;
    y_sample.row(i) = y_i;
  }

  // Generate the posterior distribution for the GP given the observed data.
  gpr_.Fit(&x_sample, &y_sample);

  // Return the next proposed location that maximizes the expected improvement.
  return ProposeLocation(x_sample, y_sample);
}

void BayesianOptimization::Clear() {
  x_samples_.clear();
  y_samples_.clear();
}

VectorXd BayesianOptimization::ProposeLocation(const MatrixXd& x_sample, const MatrixXd& y_sample, int n_restarts) {
  // Objective function we wish to minimize, the negative acquisition function.
  auto f = [&](const VectorXd& x) {
    return -ExpectedImprovement(x.transpose(), x_sample)[0];
  };

  // Minimization routine. To approximate bounded LBFGS, we set to infinity the value of any input outside of bound.
  auto min_obj = [&](const VectorXd& x, VectorXd& grad) {
    double fx = CheckBounds(x) ? f(x) : std::numeric_limits<double>::max();
    GaussianProcessRegressor::ApproxFPrime(x, f, fx, grad);
    return fx;
  };

  // Use the L-BFGS method for minimizing the objective, limit our search to a set number of iterations
  // if convergence has not be reached within the threshold (epsilon).
  LBFGSpp::LBFGSParam<double> param;
  param.epsilon = 1e-5;
  param.max_iterations = 100;
  LBFGSpp::LBFGSSolver<double> solver(param);

  // Optimize with random restarts to avoid getting stuck in local minimum.
  VectorXd x_next = VectorXd::Zero(d_);
  double fx_min = std::numeric_limits<double>::max();
  for (int i = 0; i < n_restarts; ++i) {
    // Generate a random starting point by drawing from our bounded distributions.
    VectorXd x = VectorXd::Zero(d_);
    for (unsigned int j = 0; j < d_; ++j) {
      x[j] = dists_[j](gen_);
    }

    // Minimize the objective function.
    double fx;
    solver.minimize(min_obj, x, fx);

    // Update the new minimum among all attempts.
    if (fx < fx_min) {
      fx_min = fx;
      x_next = x;
    }
  }

  // Return the input point that minimized the negative expected improvement.
  return x_next;
}

VectorXd BayesianOptimization::ExpectedImprovement(const MatrixXd& x, const MatrixXd& x_sample) {
  // Compute sufficient statistics for the proposed locations.
  Eigen::VectorXd mu;
  Eigen::VectorXd sigma;
  gpr_.Predict(x, mu, &sigma);

  // Compute sufficient statistics for the observed locations.
  Eigen::VectorXd mu_sample;
  gpr_.Predict(x_sample, mu_sample);

  // Needed for noise-based model, otherwise use y_sample.maxCoeff().
  // See also section 2.4 in https://arxiv.org/pdf/1012.2599.pdf:
  // Eric Brochu, Vlad M. Cora, Nando de Freitas,
  // A Tutorial on Bayesian Optimization of Expensive Cost Functions
  double mu_sample_opt = mu_sample.maxCoeff();

  // Probability density function of the standard normal distribution.
  auto pdf = [](double x) {
    return std::exp(-(x * x) / 2.0) / NORM_PDF_C;
  };

  // Cumulative distribution function of the standard normal distribution.
  auto cdf = [](double x) {
    return 0.5 * std::erfc(-x * M_SQRT1_2);
  };

  // Parameter xi_ determines the amount of exploration during optimization. Higher values of xi_ results
  // in more exploration. With higher values of xi_, the importance of improvements predicted by the
  // underlying GP posterior mean mu_sample_opt decreases relative to the importance of improvements
  // in regions of high prediction uncertainty, as indicated by large values of variable sigma.
  Eigen::VectorXd imp = mu.array() - mu_sample_opt - xi_;
  VectorXd z = imp.array() / sigma.array();

  // The first term of the summation is the exploitation term, the second the exploration term.
  VectorXd ei = imp.cwiseProduct(z.unaryExpr(cdf)) + sigma.cwiseProduct(z.unaryExpr(pdf));
  ei = (sigma.array() != 0).select(ei, 0.0);
  return ei;
}

bool BayesianOptimization::CheckBounds(const Eigen::VectorXd& x) {
  for (int i = 0; i < x.size(); ++i) {
    if (x[i] < bounds_[i].first || x[i] > bounds_[i].second) {
      return false;
    }
  }
  return true;
}

} // namespace common
} // namespace horovod


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  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 Willow Garage, Inc. 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 OWNER 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.
 *
 * @author: Cedric Cagniart, Koen Buys
 */

#include <libBVH/BVHMesh.h>
#include <libBVH/BVHMesh_smooth.h>
#include <libBVH/BVHMotionFile.h>
#include <libBVH/CMUTransforms.h>

#include <RGBDRenderer.h>

#include <OptionParser.h>

#include <cmath>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <stdexcept>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

// boost random
#include <boost/random/mersenne_twister.hpp> // the randomness source
#include <boost/random/uniform_real.hpp> // the distribution
#include <boost/random/variate_generator.hpp>
#include <ctime> // for randomness seeding

using namespace pcl::people::BVH;
using namespace pcl::people::datasynth;

// #############################
// Matrix Building and Loading
// #############################
void buildPMat(float f, int W, int H, float zNear, float zFar, float *PMat)
{
  PMat[0 * 4 + 0] = 2.*f / W;
  PMat[0 * 4 + 1] =  0;
  PMat[0 * 4 + 2] = 0;
  PMat[0 * 4 + 3] = 0;
  PMat[1 * 4 + 0] = 0;
  PMat[1 * 4 + 1] =  2.*f / H;
  PMat[1 * 4 + 2] = 0;
  PMat[1 * 4 + 3] = 0;
  PMat[2 * 4 + 0] = 0.;
  PMat[2 * 4 + 1] =  0;
  PMat[2 * 4 + 2] = (zFar + zNear) / (zFar - zNear);
  PMat[2 * 4 + 3] = 1; //we ll have to add 1/H and 1/W to get the 1/2 pixel offet later
  PMat[3 * 4 + 0] = 0;
  PMat[3 * 4 + 1] =  0;
  PMat[3 * 4 + 2] = -(2 * zFar * zNear) / (zFar - zNear);
  PMat[3 * 4 + 3] = 0;
}

void loadMMat(const std::string &filename, float *MMat)
{
  std::ifstream fin(filename.c_str());
  if (!fin.is_open())
  {
    throw (std::runtime_error(std::string("(E)could not open: ") + filename));
  }
  for (int i = 0; i < 4; ++i)
  {
    for (int j = 0; j < 4; ++j)
    {
      fin >> MMat[4 * i + j];
    }
  }
  if (fin.fail())
  {
    throw (std::runtime_error(std::string("(E) malformed matrix file: ") + filename));
  }
}

void export_PPM(const char *filename,
                int w, int h,
                const uint8_t *l)
{
  std::ofstream fout(filename);
  fout << "P2\n" << w << " " << h << "\n" << int(*std::max_element(l, l + w * h)) << "\n";
  for (int y = 0; y < h; ++y)
  {
    for (int x = 0; x < w; ++x)
    {
      fout << int(l[w * y + x]) << " ";
    }
    fout << "\n";
  }
}

void exportPPM_depth(const char *filename,
                     int w, int h,
                     const uint16_t *d)
{
  std::ofstream fout(filename);
  fout << "P2\n" << w << " " << h << "\n" << int(*std::max_element(d, d + w * h)) << "\n";
  for (int y = 0; y < h; ++y)
  {
    for (int x = 0; x < w; ++x)
    {
      fout << d[w * y + x] << " ";
    }
    fout << "\n";
  }
}

void printGLMat(const float *M, std::ostream &os)
{
  os << std::fixed;
  os << "[";
  for (int i = 0; i < 3; ++i)
  {
    os << "[";
    for (int j = 0; j < 3; ++j)
    {
      os << M[4 * j + i] << ",";
    }
    os << M[4 * 3 + i] << "],\n";
  }
  os << "[";
  for (int j = 0; j < 3; ++j)
  {
    os << M[4 * j + 3] << ",";
  }
  os << M[4 * 3 + 3] << "]]\n";
}

// #############################
// Main prog
// #############################
// The simulated camera will look down its Z positive
int main(int argc, char **argv)
{
  OptionParser opt(argc, argv);
  std::string MMatFile  = opt.getOption<std::string>("-MMat");
  std::string BVH_ref   = opt.getOption<std::string>("-BVH_ref");
  std::string fmotion   = opt.getOption<std::string>("-MotionFile");
  std::string fverts    = opt.getOption<std::string>("-verts");
  std::string ftris     = opt.getOption<std::string>("-tris");
  std::string fbones    = opt.getOption<std::string>("-bones");
  std::string flabels   = opt.getOption<std::string>("-labels");
  std::string doutbasename = opt.getOption<std::string>("-dout");
  std::string loutbasename = opt.getOption<std::string>("-lout");
  uint16_t    backgroundDepth_mm = opt.getOption<uint16_t>("-bgdepth_mm");
  int         firstFrame         = opt.getOption<int>("-F");
  int         lastFrame          = opt.getOption<int>("-L");
  double      angleRange         = opt.getOption<double>("-angleRange");

  // ---------------------------
  // load BVH reference
  std::vector<bvhJoint> joints_ref;
  std::vector<Vec3>     rest_pose;
  try
  {
    float                             period_ref;
    std::vector<std::vector<float> >  values_ref;
    bvhparseFile(BVH_ref, joints_ref, period_ref, values_ref);
    computeRestStateJointPos(joints_ref, rest_pose);
  }
  catch (std::exception &err)
  {
    std::cout << err.what() << std::endl;
    exit(0);
  }

  // --------------------------
  // load Mesh vectors
  std::vector<Triangle>     tris;
  std::vector<vertex_joint> vjs;
  std::vector<int>          vjs_bounds;
  std::vector<Vec3>         X0;
  std::vector<int>          labels;
  loadMesh_Tri(ftris.c_str(), tris);
  loadMesh_VJ_smooth(fbones.c_str(), joints_ref, vjs, vjs_bounds);
  loadMesh_Vertices(fverts.c_str(),  X0);
  loadMesh_labels(flabels.c_str(), labels);

  // -------------------------
  // create the renderer
  RGBDRenderer renderer;
  RGBDRenderer::LabeledVertex *vBuffer = new RGBDRenderer::LabeledVertex[X0.size()];
  GLuint                      *iBuffer = new GLuint[3 * tris.size()];
  // copy triangle data
  {
    GLuint *t_itr = iBuffer;
    for (size_t ti = 0; ti < tris.size(); ++ti)
    {
      *t_itr++ = tris[ti].v0;
      *t_itr++ = tris[ti].v1;
      *t_itr++ = tris[ti].v2;
    }
  }

  uint16_t *dmap = new uint16_t[640 * 480];
  uint8_t  *lmap = new uint8_t[640 * 480];
  float PMat[16];
  float MMat[16];
  buildPMat(640, 640, 480, 0.1, 100., PMat);
  loadMMat(MMatFile, MMat);

  printGLMat(PMat, std::cout);
  printGLMat(MMat, std::cout);

  // -------------------------
  // create the randomness source
  boost::mt19937 rng; // the random number generator
  rng.seed(static_cast<unsigned int>(std::time(0)));
  boost::uniform_real<double> distrib_a(-angleRange, angleRange);
  boost::variate_generator < boost::mt19937 &,
        boost::uniform_real<double> > angle(rng, distrib_a);

  // -------------------------
  // load the motion file
  BVHMotionFile MFile(fmotion);

  std::vector<float> values;
  for (int frameId = 0; frameId < firstFrame; ++frameId)
  {
    MFile.readNextFrame(joints_ref, values);
  }
  // -------------------------
  // load the new pose
  for (int frameid = firstFrame; frameid <= lastFrame; ++frameid)
  {
    // skip one half of the files horribly
    //MFile.readNextFrame(joints_ref, values);
    // actually get the file
    if (!MFile.readNextFrame(joints_ref, values))
    {
      std::cout << "(E) " << fmotion
                << " ended up after " << frameid << " samples" << std::endl;
      break;
    }
    // compute transforms
    std::vector<Transform3> Tis;
    computeTis(joints_ref, rest_pose, values, Tis);
    // rebase the mesh at the origin and scale it to get everything in meters
    double rangle = angle(); //generate a random angle
    CMU_scaleRotateHips(joints_ref, Tis, rangle);
    // accum
    std::vector<Transform3> TTis;
    computeTTis(joints_ref, Tis, TTis);

    // transform mesh
    std::vector<Vec3> X;
    transformMesh_smooth(TTis, vjs, vjs_bounds, X0, X);
    // copy the mesh to gl
    RGBDRenderer::LabeledVertex *v_itr = vBuffer;
    for (size_t vi = 0; vi < X.size(); ++vi)
    {
      v_itr->x = X[vi](0);
      v_itr->y = X[vi](1);
      v_itr->z = X[vi](2);
      v_itr->label = labels[vi];
      v_itr++;
    }

    // do the actual rendering
    renderer.draw(PMat, MMat, backgroundDepth_mm, X.size(), tris.size(), vBuffer, iBuffer, dmap, lmap) ;

    // save to the file
    //export_PPM("lmap.ppm", 640,480,lmap);
    //exportPPM_depth("dmap.ppm", 640,480,dmap);
    std::string Lout = buildFilename(loutbasename, frameid);
    std::string Dout = buildFilename(doutbasename, frameid);

    cv::Mat LImage(480, 640, CV_8U,  lmap);
    cv::Mat DImage(480, 640, CV_16U, dmap);
    if (!cv::imwrite(Lout.c_str(), LImage))
    {
      std::cout << "(E) BVHtoRGBD : unable to write : Lout filename " << Lout << std::endl;
    }
    if (!cv::imwrite(Dout.c_str(), DImage))
    {
      std::cout << "(E) BVHtoRGBD : unable to write : Dout filename " << Dout << std::endl;
    }

    // tell the world where we stand
    std::cout << "wrote " << frameid << " frame to disk with angle " << rangle << "\r";
    std::cout.flush();
  }

  // -----------------------
  // clean up
  delete[] dmap;
  delete[] lmap;
  delete[] vBuffer;
  delete[] iBuffer;
  return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file LinearizedFactor.cpp
 * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
 * @author Alex Cunningham
 */

#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
#include <boost/foreach.hpp>
#include <iostream>

namespace gtsam {

/* ************************************************************************* */
LinearizedGaussianFactor::LinearizedGaussianFactor(
    const GaussianFactor::shared_ptr& gaussian, const Values& lin_points)
: NonlinearFactor(gaussian->keys())
{
  // Extract the keys and linearization points
  BOOST_FOREACH(const Key& key, gaussian->keys()) {
    // extract linearization point
    assert(lin_points.exists(key));
    this->lin_points_.insert(key, lin_points.at(key));
  }
}

/* ************************************************************************* */
// LinearizedJacobianFactor
/* ************************************************************************* */
LinearizedJacobianFactor::LinearizedJacobianFactor() {
}

/* ************************************************************************* */
LinearizedJacobianFactor::LinearizedJacobianFactor(
    const JacobianFactor::shared_ptr& jacobian, const Values& lin_points)
: Base(jacobian, lin_points) {

  // Create the dims array
  size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1));
  size_t index = 0;
  for(JacobianFactor::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) {
    dims[index++] = jacobian->getDim(iter);
  }
  dims[index] = 1;

  // Update the BlockInfo accessor
  Ab_ = VerticalBlockMatrix(dims, dims+jacobian->size()+1, jacobian->rows());
  // Get the Ab matrix from the Jacobian factor, with any covariance baked in
  Ab_.matrix() = jacobian->augmentedJacobian();
}

/* ************************************************************************* */
void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {

  std::cout << s << std::endl;

  std::cout << "Nonlinear Keys: ";
  BOOST_FOREACH(const Key& key, this->keys())
    std::cout << keyFormatter(key) << " ";
  std::cout << std::endl;

  for(const_iterator key=begin(); key!=end(); ++key)
    std::cout << boost::format("A[%1%]=\n")%keyFormatter(*key) << A(*key) << std::endl;
  std::cout << "b=\n" << b() << std::endl;

  lin_points_.print("Linearization Point: ");
}

/* ************************************************************************* */
bool LinearizedJacobianFactor::equals(const NonlinearFactor& expected, double tol) const {

  const This *e = dynamic_cast<const This*> (&expected);
  if (e) {

    Matrix thisMatrix = this->Ab_.range(0, Ab_.nBlocks());
    Matrix rhsMatrix = e->Ab_.range(0, Ab_.nBlocks());

    return Base::equals(expected, tol)
        && lin_points_.equals(e->lin_points_, tol)
        && equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
  } else {
    return false;
  }
}

/* ************************************************************************* */
double LinearizedJacobianFactor::error(const Values& c) const {
  Vector errorVector = error_vector(c);
  return 0.5 * errorVector.dot(errorVector);
}

/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedJacobianFactor::linearize(const Values& c) const {

  // Create the 'terms' data structure for the Jacobian constructor
  std::vector<std::pair<Key, Matrix> > terms;
  BOOST_FOREACH(Key key, keys()) {
    terms.push_back(std::make_pair(key, this->A(key)));
  }

  // compute rhs
  Vector b = -error_vector(c);

  return boost::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim())));
}

/* ************************************************************************* */
Vector LinearizedJacobianFactor::error_vector(const Values& c) const {

  Vector errorVector = -b();

  BOOST_FOREACH(Key key, this->keys()) {
    const Value& newPt = c.at(key);
    const Value& linPt = lin_points_.at(key);
    Vector d = linPt.localCoordinates_(newPt);
    const constABlock A = this->A(key);
    errorVector += A * d;
  }

  return errorVector;
}

/* ************************************************************************* */
// LinearizedHessianFactor
/* ************************************************************************* */
LinearizedHessianFactor::LinearizedHessianFactor() {
}

/* ************************************************************************* */
LinearizedHessianFactor::LinearizedHessianFactor(
    const HessianFactor::shared_ptr& hessian, const Values& lin_points)
: Base(hessian, lin_points) {

  // Create the dims array
  size_t *dims = (size_t*)alloca(sizeof(size_t)*(hessian->size() + 1));
  size_t index = 0;
  for(HessianFactor::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) {
    dims[index++] = hessian->getDim(iter);
  }
  dims[index] = 1;

  // Update the BlockInfo accessor
  info_ = SymmetricBlockMatrix(dims, dims+hessian->size()+1);
  // Copy the augmented matrix holding G, g, and f
  info_.full() = hessian->info();
}

/* ************************************************************************* */
void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {

  std::cout << s << std::endl;

  std::cout << "Nonlinear Keys: ";
  BOOST_FOREACH(const Key& key, this->keys())
    std::cout << keyFormatter(key) << " ";
  std::cout << std::endl;

  gtsam::print(Matrix(info_.full()), "Ab^T * Ab: ");

  lin_points_.print("Linearization Point: ");
}

/* ************************************************************************* */
bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol) const {

  const This *e = dynamic_cast<const This*> (&expected);
  if (e) {

    Matrix thisMatrix = this->info_.full();
    thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
    Matrix rhsMatrix = e->info_.full();
    rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;

    return Base::equals(expected, tol)
        && lin_points_.equals(e->lin_points_, tol)
        && equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
  } else {
    return false;
  }
}

/* ************************************************************************* */
double LinearizedHessianFactor::error(const Values& c) const {

  // Construct an error vector in key-order from the Values
  Vector dx = zero(dim());
  size_t index = 0;
  for(unsigned int i = 0; i < this->size(); ++i){
    Key key = this->keys()[i];
    const Value& newPt = c.at(key);
    const Value& linPt = lin_points_.at(key);
    dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
    index += linPt.dim();
  }

  // error 0.5*(f - 2*x'*g + x'*G*x)
  double f = constantTerm();
  double xtg = dx.dot(linearTerm());
  double xGx = dx.transpose() * squaredTerm() * dx;

  return 0.5 * (f - 2.0 * xtg +  xGx);
}

/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedHessianFactor::linearize(const Values& c) const {

  // Construct an error vector in key-order from the Values
  Vector dx = zero(dim());
  size_t index = 0;
  for(unsigned int i = 0; i < this->size(); ++i){
    Key key = this->keys()[i];
    const Value& newPt = c.at(key);
    const Value& linPt = lin_points_.at(key);
    dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
    index += linPt.dim();
  }

  // f2 = f1 - 2*dx'*g1 + dx'*G1*dx
  //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
  double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx;

  // g2 = g1 - G1*dx
  //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx;
  Vector g = linearTerm() - squaredTerm() * dx;
  std::vector<Vector> gs;
  for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
    gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i)));
  }

  // G2 = G1
  // Do Nothing
  std::vector<Matrix> Gs;
  for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
    for(DenseIndex j = i; j < info_.nBlocks()-1; ++j) {
      Gs.push_back(info_(i,j));
    }
  }

  // Create a Hessian Factor from the modified info matrix
  //return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
  return boost::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f));
}

} // \namespace aspn


#include "multi_alignment_model.h"
#include <blitz/tinyvec.h>
#include "io.h"

multi_alignment_model::multi_alignment_model( int i ) : nmodels(i), PHMM(kstates+i*SUBSIZE), dirty(true), stop_at_repeat(false) {
	set_default_parameters( blitz::TinyVector<double,4>(0.25,0.25,0.25,0.25) );
}

void multi_alignment_model::reset() { store.clear(); dstore.clear(); }

void multi_alignment_model::start_training() {
	PHMM.start_training();
}

ereal multi_alignment_model::end_training() {
	ereal pr = PHMM.end_training();

	// We need to normalize the model.
	for( int k = 0; k < nmodels; ++k ) {
		int len = get_region_length(k);
		nw_model_parameters_ptr p = get_parameters(k);
		set_parameters( k, *p, len );
	}
	return pr;
}

void multi_alignment_model::set_default_parameters( blitz::TinyVector<double,4> nq ) {
	using namespace std;
	using namespace boost;
	assert(nmodels < 30);

	typedef boost::tuple<double,double,double> double3;
	vector<double3> custom_pts;
	custom_pts.push_back( double3(0.4,0.0625,2.0) );
	custom_pts.push_back( double3(0.2,0.03125,1.333) );
	custom_pts.push_back( double3(0.6,0.0625,3.41424) );
	custom_pts.push_back( double3(0.8,0.03125,2.0) );

	assert( (unsigned int)nmodels <= custom_pts.size() );
	for( int k = 0; k < nmodels; ++k ) {
		nw_model_parameters newp;
		newp.q = nq;
		newp.set_p_hky( custom_pts[k].get<0>(), 1.0 );
		newp.pr_open = custom_pts[k].get<1>();
		newp.mean_gap_length = custom_pts[k].get<2>();
		set_parameters( k, newp, 200 );

		int ms = kstates+k*SUBSIZE;
		// set switch
		PHMM.set_transition(state_switch,ms+state_S,1.0/nmodels);

		// set end to switch
		PHMM.set_transition(ms+state_E,state_switch,1.0);

		// set silent states for model 0
		PHMM.set_emmission(ms+state_S,APHMM::NOTHING,APHMM::NOTHING,1.0);
		PHMM.set_emmission(ms+state_E,APHMM::NOTHING,APHMM::NOTHING,1.0);
	}

	// set silent states for switch and random
	PHMM.set_emmission(state_switch,APHMM::NOTHING,APHMM::NOTHING,1.0);

	// set start and end states
	PHMM.set_start_state(state_switch);
	PHMM.set_end_state(state_switch);

	set_random_distribution(nq);
	PHMM.optimize();
	dirty = false;
}

int multi_alignment_model::num_models() const {
	return nmodels;
}

double multi_alignment_model::get_region_pr( int k ) const {
	return (double)PHMM.get_transition(state_switch,kstates + k*SUBSIZE + state_S);
}

double multi_alignment_model::get_region_length( int k ) const {
	// since we didn't tie the "t" parts, we'll just average all three values
	int ms = kstates + k*SUBSIZE;
	double sum = (double)PHMM.get_transition(ms+state_M,ms+state_E);
	sum += (double)PHMM.get_transition(ms+state_Ia,ms+state_E);
	sum += (double)PHMM.get_transition(ms+state_Ib,ms+state_E);
	double p_1mt = sum/3.0;
	return 1.0/p_1mt;
}

void multi_alignment_model::set_random_distribution( blitz::TinyVector<double,4> nq ) {
	dirty = true;
	for( int i = 0; i < 4; ++i) q(i) = nq(i);
}

void multi_alignment_model::set_stop_at_repeat( bool t ) {
	stop_at_repeat = t;
}

void multi_alignment_model::set_parameters( int k, nw_model_parameters const &nwp, int len ) {
	dirty = true;
	double pt = 1.0-1.0/(double)len;
	int ms = kstates + k*SUBSIZE;

	// set open
    double p_at = nwp.pr_open*pt;
	double p_1m2at = (1.0-2.0*nwp.pr_open)*pt;
	PHMM.set_transition(ms+state_M,ms+state_Ia,p_at);
	PHMM.set_transition(ms+state_M,ms+state_Ib,p_at);
	PHMM.set_transition(ms+state_M,ms+state_M,p_1m2at);

	// set start
	double p_a = nwp.pr_open;
	double p_1m2a = 1.0 - 2.0*nwp.pr_open;
	PHMM.set_transition(ms+state_S,ms+state_Ia,p_a);
	PHMM.set_transition(ms+state_S,ms+state_Ib,p_a);
	PHMM.set_transition(ms+state_S,ms+state_M,p_1m2a);

	// Since a gap is length 1 by the virtue of opening it, we adjust n down by 1.
	// The remaining gap extension probability corresponds to the mean of a
	// geometric distribution allowing zero extensions.
	double p_b = 1.0-1.0/nwp.mean_gap_length;
	double p_bt = p_b*pt;
	double p_1mbt = (1.0 - p_b)*pt;
	PHMM.set_transition(ms+state_Ia,ms+state_Ia,p_bt);
	PHMM.set_transition(ms+state_Ia,ms+state_M,p_1mbt);
	PHMM.set_transition(ms+state_Ib,ms+state_Ib,p_bt);
	PHMM.set_transition(ms+state_Ib,ms+state_M,p_1mbt);

	// set end transitions
	double p_1mt = 1.0 - pt;
	PHMM.set_transition( ms+state_M,ms+state_E,p_1mt);
	PHMM.set_transition( ms+state_Ia,ms+state_E,p_1mt);
	PHMM.set_transition( ms+state_Ib,ms+state_E,p_1mt);

	// set background
	for( int i = 0; i < 4; i++ ) {
      PHMM.set_emmission(ms+state_Ia,i,APHMM::NOTHING,nwp.q(i));
      PHMM.set_emmission(ms+state_Ib,APHMM::NOTHING,i,nwp.q(i));
   }

	// set p
	for( int i = 0; i < 4; i++ ) {
		for( int j = 0; j < 4; j++ ) {
			PHMM.set_emmission(ms+state_M,i,j,nwp.p(i,j));
		}
   }
}

nw_model_parameters_ptr multi_alignment_model::get_parameters( int k ) const {
	nw_model_parameters_ptr nwp( new nw_model_parameters() );
	int ms = kstates + SUBSIZE*k;
	double t = 1.0 - 1.0/get_region_length(k);
	nwp->pr_open = (double) PHMM.get_transition(ms+state_M,ms+state_Ia)/t;
	nwp->pr_open += (double) PHMM.get_transition(ms+state_M,ms+state_Ib)/t;
	nwp->pr_open /= 2.0;

	double gap_a = 1.0/(1.0- (double)PHMM.get_transition(ms+state_Ia,ms+state_Ia)/t );
	double gap_b = 1.0/(1.0- (double)PHMM.get_transition(ms+state_Ib,ms+state_Ib)/t );
	nwp->mean_gap_length = (gap_a + gap_b)/2.0;

	for( int x = 0; x < APHMM::NOTHING; ++x ) {
		double qx = 0.0;
		for( int y = 0; y < APHMM::NOTHING; ++y ) {
			nwp->p(x,y) = (double)PHMM.get_emmission(ms+state_M,x,y);
			qx += nwp->p(x,y);
		}
		nwp->q(x) = qx;
	}

	return nwp;
}

blitz::TinyVector<double,4> multi_alignment_model::get_random_distribution() const {
	return q;
}

void multi_alignment_model::pretty_print_parameters( std::ostream &out ) const {
	using namespace std;

	out  << "Random background: " << get_random_distribution() << endl;

	for( int k = 0; k < num_models(); ++k ) {
		out << endl << "Model " << k << ": " << get_region_pr(k) << endl;
		out << "Length: " << get_region_length(k) << endl;
		nw_model_parameters_ptr nwp = get_parameters(k);
		nwp->pretty_print(out);
		double pr_match = 0;
		for( int i = 0; i < 4; ++i ) pr_match += nwp->p(i,i);
	}
}

void multi_alignment_model::write_parameters( std::ostream &out ) const {
	using namespace std;

	out  << "# Random background: " << get_random_distribution() << endl << endl;

	for( int j = 0; j < num_models(); ++j ) {
		out << "# Region " << j << " with pr " << get_region_pr(j);
		out << " and length " << get_region_length(j) << "." << endl;
		nw_model_parameters_ptr nwp = get_parameters(j);
		nwp->pretty_print( out, "# " );
		out << endl;
	}
	out << endl << "# Machine readable parameters follow. " << "---------------------------------------------" << endl;

	// Write dummy lengths for random background.
	out << "0.0 200 200 ";
	blitz::TinyVector<double,4> q = get_random_distribution();
	for( int i = 0; i < 4; ++i )  out << q(i) << " ";
	out << endl;

	out << num_models() << endl;
	for( int j = 0; j < num_models(); ++j ) {
		out <<  get_region_pr(j) << " " << get_region_length(j) << " " << *get_parameters(j) << endl;
	}

}

// We don't actually do anything here. This is legacy to take care of mm_aligner.
void multi_alignment_model::disable_random_model() {}

void multi_alignment_model::read_parameters( std::istream &in ) {
	using namespace std;
	vector<double> mdist;
	mdist.push_back( read<double>(in) );
	// Skip unused random model parameters.
	read<double>(in);	read<double>(in);
	for( int i = 0; i < 4; ++i ) q(i) = read<double>(in);

	int nmodels = read<int>(in);
	for( int j = 0; j < nmodels; ++j ) {
		mdist.push_back( read<double>(in) );
		int length = (int)read<double>(in);
		nw_model_parameters param;
		in >> param;
		set_parameters( j, param, length );
	}
	set_region_dist( mdist );
	PHMM.optimize();
	dirty = false;
}


#pragma once

#include <iostream>
#include <memory>
#include <random>
#include <unsupported/Eigen/SpecialFunctions>
#include <vector>

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

namespace myFM {

using namespace std;
namespace types {
template <typename Real>
using DenseMatrix = Eigen::Matrix<Real, -1, -1, Eigen::ColMajor>;

template <typename Real> using Vector = Eigen::Matrix<Real, -1, 1>;

template <typename Real>
using SparseMatrix = Eigen::SparseMatrix<Real, Eigen::RowMajor>;

template <typename Real> using SparseVector = Eigen::SparseVector<Real>;

} // namespace types

namespace relational {

template <typename Real> struct RelationBlock {
  typedef Eigen::SparseMatrix<Real, Eigen::RowMajor> SparseMatrix;
  typedef Eigen::Matrix<Real, -1, 1> Vector;

  inline RelationBlock(vector<size_t> original_to_block, const SparseMatrix &X)
      : original_to_block(original_to_block),
        mapper_size(original_to_block.size()), X(X), block_size(X.rows()),
        feature_size(X.cols()) {
    for (auto c : original_to_block) {
      if (c >= block_size)
        throw runtime_error("index mapping points to non-existing row.");
    }
  }

  inline RelationBlock(const RelationBlock &other)
      : RelationBlock(other.original_to_block, other.X) {}

  const vector<size_t> original_to_block;
  const size_t mapper_size;
  const SparseMatrix X;
  const size_t block_size;
  const size_t feature_size;
};

template <typename Real> struct RelationWiseCache {
  typedef typename RelationBlock<Real>::Vector Vector;
  typedef typename RelationBlock<Real>::SparseMatrix SparseMatrix;

  inline RelationWiseCache(const RelationBlock<Real> &source)
      : target(source), X_t(source.X.transpose()), cardinality(source.X.rows()),
        y(source.X.rows()), q(source.X.rows()), q_S(source.X.rows()),
        c(source.X.rows()), c_S(source.X.rows()), e(source.X.rows()),
        e_q(source.X.rows()) {
    X_t.makeCompressed();
    cardinality.array() = static_cast<Real>(0);
    for (auto v : source.original_to_block) {
      cardinality(v)++;
    }
  }

  const RelationBlock<Real> ⌖
  SparseMatrix X_t;
  Vector cardinality; // for each

  Vector y;

  Vector q;
  Vector q_S;

  Vector c;
  Vector c_S;

  Vector e;
  Vector e_q;
};
} // namespace relational

} // namespace myFM


// Boost.Geometry Index
//
// n-dimensional bounds
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_BOUNDS_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_BOUNDS_HPP

#include <boost/geometry/index/detail/algorithms/bounds.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

template <typename Geometry, typename Bounds, std::size_t DimensionIndex, std::size_t DimensionCount>
struct bounds_nsphere_box
{
    static inline void apply(Geometry const& g, Bounds & b)
    {
        set<min_corner, DimensionIndex>(b, get<DimensionIndex>(g) - get_radius<0>(g));
        set<max_corner, DimensionIndex>(b, get<DimensionIndex>(g) + get_radius<0>(g));
        bounds_nsphere_box<Geometry, Bounds, DimensionIndex+1, DimensionCount>::apply(g, b);
    }
};

template <typename Geometry, typename Bounds, std::size_t DimensionCount>
struct bounds_nsphere_box<Geometry, Bounds, DimensionCount, DimensionCount>
{
    static inline void apply(Geometry const& , Bounds & ) {}
};

namespace dispatch {

template <typename Geometry, typename Bounds>
struct bounds<Geometry, Bounds, nsphere_tag, box_tag>
    : bounds_nsphere_box<Geometry, Bounds, 0, dimension<Geometry>::value>
{};

} // namespace dispatch

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_BOUNDS_HPP