/*
 * 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 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


#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_typedefs.hpp>
#include <map>
#include <vector>

#ifndef ABB_IRB1600_145_FK_FAST_HPP
#define ABB_IRB1600_145_FK_FAST_HPP

namespace ABB_IRB1600_145_FK_FAST {
const size_t ABB_IRB1600_145_NUM_ACTIVE_JOINTS = 6;
const size_t ABB_IRB1600_145_NUM_LINKS = 8;

const std::string ABB_IRB1600_145_ACTIVE_JOINT_1_NAME = "joint_1";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_2_NAME = "joint_2";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_3_NAME = "joint_3";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_4_NAME = "joint_4";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_5_NAME = "joint_5";
const std::string ABB_IRB1600_145_ACTIVE_JOINT_6_NAME = "joint_6";

const std::string ABB_IRB1600_145_LINK_1_NAME = "link_0";
const std::string ABB_IRB1600_145_LINK_2_NAME = "link_1";
const std::string ABB_IRB1600_145_LINK_3_NAME = "link_2";
const std::string ABB_IRB1600_145_LINK_4_NAME = "link_3";
const std::string ABB_IRB1600_145_LINK_5_NAME = "link_4";
const std::string ABB_IRB1600_145_LINK_6_NAME = "link_5";
const std::string ABB_IRB1600_145_LINK_7_NAME = "link_6";
const std::string ABB_IRB1600_145_LINK_8_NAME = "link_7";

inline Eigen::Isometry3d Get_base_joint1_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1245);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.15, -0.1395, 0.362);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.0, 0.028, 0.7);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.314, 0.107, 0.0);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.286, 0.0, 0.0);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val) {
  Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
  Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_Fixed_link_6_joint_tool_LinkJointTransform(void) {
  Eigen::Translation3d pre_joint_translation(0.065, 0.0, 0.0);
  Eigen::Quaterniond pre_joint_rotation(0.7071067811865476, 0.0, 0.7071067811865476, 0.0);
  Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  return pre_joint_transform;
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  assert(configuration.size() == ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
  EigenHelpers::VectorIsometry3d link_transforms(ABB_IRB1600_145_NUM_LINKS);
  link_transforms[0] = base_transform;
  link_transforms[1] = link_transforms[0] * Get_base_joint1_LinkJointTransform(configuration[0]);
  link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
  link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
  link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
  link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
  link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
  link_transforms[7] = link_transforms[6] * Get_Fixed_link_6_joint_tool_LinkJointTransform();
  return link_transforms;
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    std::map<std::string, double> configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  std::vector<double> configuration_vector(ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
  configuration_vector[0] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_1_NAME];
  configuration_vector[1] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_2_NAME];
  configuration_vector[2] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_3_NAME];
  configuration_vector[3] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_4_NAME];
  configuration_vector[4] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_5_NAME];
  configuration_vector[5] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_6_NAME];
  return GetLinkTransforms(configuration_vector, base_transform);
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}
}  // namespace ABB_IRB1600_145_FK_FAST

#endif  // ABB_IRB1600_145_FK_FAST_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.
//
// Written by Jiahu Deng and Peter Gottschling

#ifndef MTL_DILATED_INT_INCLUDE
#define MTL_DILATED_INT_INCLUDE

#include <boost/numeric/mtl/detail/masked_dilation_tables.hpp>
#include <iostream>

namespace mtl { namespace dilated {

template <typename T>
struct even_bits
{
    static T const value = T(-1) / T(3);
};

template <typename T>
struct odd_bits
{
    static T const value = ~even_bits<T>::value;
};

// And is mostly used with original mask
template <typename T, T BitMask, bool Normalized>
struct masking
{
    inline void operator() (T& x) const
    {
	x &= BitMask;
    }
};

// Or is mostly used with complementary mask
template <typename T, T BitMask>
struct masking<T, BitMask, false>
{
    inline void operator() (T& x) const
    {
	static T const anti_mask = ~BitMask;
	x |= anti_mask;
    }
};

template <typename T, T BitMask>
struct last_bit;

template <typename T, T BitMask, bool IsZero>
struct last_bit_helper {
    static T const tmp = BitMask >> 1;
    static T const value = BitMask & 1 ? 1 : last_bit_helper<T, tmp, tmp == 0>::value << 1;
};

template <typename T, T BitMask>
struct last_bit_helper<T, BitMask, true> {
    static T const value = 0;
};

template <typename T, T BitMask>
struct last_bit
{
    static T const value = last_bit_helper<T, BitMask, BitMask == 0>::value;
};

template <typename T, T BitMask, bool Normalized>
struct dilated_int
{
    typedef T                                       value_type;
    typedef dilated_int<T, BitMask, Normalized>     self;

    typedef masking<T, BitMask, Normalized>         clean_carry;
    typedef masking<T, BitMask, !Normalized>        init_carry;

    static T const       bit_mask = BitMask,
	                 anti_mask = ~BitMask,
	                 dilated_zero = Normalized ? 0 : anti_mask,
			 dilated_one = dilated_zero + last_bit<T, bit_mask>::value;
protected:
    // masked_dilation_tables<T, bit_mask>   mask_tables;
    // masked_dilation_tables<T, anti_mask>  anti_tables; probably not needed

// will be protected later
public:
    T i;

    void dilate(T x)
    {
		static const T to_switch_on = Normalized ? 0 : anti_mask;
		// auto aa = mask<bit_mask>(x) | to_switch_on;
		// auto ab = mask<static_cast<T>(bit_mask)>(x) | to_switch_on
		i = mask<bit_mask>(x) | to_switch_on;
    }

public:

    // Default constructor
    dilated_int()
    {
	i = Normalized ? 0 : anti_mask;
    }

    // Only works for odd and even bits and 4-byte-int at this point !!!!!!!!!!!!!!!!!!!
    explicit dilated_int(T x)
    {
	dilate(x);
    }

    // Only works for odd and even bits and 4-byte-int at this point !!!!!!!!!!!!!!!!!!!
    T undilate()
    {
	return unmask<bit_mask>(i);
    }

    T dilated_value() const
    {
	return i;
    }

    self& operator= (self const& x)
    {
	i = x.i;
	return *this;
    }

    self& operator= (T x)
    {
	dilate(x);
	return *this;
    }

    self& operator++ ()
    {
	static T const x = Normalized ? bit_mask : T(-1);
	i -= x;
	clean_carry()(i);
	return *this;
    }

    self operator++ (int)
    {
	self tmp(*this);
	++*this;
	return tmp;
    }

    self& operator+= (self const& x)
    {
	init_carry()(i);
	i+= x.i;
	clean_carry()(i);
	return *this;
    }

    self operator+ (self const& x)
    {
	self tmp(*this);
	return tmp += x;
    }

    self& operator-- ()
    {
	i -= dilated_one;
	clean_carry()(i);
	return *this;
    }

    self operator-- (int)
    {
	self tmp(*this);
	--*this;
	return tmp;
    }

    self& operator-= (self const& x)
    {
	i -= x.i;
	clean_carry()(i);
	return *this;
    }

    self operator- (self const& x) const
    {
	self tmp(*this);
	return tmp -= x;
    }

    // advance in both directions, special care is needed for negative values
    self& advance(long inc)
    {
	value_type incv(inc >= 0 ? inc : -inc);
	self incd(incv);
	if (inc >= 0)
	    operator+=(incd);
	else
	    operator-=(incd);
	return *this;
    }

    bool operator== (self const& x) const
    {
	return i == x.i;
    }

    bool operator!= (self const& x) const
    {
	return i != x.i;
    }

    bool operator<= (self const& x) const
    {
	return i <= x.i;
    }

    bool operator< (self const& x) const
    {
	return i < x.i;
    }

    bool operator>= (self const& x) const
    {
	return i >= x.i;
    }

    bool operator> (self const& x) const
    {
	return i > x.i;
    }

};

} // namespace mtl::dilated

using dilated::dilated_int;

} // namespace mtl

template <typename T, T BitMask, bool Normalized>
inline std::ostream& operator<< (std::ostream& os, mtl::dilated::dilated_int<T, BitMask, Normalized> d)
{
    os.setf(std::ios_base::hex, std::ios_base::basefield);
    return os << d.i;
}

#endif // MTL_DILATED_INT_INCLUDE


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include <cstdio>
#include <cstdlib>
#include <stdexcept>
#include <boost/lexical_cast.hpp>

#include "kernelregressor.hpp"

#include "log.hpp"
#include "ublas_extra.hpp"

namespace bayesopt
{
  KernelRegressor::KernelRegressor(size_t dim, Parameters parameters,
				   const Dataset& data,
				   MeanModel& mean, randEngine& eng):
    NonParametricProcess(dim,parameters,data,mean,eng),
    mRegularizer(parameters.noise),
    mKernel(dim, parameters),
    mScoreType(parameters.sc_type),
    mLearnType(parameters.l_type),
    mLearnAll(parameters.l_all)
  { }

  KernelRegressor::~KernelRegressor(){}

  void KernelRegressor::updateSurrogateModel()
  {
    const vectord lastX = mData.getLastSampleX();
    vectord newK = computeCrossCorrelation(lastX);
    newK(newK.size()-1) += mRegularizer;   // We add it to the last element
    utils::cholesky_add_row(mL,newK);
    precomputePrediction();
  } // updateSurrogateModel

  void KernelRegressor::computeCholeskyCorrelation()
  {
    size_t nSamples = mData.getNSamples();
    mL.resize(nSamples,nSamples);

    //  const matrixd K = computeCorrMatrix();
    matrixd K(nSamples,nSamples);
    computeCorrMatrix(K);
    size_t line_error = utils::cholesky_decompose(K,mL);
    if (line_error)
      {
	throw std::runtime_error("Cholesky decomposition error at line " +
				 boost::lexical_cast<std::string>(line_error));
      }
  }

  matrixd KernelRegressor::computeDerivativeCorrMatrix(int dth_index)
  {
    const size_t nSamples = mData.getNSamples();
    matrixd corrMatrix(nSamples,nSamples);
    mKernel.computeDerivativeCorrMatrix(mData.mX,corrMatrix,dth_index);
    return corrMatrix;
  }

} //namespace bayesopt


/*

Copyright (c) 2015, Project OSRM contributors
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.

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.

*/

#ifndef HIDDEN_MARKOV_MODEL
#define HIDDEN_MARKOV_MODEL

#include "../util/integer_range.hpp"

#include <boost/assert.hpp>

#include <cmath>

#include <limits>
#include <vector>

namespace osrm
{
namespace matching
{
static const double log_2_pi = std::log(2. * M_PI);
static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits<double>::infinity();
static const double MINIMAL_LOG_PROB = std::numeric_limits<double>::lowest();
static const std::size_t INVALID_STATE = std::numeric_limits<std::size_t>::max();
} // namespace matching
} // namespace osrm

// closures to precompute log -> only simple floating point operations
struct EmissionLogProbability
{
    double sigma_z;
    double log_sigma_z;

    EmissionLogProbability(const double sigma_z) : sigma_z(sigma_z), log_sigma_z(std::log(sigma_z))
    {
    }

    double operator()(const double distance) const
    {
        return -0.5 * (osrm::matching::log_2_pi + (distance / sigma_z) * (distance / sigma_z)) -
               log_sigma_z;
    }
};

struct TransitionLogProbability
{
    double beta;
    double log_beta;
    TransitionLogProbability(const double beta) : beta(beta), log_beta(std::log(beta)) {}

    double operator()(const double d_t) const { return -log_beta - d_t / beta; }
};

template <class CandidateLists> struct HiddenMarkovModel
{
    std::vector<std::vector<double>> viterbi;
    std::vector<std::vector<std::pair<unsigned, unsigned>>> parents;
    std::vector<std::vector<float>> path_lengths;
    std::vector<std::vector<bool>> pruned;
    std::vector<std::vector<bool>> suspicious;
    std::vector<bool> breakage;

    const CandidateLists &candidates_list;
    const EmissionLogProbability &emission_log_probability;

    HiddenMarkovModel(const CandidateLists &candidates_list,
                      const EmissionLogProbability &emission_log_probability)
        : breakage(candidates_list.size()), candidates_list(candidates_list),
          emission_log_probability(emission_log_probability)
    {
        viterbi.resize(candidates_list.size());
        parents.resize(candidates_list.size());
        path_lengths.resize(candidates_list.size());
        suspicious.resize(candidates_list.size());
        pruned.resize(candidates_list.size());
        breakage.resize(candidates_list.size());
        for (const auto i : osrm::irange<std::size_t>(0u, candidates_list.size()))
        {
            const auto& num_candidates = candidates_list[i].size();
            // add empty vectors
            if (num_candidates > 0)
            {
                viterbi[i].resize(num_candidates);
                parents[i].resize(num_candidates);
                path_lengths[i].resize(num_candidates);
                suspicious[i].resize(num_candidates);
                pruned[i].resize(num_candidates);
            }
        }

        clear(0);
    }

    void clear(std::size_t initial_timestamp)
    {
        BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_lengths.size() &&
                     path_lengths.size() == pruned.size() && pruned.size() == breakage.size());

        for (const auto t : osrm::irange(initial_timestamp, viterbi.size()))
        {
            std::fill(viterbi[t].begin(), viterbi[t].end(), osrm::matching::IMPOSSIBLE_LOG_PROB);
            std::fill(parents[t].begin(), parents[t].end(), std::make_pair(0u, 0u));
            std::fill(path_lengths[t].begin(), path_lengths[t].end(), 0);
            std::fill(suspicious[t].begin(), suspicious[t].end(), true);
            std::fill(pruned[t].begin(), pruned[t].end(), true);
        }
        std::fill(breakage.begin() + initial_timestamp, breakage.end(), true);
    }

    std::size_t initialize(std::size_t initial_timestamp)
    {
        auto num_points = candidates_list.size();
        do
        {
            BOOST_ASSERT(initial_timestamp < num_points);

            for (const auto s : osrm::irange<std::size_t>(0u, viterbi[initial_timestamp].size()))
            {
                viterbi[initial_timestamp][s] =
                    emission_log_probability(candidates_list[initial_timestamp][s].second);
                parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                pruned[initial_timestamp][s] =
                    viterbi[initial_timestamp][s] < osrm::matching::MINIMAL_LOG_PROB;
                suspicious[initial_timestamp][s] = false;

                breakage[initial_timestamp] =
                    breakage[initial_timestamp] && pruned[initial_timestamp][s];
            }

            ++initial_timestamp;
        } while (initial_timestamp < num_points && breakage[initial_timestamp - 1]);

        if (initial_timestamp >= num_points)
        {
            return osrm::matching::INVALID_STATE;
        }

        BOOST_ASSERT(initial_timestamp > 0);
        --initial_timestamp;

        BOOST_ASSERT(breakage[initial_timestamp] == false);

        return initial_timestamp;
    }
};

#endif // HIDDEN_MARKOV_MODEL


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP

#include <string>
#include <vector>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/projections/impl/pj_ellps.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_param.hpp>

namespace boost { namespace geometry { namespace projections {

namespace detail {

/* set ellipsoid parameters a and es */
static const double SIXTH =  .1666666666666666667; /* 1/6 */
static const double RA4 = .04722222222222222222; /* 17/360 */
static const double RA6 = .02215608465608465608; /* 67/3024 */
static const double RV4 = .06944444444444444444; /* 5/72 */
static const double RV6 = .04243827160493827160; /* 55/1296 */

/* initialize geographic shape parameters */
inline void pj_ell_set(std::vector<pvalue>& parameters, double &a, double &es)
{
    double b = 0.0;
    double e = 0.0;
    std::string name;

    /* check for varying forms of ellipsoid input */
    a = es = 0.;

    /* R takes precedence */
    if (pj_param(parameters, "tR").i)
        a = pj_param(parameters, "dR").f;
    else { /* probable elliptical figure */

        /* check if ellps present and temporarily append its values to pl */
        name = pj_param(parameters, "sellps").s;
        if (! name.empty())
        {
            const int n = sizeof(pj_ellps) / sizeof(pj_ellps[0]);
            int index = -1;
            for (int i = 0; i < n && index == -1; i++)
            {
                if(pj_ellps[i].id == name)
                {
                    index = i;
                }
            }

            if (index == -1) { throw proj_exception(-9); }

            parameters.push_back(pj_mkparam(pj_ellps[index].major));
            parameters.push_back(pj_mkparam(pj_ellps[index].ell));
        }
        a = pj_param(parameters, "da").f;
        if (pj_param(parameters, "tes").i) /* eccentricity squared */
            es = pj_param(parameters, "des").f;
        else if (pj_param(parameters, "te").i) { /* eccentricity */
            e = pj_param(parameters, "de").f;
            es = e * e;
        } else if (pj_param(parameters, "trf").i) { /* recip flattening */
            es = pj_param(parameters, "drf").f;
            if (!es) {
                throw proj_exception(-10);
            }
            es = 1./ es;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tf").i) { /* flattening */
            es = pj_param(parameters, "df").f;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tb").i) { /* minor axis */
            b = pj_param(parameters, "db").f;
            es = 1. - (b * b) / (a * a);
        }     /* else es == 0. and sphere of radius a */
        if (!b)
            b = a * sqrt(1. - es);
        /* following options turn ellipsoid into equivalent sphere */
        if (pj_param(parameters, "bR_A").i) { /* sphere--area of ellipsoid */
            a *= 1. - es * (SIXTH + es * (RA4 + es * RA6));
            es = 0.;
        } else if (pj_param(parameters, "bR_V").i) { /* sphere--vol. of ellipsoid */
            a *= 1. - es * (SIXTH + es * (RV4 + es * RV6));
            es = 0.;
        } else if (pj_param(parameters, "bR_a").i) { /* sphere--arithmetic mean */
            a = .5 * (a + b);
            es = 0.;
        } else if (pj_param(parameters, "bR_g").i) { /* sphere--geometric mean */
            a = sqrt(a * b);
            es = 0.;
        } else if (pj_param(parameters, "bR_h").i) { /* sphere--harmonic mean */
            a = 2. * a * b / (a + b);
            es = 0.;
        } else {
            int i = pj_param(parameters, "tR_lat_a").i;
            if (i || /* sphere--arith. */
                pj_param(parameters, "tR_lat_g").i) { /* or geom. mean at latitude */
                double tmp;

                tmp = sin(pj_param(parameters, i ? "rR_lat_a" : "rR_lat_g").f);
                if (geometry::math::abs(tmp) > geometry::math::half_pi<double>()) {
                    throw proj_exception(-11);
                }
                tmp = 1. - es * tmp * tmp;
                a *= i ? .5 * (1. - es + tmp) / ( tmp * sqrt(tmp)) :
                    sqrt(1. - es) / tmp;
                es = 0.;
            }
        }
    }

    /* some remaining checks */
    if (es < 0.)
        { throw proj_exception(-12); }
    if (a <= 0.)
        { throw proj_exception(-13); }
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP


/**
 *  staticplan.cpp
 *
 *  Generate a static plan for a planar vehicle to satisfy a DBA
 *  by using abstraction-based control synthesis.
 *
 *  Created by Yinan Li on Feb. 8, 2021.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <cstdlib>
#include <sys/stat.h>
#include <boost/algorithm/string.hpp>

#include "src/DBAparser.h"
#include "src/abstraction.hpp"

#include "src/bsolver.hpp"
#include "src/patcher.h"

#include "src/hdf5io.h"

#include "car.hpp"

int main(int argc, char *argv[])
{
    /**
     * Default values
     **/
    std::string specfile;
    double eta[]{0.2, 0.2, 0.2}; /* partition precision */

    /* Input arguments:
     * carAbst dbafile precision(e.g. 0.2 0.2 0.2)
     */
    if (argc < 2 || argc > 5) {
	std::cout << "Improper number of arguments.\n";
	std::exit(1);
    }
    specfile = std::string(argv[1]);
    if (argc > 2 && argc < 4) {
	std::cout << "Input precision should be of 3-dim, e.g. 0.2 0.2 0.2.\n";
	std::exit(1);
    }
    if (argc == 5) {
	for(int i = 2; i < 5; ++i)
	    eta[i-2] = std::atof(argv[i]);
    }
    std::cout << "Partition precision: " << eta[0] << ' '
	      << eta[1] << ' ' << eta[2] << '\n';

    clock_t tb, te;
    /* set the state space */
    const double theta = 3.5;
    double xlb[] = {0, 0, -theta};
    double xub[] = {10, 10, theta};
    /* set the control values */
    double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    double mu[] = {0.3, 0.3};
    /* define the control system */
    rocs::DTCntlSys<carde> car("DBA", tau, carde::n, carde::m);
    car.init_workspace(xlb, xub);
    car.init_inputset(mu, ulb, uub);

    /**
     * Construct and save the abstraction
     */
    // const double eta[] = {0.2, 0.2, 0.2}; /* set precision */
    rocs::abstraction<rocs::DTCntlSys<carde>> abst(&car);
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    /* Assign the label of avoid area to -1 */
    rocs::UintSmall nAvoid = 6;
    double obs[6][4] = {
	{0.0, 1.0, 4.6, 6.4},
	{2.0, 3.0, 0.0, 3.6},
	{3.5, 4.5, 8.5, 10.0}, //{3.5, 4.5, 7.5, 10.0},
	{5.5, 6.5, 0.0, 1.0},
	{5.5, 6.5, 3.4, 5.5}, //{5.5, 6.5, 3.4, 6.5},
	{6.5, 10.0, 4.5, 5.5} //{6.5, 10.0, 5.5, 6.5}
    };
    auto label_avoid = [&obs, &nAvoid, &abst, &eta](size_t i) {
    		     std::vector<double> x(abst._x._dim);
    		     abst._x.id_to_val(x, i);
    		     double c1= eta[0]/2.0; //+1e-10;
    		     double c2= eta[1]/2.0; //+1e-10;
    		     for(size_t i = 0; i < nAvoid; ++i) {
    			 if ((obs[i][0]-c1) <= x[0] && x[0] <= (obs[i][1]+c1) &&
    			     (obs[i][2]-c2) <= x[1] && x[1] <= (obs[i][3]+c2))
    			     return -1;
    		     }
    		     return 0;
    		 };
    abst.assign_labels(label_avoid);
    abst.assign_label_outofdomain(-1); // out of domain is banned
    std::vector<size_t> obstacles;
    for (size_t i = 0; i < abst._x._nv; ++i) {
    	if (abst._labels[i] < 0)
    	    obstacles.push_back(i);
    }
    /* Compute abstraction */
    std::string suffix;
    for(auto &item : eta) {
	std::stringstream ss;
    	ss << std::setprecision(1);
	ss << item;
	suffix += '-';
	suffix += ss.str();
    }
    std::string transfile = "abstfull" + suffix + ".h5";
    struct stat buffer;
    float tabst;
    if(stat(transfile.c_str(), &buffer) == 0) {
	/* Read from a file */
	std::cout << "Transitions have been computed. Reading transitions...\n";
	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
	tb = clock();
	transRdr.read_transitions(abst._ts);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of reading abstraction: " << tabst << '\n';
	std::cout << "# of transitions: " << abst._ts._ntrans << '\n';
    } else {
	std::cout << "Transitions haven't been computed. Computing transitions...\n";
	/* Robustness margins */
	double e1[] = {0,0,0};
	double e2[] = {0,0,0};
	tb = clock();
	abst.assign_transitions(e1, e2);
	// abst.assign_transitions();
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of computing abstraction: " << tabst << '\n';
	std::cout << "# of transitions: " << abst._ts._ntrans << '\n';
	/* Write abstraction to file */
	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
	transWtr.write_transitions(abst._ts);
	transWtr.write_array<size_t>(obstacles, "obs");
	transWtr.write_array<double>(eta, carde::n, "eta");
	transWtr.write_2d_array<double>(abst._x._data, "xgrid");
	transWtr.write_problem_setting< rocs::DTCntlSys<carde> >(car);
    }

    /**
     * Read DBA from dba*.txt file
     */
    std::cout << "Reading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);
    std::vector<std::string> tokens;
    boost::split(tokens, specfile, boost::is_any_of("."));

    /*
     * Assign labels to states: has to be consistent with the dba file.
     */
    rocs::UintSmall nGoal = 3;
    double e = tau/2.0;
    double goal[3][4] = {{0.5+e, 2.0-e, 7.5+e, 9.5-e},
			 {8.0, 0.8-e, 8.0, 0.8-e}, // {5.5, 0.5-e, 8.5, 0.5-e},
			 {7.5+e, 9.5-e, 0.8+e, 3.0-e}}; //{7.5+e, 9.5-e, 0.8+e, 4.0-e}
    auto label_target = [&goal, &nGoal, &abst, &eta](size_t i) {
		      std::vector<double> x(abst._x._dim);
		      abst._x.id_to_val(x, i);
		      // double c1= eta[0]/2.0; //+1e-10;
		      // double c2= eta[1]/2.0; //+1e-10;
		      double xl = x[0] - eta[0]/2.;
		      double xr = x[0] + eta[0]/2.;
		      double yl = x[1] - eta[1]/2.;
		      double yr = x[1] + eta[1]/2.;
		      boost::dynamic_bitset<> label(nGoal, false); // n is the number of goals
		      for(rocs::UintSmall i = 0; i < nGoal; ++i) {
			  if(i != 1) {
			  label[2-i] = (goal[i][0] <= xl && xr <= goal[i][1] &&
					goal[i][2] <= yl && yr <= goal[i][3])
			      ? true: false;
			  } else {
			      double rxr = xr - goal[i][0];
			      double rxl = xl - goal[i][0];
			      double ryr = yr - goal[i][2];
			      double ryl = yl - goal[i][2];
			      double xsqr = (rxr*rxr) < (rxl*rxl) ? (rxl*rxl) : (rxr*rxr);
			      double ysqr = (ryr*ryr) < (ryl*ryl) ? (ryl*ryl) : (ryr*ryr);
			      label[2-i] = (xsqr+ysqr)<goal[i][1]*goal[i][3] ? true : false;
			  }
		      }
		      return label.to_ulong();
		  };
    abst.assign_labels(label_target);

    std::cout << "Save labels to file.\n";
    std::string labelfile = "labels_" + tokens[0] + "_" + transfile;
    rocs::h5FileHandler labelWtr(labelfile, H5F_ACC_TRUNC);
    labelWtr.write_array<int>(abst._labels, "labels");
    std::cout << "Specification assignment is done.\n";

    /**
     * Solve a Buchi game on the product of NTS and DBA.
     */
    std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    rocs::BSolver solver; // memories will be allocated for psolver
    solver.construct_dba((int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    tb = clock();
    solver.load_abstraction(abst);
    solver.generate_product(abst);
    solver.solve_buchigame_on_product();
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of synthesizing controller: " << tsyn << '\n';

    /**
     * Display and save memoryless controllers.
     */
    std::string datafile = "controller_" + tokens[0] + "_";
    for(int i = 0; i < 3; ++i) {
    	std::stringstream ss;
    	ss << std::setprecision(1);
    	ss << eta[i];
    	datafile += ss.str();
    	if (i < 2)
    	    datafile += "-";
    }
    datafile += ".h5";
    std::cout << "Writing the controller...\n";
    // solver.write_controller_to_txt(const_cast<char*>(datafile.c_str()));
    rocs::h5FileHandler ctlrWtr(datafile, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::DTCntlSys<carde> >(car);
    // ctlrWtr.write_2d_array<double>(targetPts, "G");
    ctlrWtr.write_array<double>(eta, carde::n, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(&(solver._sol));
    std::cout << "Controller writing is done.\n";

    std::cout << "Total time of used (abstraction+synthesis): " << tabst+tsyn << '\n';

    /**
     * Create a patcher and save the winning graph
     */
    std::string winfile = "gwin.h5";
    rocs::Patcher local;
    std::cout << "Extracting the winning graph...\n";
    tb = clock();
    local.initialize_winning_graph(solver._sol);
    te = clock();
    std::cout << "Time of extracting the winning graph: " << (float)(te - tb)/CLOCKS_PER_SEC << '\n';

    /* Save the winning graph to a file */
    std::cout << "Writing the winning graph to file...\n";
    rocs::h5FileHandler graphWtr(winfile, H5F_ACC_TRUNC);
    tb = clock();
    if(graphWtr.write_winning_graph(local)) {
    	std::cout << "Error in saving the winning graph to file.\n";
    	return 1;
    }
    te = clock();
    std::cout << "Time of writing the winning graph: " << (float)(te - tb)/CLOCKS_PER_SEC << '\n';

    return 0;
}


//==================================================//
///
/// @file   MyFFT.cpp
/// @brief  Implementation of MyFFT.
/// @author cogoto
///
/// [MyAll]
/// Copyright (c) 2021 cogoto
/// Released under the MIT license
/// https://opensource.org/licenses/mit-license.php
///
//==================================================//

#include "MyFFT.hpp"

#include <algorithm>

#include <chrono>

#include <cstdint>

#include <ctime>

//#include <execution>

#include <iostream>

#include <iomanip>

//#include <Eigen/Dense>

#include <complex>

#include <thread>

#include <time.h>

//

namespace MyAll {

//

using namespace std;

//using namespace Eigen;

//using fint = int_fast32_t;

//using ufint = uint_fast32_t;

//--------------------------------------------------//
///
/// @brief  First process. Initialize member variables.
///
//--------------------------------------------------//

MyFFT::MyFFT(const int &n_data__, const int &n_thread__) {

    print_ = false;

    MyFFT::Init(n_data__, n_thread__);

};

//--------------------------------------------------//
///
/// @brief   End process.
///
//--------------------------------------------------//

MyFFT::~MyFFT() {

};

//--------------------------------------------------//
///
/// @brief   Initialize calculation parameters.
/// @return  None.
///
//--------------------------------------------------//

void MyFFT::Init(const int &n_data__, const int &n_thread__) {

    if(!SetN(n_data__)) { exit(0); }

    //

    double theta = 2 * PI_ / n_;

    complex<double> Wo = complex<double> (cos(theta), -sin(theta));

    W_0_.reserve(n_half_);
    W_0_.push_back(1.0L);
    for(uint_fast32_t i=1;i<n_half_;i++) { W_0_.push_back(W_0_[i-1] * Wo); }

    //

    n_thread_ = n_thread__;

    int n_cpu = static_cast<int>(thread::hardware_concurrency());

    if(n_thread__ <= 0) {
        n_thread_ = n_cpu - 1;
        if(n_thread_ <= 0) { n_thread_ = 1; }
    }
    else if(n_thread_ > n_cpu) {
        n_thread_ = n_cpu;
    }

    if(print_) { cout << "[MyFFT] n_thread = " << n_thread_ << endl; }

    //cout << "[MyFFT] n_thread = " << n_thread_ << endl;

    //

    thread_exec_.reserve(n_thread_);
    //for(int i=0;i<n_thread_;i++) { thread_exec_.push_back(0); }
    for(int i=0;i<n_thread_;i++) { thread_exec_.push_back(-1); }

    //

    n_stream_ = static_cast<uint_fast32_t>(static_cast<double>(n_half_) / static_cast<double>(n_thread_));
    n_stream_ += 1;

    //cout << ":: " << n_ << " : " << n_thread_ << " : " << n_half_ << " : " << n_step_ << " : " << endl;

    //

    vector<vector<uint_fast32_t>>().swap(iW_);
    iW_.resize(n_step_);
    for(auto &&it:iW_) {
        //it.resize(n_);
        it.reserve(n_);
        for(uint_fast32_t i=0;i<n_;i++) { it.push_back(0); }
    }

    //iW_.resize(n_);

    //W_.reserve(n_); // cannot resize the object! (It will occur core-dump)
    //for(uint_fast32_t i=0;i<n_;i++) { W_.push_back(1.0L); }

    //

    vector<vector<uint_fast32_t>>().swap(m_);
    m_.resize(n_step_);
    for(auto &&it:m_) {
        //it.resize(n_half_);
        it.reserve(n_half_);
        for(uint_fast32_t i=0;i<n_half_;i++) { it.push_back(0); }
    }

    //m_.reserve(n_half_);
    //for(uint_fast32_t i=0;i<n_half_;i++) { m_.push_back(0); }

    n_block_half_ = n_;

    uint_fast32_t i0, sss;

    for(uint_fast32_t s=0;s<n_step_;s++) {

        n_block_ = n_block_half_;
        n_block_half_ >>= 1;

        i0 = 0;

        for(uint_fast32_t i1=0; i1<n_block_half_; i1++) {

            sss = (i1 << s);

            for(uint_fast32_t i2=i1; i2<n_; i2+=n_block_) {
                m_[s][i0] = i2;
                iW_[s][i2] = sss;
                i0++;
            }

        }

    }

    //

    i_bit_rev_.resize(n_);

    uint_fast32_t i2;

    for(uint_fast32_t i1=0;i1<n_;i1++) {

        i2 = i1;

        i2 = (((i2 & 0xaaaaaaaa) >> 1) | ((i2 & 0x55555555) << 1));
        i2 = (((i2 & 0xcccccccc) >> 2) | ((i2 & 0x33333333) << 2));
        i2 = (((i2 & 0xf0f0f0f0) >> 4) | ((i2 & 0x0f0f0f0f) << 4));
        i2 = (((i2 & 0xff00ff00) >> 8) | ((i2 & 0x00ff00ff) << 8));
        i2 = ((i2 >> 16) | (i2 << 16)) >> (32 - n_step_);

        if(i2 > i1) {
            i_bit_rev_[i1] = i2;
        }
        else {
            i_bit_rev_[i1] = i1;
            //i_bit_rev_[i1] = -1;
        }

    }

    //

    window_func_name_ = "";

    //SetWindow(""); // rectangular window function

    //

    /*
    uint_fast32_t thread_no = 0;

    vector<thread> ths( n_thread_ );

    for (auto &&th : ths) {

        th = thread(
            &MyFFT::_calc_butterfly,
            this,
            thread_no
        );

        thread_no++;

    }

    for (auto &&th : ths) {
        try {
            if(th.joinable()) {
                th.join();
            }
        } catch(const std::system_error& e) {
            cout << "Caught system_error with code " << e.code() << " meaning " << e.what() << endl;
        }
    }
    */

    //

    return;

};

//--------------------------------------------------//
///
/// @brief   Sets the number of data.
/// @return  bool.
///
//--------------------------------------------------//

bool MyFFT::SetN(const int &n_data__) {

    int n_check_pre = 0;
    int n_check_now = 1;

    for(int i=0;i<32;i++) {

        n_check_pre = n_check_now;

        n_check_now = (n_check_now << 1);

        if(n_check_now == n_data__) { break; }

        if(n_check_pre < n_data__ && n_data__ < n_check_now) {
            cerr << "[ERROR][MyFFT] The number of data is not a multiply of 2." << endl;
            return false;
        }

    }

    n_ = n_data__;

    n_half_ = (n_ >> 1);

    n_step_ = static_cast<uint_fast32_t>(log2(n_));

    //coeff_amp_ = 2.0 / n_;

    //coeff_amp_ = complex<double> (1.0 / n_, 0.0);

    return true;

}

//--------------------------------------------------//
///
/// @brief     Control standard outputs of current status.
/// @param[in] use__ : Print stdout? (yes=true, no=false)
/// @return    None.
///
//--------------------------------------------------//

void MyFFT::Print(const bool &use__){

    print_ = use__;

};

//--------------------------------------------------//
///
/// @brief   Gets frequency values.
/// @return  None.
///
//--------------------------------------------------//

std::vector<double> MyFFT::F(const double &df__, const double &offset__, const bool &wrap_back__) {

    df_ = df__;

    dt_ = 1 / (n_ * df_);

    vector<double> f;

    if(wrap_back__) { f.reserve(n_); }
    else            { f.reserve(n_half_+1); }

    for(uint_fast32_t i=0;i<=n_half_;i++) {
        f.push_back(df_ * i + offset__);
    }

    //f[n_half_] *= -1;

    if(wrap_back__) {

        for(int i=(n_half_-1);i>0;i--) {
            f.push_back(-1 * f[i]);
        }

    }

    return f;

}

//--------------------------------------------------//
///
/// @brief   Gets time values.
/// @return  None.
///
//--------------------------------------------------//

std::vector<double> MyFFT::T(const double &dt__, const double &offset__) {

    dt_ = dt__;

    df_ = 1 / (n_ * dt_);

    vector<double> t;

    t.reserve(n_);

    for(uint_fast32_t i=0;i<n_;i++) {
        t.push_back(dt_ * i + offset__);
    }

    return t;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::_FFT_CT_multi(const vector<double> &t__, vector<complex<double>> &f__) {

    if(n_ != static_cast<uint_fast32_t>(t__.size())) { return false; }

    chrono::system_clock::time_point start, end;
    //chrono::system_clock::time_point start1, end1;

    //struct timespec ts;
    //ts.tv_sec  = 0;
    //ts.tv_nsec = 0;

    start = chrono::system_clock::now();

    //f_ = &f__;

    vector<complex<double>> ().swap(f_);
    f_.reserve(n_);
    for(const auto &it:t__) {
        f_.push_back(complex<double> (it, 0.0));
    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [init val] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //start = chrono::system_clock::now();
    cout << ":" << endl;
    vector<thread> ths( n_thread_ );

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [prep ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //for (int i=0;i<n_thread_;i++) { thread_exec_[i] = 0; }

    //start = chrono::system_clock::now();

    uint_fast32_t thread_no = 0;

    thread_init_cnt_ = 0;

    for (auto &&th : ths) {

        uint_fast32_t i_begin = n_stream_ * thread_no;
        uint_fast32_t i_end   = i_begin + n_stream_;

        if(i_end > n_half_) { i_end = n_half_; }

        th = thread(
            &MyFFT::_calc_butterfly,
            this,
            thread_no,
            i_begin,
            i_end
        );

        //nanosleep(&ts, NULL);

        thread_no++;

    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [make ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //start = chrono::system_clock::now();

    //while(thread_init_cnt_ < n_thread_) {
    //    continue;
    //}

    while(1) {
        {
            lock_guard<mutex> lock(mtx_thread_);
            //lock_guard<recursive_mutex> lock(mtx_thread_);
            if(thread_init_cnt_ == n_thread_) { break; }
        }
    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [wait ini] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    start = chrono::system_clock::now();

    n_block_half_ = n_;

    thread_cnt_ = 0;

    for(s_=0;s_<n_step_;s_++) {
    //for(uint_fast32_t s=1;s<=n_step_;s++) {

        n_block_ = n_block_half_;
        n_block_half_ >>= 1;

        //s_ = s;

        //

        //start1 = chrono::system_clock::now();

        /*{
            lock_guard<mutex> lock(mtx_thread_);
            //lock_guard<recursive_mutex> lock(mtx_thread_);
            n_calc_ = 0;
            for (int i=0;i<n_thread_;i++) {
                thread_exec_[i] = 1;
            }
        }*/

        {
            unique_lock<std::mutex> wait_lock(mtx_wait_);
            thread_ready_ = true;
        }

        cond_.notify_all();

        //end1 = chrono::system_clock::now();
        //cout << "[FFT_CT] [loop] [exec ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end1-start1).count() << endl;

        //

        /*
        start1 = chrono::system_clock::now();

        //int i_test = 0;

        for(;;) {
            {
                lock_guard<mutex> lock(mtx_thread_);
                if(thread_cnt_ >= 0) {
                    cout << ":: " << thread_cnt_ << endl;
                    break;
                }
            }
        }

        end1 = chrono::system_clock::now();
        cout << "[FFT_CT] [loop] [test] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end1-start1).count() << endl;
        */

        //

        //{
            //lock_guard<mutex> lock(mtx_thread_);
            //start1 = chrono::system_clock::now();
            //cout << "[FFT_CT] [loop] wait st = " << time_.HMS_nsec() << endl;
        //}

        int i = 0;
        //while(1) {
        for(;;) {
            {
                lock_guard<mutex> lock(mtx_thread_);
                //lock_guard<recursive_mutex> lock(mtx_thread_);
                //cout << thread_cnt_ << ":" << n_thread_ << endl;
                if(thread_cnt_ == n_thread_) { break; }
            }
            //if(thread_cnt_ >= n_thread_) {
                //cout << "wait count = " << i << endl;
            //    break;
            //}
            i++;
            //if(i==30) {cout << thread_cnt_ << endl;}
            //cout << thread_cnt_ << " " ;
            //cout << i << " " ;
            //nanosleep(&ts, NULL);
            //continue;
        }

        thread_cnt_ = 0;

        thread_ready_ = false;

        /*{
            lock_guard<mutex> lock(mtx_thread_);
            //cout << "[FFT_CT] [loop] wait et = " << time_.HMS_nsec() << endl;
            cout << "wait loop n = " << i << endl ;
            //end1 = chrono::system_clock::now();
            //cout << "[FFT_CT] [loop] [wait ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end1-start1).count() << endl;
        }*/

    }

    //

    {
        lock_guard<mutex> lock(mtx_thread_);
        //lock_guard<recursive_mutex> lock(mtx_thread_);
        for (int i=0;i<n_thread_;i++) {
            thread_exec_[i] = -1;
        }
    }

    //

    for (auto &&th : ths) {
        try {
            if(th.joinable()) {
                th.join();
            }
        } catch(const std::system_error& e) {
            cerr << "[FFT_CT] Caught system_error with code " << e.code() << " meaning " << e.what() << endl;
            return false;
        }
    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [exec fft] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //
    // Reverse bits
    //

    //start = chrono::system_clock::now();

    complex<double> tmp_swap;

    int i1 = 0;

    for(const auto &i2 : i_bit_rev_) {
        tmp_swap = f_[i1];
        f_[i1] = f_[i2];
        f_[i2] = tmp_swap;
        i1++;
    }

    /*
    uint_fast32_t i2;

    for(uint_fast32_t i1=0;i1<n_;i1++) {
        if(i_bit_rev_[i1] != -1) {
            i2 = i_bit_rev_[i1];
            tmp_swap = f_[i1];
            f_[i1] = f_[i2];
            f_[i2] = tmp_swap;
        }
    }
    */

    /*
    uint_fast32_t i2;

    for(uint_fast32_t i1=0;i1<n_;i1++) {

        i2 = i1;

        i2 = (((i2 & 0xaaaaaaaa) >> 1) | ((i2 & 0x55555555) << 1));
        i2 = (((i2 & 0xcccccccc) >> 2) | ((i2 & 0x33333333) << 2));
        i2 = (((i2 & 0xf0f0f0f0) >> 4) | ((i2 & 0x0f0f0f0f) << 4));
        i2 = (((i2 & 0xff00ff00) >> 8) | ((i2 & 0x00ff00ff) << 8));
        i2 = ((i2 >> 16) | (i2 << 16)) >> (32 - n_step_);

        if(i2 > i1) {
            tmp_swap = f_[i1];
            f_[i1] = f_[i2];
            f_[i2] = tmp_swap;
        }

    }
    */

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [bit rev.] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //start = chrono::system_clock::now();

    f__ = f_;

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [copy val] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    return true;

};

//--------------------------------------------------//

void MyFFT::_calc_butterfly(
    uint_fast32_t thread_no__,
    uint_fast32_t i_begin__,
    uint_fast32_t i_end__
) {

    //chrono::system_clock::time_point start, end;

    uint_fast32_t m1, m2, n_step;

    complex<double> tmp;

    //uint_fast32_t n_block_half = n_block_half_;

    {
        lock_guard<mutex> lock(mtx_thread_);
        //lock_guard<recursive_mutex> lock(mtx_thread_);
        //unique_lock<mutex> lock(mtx_thread_);
        thread_exec_[thread_no__] = 0;
        thread_init_cnt_++;
        //cout << "[_calc_butterfly] init end : " << thread_no__ << endl;
        n_step = n_step_;
    }

    //sleep(1);
    //this_thread::sleep_for(chrono::nanoseconds(1));

    //

    //int thread_exec = 0;

    //uint_fast32_t n_calc;

    uint_fast32_t n_block_half;

    for(uint_fast32_t s=0;s<n_step;s++) {

    //while(thread_exec_[thread_no__] >= 0) {
        //while(1) {

        //this_thread::sleep_for(chrono::nanoseconds(1));

        //cout << "th " << thread_no__ << " sts = " << thread_exec_[thread_no__] << endl;

        /*
        {
            lock_guard<mutex> lock(mtx_thread_);
            //lock_guard<recursive_mutex> lock(mtx_thread_);
            thread_exec = thread_exec_[thread_no__];
        }
        */

        {
            unique_lock<std::mutex> wait_lock(mtx_wait_);

            // データの準備ができるまで待機してから処理する
            cond_.wait(wait_lock, [this] { return thread_ready_; });

        //if(thread_exec_[thread_no__] == 1) {

            //start = chrono::system_clock::now();

            {
                lock_guard<mutex> lock(mtx_thread_);
                thread_exec_[thread_no__] = 0;
                n_block_half = n_block_half_;
                //cout << "[CALC] " << thread_no__ << " : st = " << time_.HMS_nsec() << endl;
            }

            //{
            //    lock_guard<mutex> lock(mtx_thread_);
            //    //lock_guard<recursive_mutex> lock(mtx_thread_);
            //    thread_exec_[thread_no__] = 0;
            //}

            for(uint_fast32_t i=i_begin__;i<i_end__;i++) {
                m1 = m_[s_][i];
                m2 = m1 + n_block_half;
                tmp = f_[m1] - f_[m2];
                f_[m1] += f_[m2];
                f_[m2] = tmp * W_0_[iW_[s_][m1]];
            }

            /*
            while(1) {

                {
                    lock_guard<mutex> lock(mtx_thread_);
                    n_calc = n_calc_;
                    n_calc_++;
                    if(n_calc_ >= n_half_) { break; }
                }

                m1 = m_[s_][n_calc];
                m2 = m1 + n_block_half;
                tmp = f_[m1] - f_[m2];
                f_[m1] += f_[m2];
                f_[m2] = tmp * W_0_[iW_[s_][m1]];

            }
            */

            //end = chrono::system_clock::now();

            {
                lock_guard<mutex> lock(mtx_thread_);
                //lock_guard<recursive_mutex> lock(mtx_thread_);
                //unique_lock<mutex> lock(mtx_thread_);
                thread_cnt_++;
                //cout << "[CALC] " << thread_no__ << " : et = " << time_.HMS_nsec() << endl;
                //cout << "[CALC] " << thread_no__ << " : dt =  " << fixed << setw(15) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;
            }

            //thread_ready_ = false;

        }

        //if(thread_exec < 0) { break; }

        //this_thread::sleep_for(chrono::nanoseconds(100));

    }

    return;

};

/*
void MyFFT::_calc_butterfly(
    uint_fast32_t i_begin__,
    uint_fast32_t i_end__,
    uint_fast32_t &Nb_half__,
    vector<complex<double>> &f__,
    vector<complex<double>> &W__,
    vector<uint_fast32_t> &w__,
    vector<uint_fast32_t> &m__
) {

    uint_fast32_t i1;
    uint_fast32_t i2;
    complex<double> tmp;

    //cout << "[begin, end] = [" << i_begin__ << ", " << i_end__ << "]" << endl;

    if(calc_butterfly_) {

        for(uint_fast32_t i=i_begin__;i<i_end__;i++) {
            i1 = m__[i];
            i2 = i1 + Nb_half__;
            tmp = f__[i1] - f__[i2];
            f__[i1] += f__[i2];
            f__[i2] = tmp * W__[w__[i1]];
        }

    }

    return;

};
*/

//--------------------------------------------------//
///
/// @brief   Implementation of multi-thread FFT with a number of data.
/// @return  None.
///
//--------------------------------------------------//

void MyFFT::_calc_fft(
    const uint_fast32_t &thread_no__,
    vector<vector<complex<double>>> &x__,
    const int &n_set__,
    int &i_set__
) {

    int i_set;
    uint_fast32_t n;
    vector<int> i_bit_rev;

    {
        unique_lock<std::mutex> wait_lock(mtx_wait_);
        i_bit_rev = i_bit_rev_;
        n = n_;
    }

    while(1) {

        {
            unique_lock<std::mutex> wait_lock(mtx_wait_);

            i_set__++;
            i_set = i_set__;

            if(i_set__ >= n_set__) { break; }

            //cout << "[_calc_fft] th.no = " << thread_no__ << ", i_set = " << i_set__;
            //cout << ", size = " << x__.size() << ", " <<  x__[i_set].size() << endl;

        }

        _FFT_CT(x__[i_set], n, i_bit_rev);

        //cout << "[_calc_fft] fin " << endl;

    }

    return;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT with a number of data.
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::FFT(
    const vector<complex<double>> &t__,
    vector<vector<complex<double>>> &f__,
    bool zero_padding__
) {

    int n_set = static_cast<int>(t__.size()/n_);

    if(zero_padding__) { n_set++; }

    vector<vector<complex<double>>> ().swap(f__);
    vector<complex<double>> f_part;

    f__.reserve(n_set);
    f_part.resize(n_);

    uint_fast32_t i1 = 0;

    for(const auto &it : t__) {

        f_part[i1] = it;

        i1++;

        if(i1 >= n_) {
            f__.push_back(f_part);
            i1 = 0;
        }

    }

    //cout << "size = " << f__.size() << endl;
    //cout << "part size =";
    //for(const auto &it:f__) { cout << " " << it.size() << ","; }
    //cout << endl;

    if(zero_padding__) {
        for(uint_fast32_t i2=i1;i2<n_;i2++) {
            f_part[i2] = complex<double> (0.0, 0.0);
        }
        f__.push_back(f_part);
    }

    //

    if(window_func_name_ != "") {
        for(auto &&it : f__) {
            ApplyWindow(it);
        }
    }

    //

    vector<thread> ths( n_thread_ );

    uint_fast32_t thread_no = 0;

    int i_set = -1;

    for (auto &&th : ths) {

        th = thread(
            &MyFFT::_calc_fft,
            this,
            thread_no,
            std::ref(f__),
            std::ref(n_set),
            std::ref(i_set)
        );

        thread_no++;

    }

    for (auto &&th : ths) {
        try {
            if(th.joinable()) {
                th.join();
            }
        } catch(const std::system_error& e) {
            cout << "Caught system_error with code " << e.code() << " meaning " << e.what() << endl;
        }
    }

    return true;

}

//--------------------------------------------------//

bool MyFFT::FFT(
    const vector<double> &t__,
    vector<vector<complex<double>>> &f__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    return FFT(t, f__);

}

//--------------------------------------------------//

/*
vector<vector<complex<double>>> MyFFT::FFT(
    const vector<double> &t__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    vector<vector<complex<double>>> f;

    FFT(t, f);

    return f;

}
*/

//--------------------------------------------------//

bool MyFFT::FFT(
    const vector<complex<double>> &t__,
    vector<complex<double>> &f__,
    bool zero_padding__
) {

    vector<vector<complex<double>>> f;

    FFT(t__, f);

    vector<complex<double>> ().swap(f__);
    f__.resize(n_);
    complex<double> zero(0.0, 0.0);

    for(auto &&it : f__) { it = zero; }

    for(auto &&it1 : f) {

        int i = 0;

        for(auto &&it2 : it1) {

            f__[i] += it2;
            i++;

        }

    }

    return true;

}

//--------------------------------------------------//

bool MyFFT::FFT(
    const vector<double> &t__,
    vector<complex<double>> &f__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    return FFT(t, f__);

}

//--------------------------------------------------//

vector<complex<double>> MyFFT::FFT(
    const vector<double> &t__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    vector<complex<double>> f;

    FFT(t, f);

    return f;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::_FFT_CT(
    vector<complex<double>> &x__,
    uint_fast32_t &n__,
    vector<int> &i_bit_rev__
) {

    double thetaT = PI_ / n__;;
    uint_fast32_t k = n__;

    uint_fast32_t n;
    complex<double> T;
    complex<double> tmp;
    complex<double> phiT = complex<double>(cos(thetaT), -sin(thetaT));

    //

    while(k > 1) {

        n = k;
        k >>= 1;
        phiT = phiT * phiT;
        T = 1.0L;

        for (uint_fast32_t l = 0; l < k; l++) {

            for (uint_fast32_t a = l; a < n__; a += n) {

                uint_fast32_t b = a + k;
                tmp = x__[a] - x__[b];
                x__[a] += x__[b];
                x__[b] = tmp * T;

            }

            T *= phiT;

        }

    }

    //
    // Reverse bits
    //

    complex<double> tmp_swap;

    int i1 = 0;

    for(const auto &i2 : i_bit_rev__) {
        tmp_swap = x__[i1];
        x__[i1] = x__[i2];
        x__[i2] = tmp_swap;
        i1++;
    }

    return true;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT with the specified number of data, sequentialy.
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::FFT_CT_seq(const vector<complex<double>> &t__, vector<complex<double>> &f__) {

    if(static_cast<uint_fast32_t>(t__.size()) != n_) {
        cerr << "[ERROR][MyFFT] The number of input data is not a multiply of 2." << endl;
        return false;
    }

    //

    f__ = t__;

    if(window_func_name_ != "") { ApplyWindow(f__); }

    if(_FFT_CT_seq(f__)) {

        for(auto &&it : f__) {
            it /= n_; // Correcting the amplitude.
        }

        return true;

    } else {

        return false;

    }

}

//--------------------------------------------------//

bool MyFFT::FFT_CT_seq(const vector<double> &t__, vector<complex<double>> &f__) {

    if(static_cast<uint_fast32_t>(t__.size()) != n_) {
        cerr << "[ERROR][MyFFT] The number of input data is not a multiply of 2." << endl;
        return false;
    }

    //

    vector<complex<double>> ().swap(f__);
    f__.reserve(n_);
    for(const auto &it:t__) {
        f__.push_back(complex<double> (it, 0.0));
    }

    if(window_func_name_ != "") { ApplyWindow(f__); }

    if(_FFT_CT_seq(f__)) {

        for(auto &&it : f__) {
            it /= n_; // Correcting the amplitude.
        }

        return true;

    } else {

        return false;

    }

}

//--------------------------------------------------//

vector<complex<double>> MyFFT::FFT_CT_seq(const vector<double> &t__) {

    vector<complex<double>> f;

    if(static_cast<uint_fast32_t>(t__.size()) != n_) {
        cerr << "[ERROR][MyFFT] The number of input data is not a multiply of 2." << endl;
        return f;
    }

    //

    if(FFT_CT_seq(t__, f)) {

        for(auto &&it : f) {
            it /= n_; // Correcting the amplitude.
        }

    } else {

        vector<complex<double>> ().swap(f);

    }

    return f;

}

//--------------------------------------------------//
///
/// @brief   Executing inverted FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::IFFT_CT_seq(const vector<complex<double>> &f__, vector<complex<double>> &t__) {

    if(!Preparing_IFFT(f__, t__)) { return false; }

    //

    if(_FFT_CT_seq(t__)) {

        for(auto &&it : t__) { it = conj(it); }

        return true;

    } else {

        return false;

    }

}

//--------------------------------------------------//

bool MyFFT::IFFT_CT_seq(const vector<complex<double>> &f__, vector<double> &t__) {

    vector<complex<double>> t;

    if(!Preparing_IFFT(f__, t)) { return false; }

    //

    if(_FFT_CT_seq(t)) {

        for(auto &&it : t) { it = conj(it); }

    } else {

        return false;

    }

    //

    vector<double> ().swap(t__);
    t__.reserve(n_);

    for(const auto &it : t) {
        t__.push_back(it.real());
    }

    return true;

}

//--------------------------------------------------//

vector<complex<double>> MyFFT::IFFT_CT_seq(const vector<complex<double>> &f__) {

    vector<complex<double>> t;

    if(!Preparing_IFFT(f__, t)) { return t; }

    //

    if(_FFT_CT_seq(t)) {

        for(auto &&it : t) { it = conj(it); }

    } else {

        vector<complex<double>> ().swap(t);

    }

    return t;

}

//--------------------------------------------------//

bool MyFFT::Preparing_IFFT(const vector<complex<double>> &f__, vector<complex<double>> &t__) {

    if(static_cast<uint_fast32_t>(f__.size()) == n_) {

        t__ = f__;

    }
    else if(static_cast<uint_fast32_t>(f__.size()) == (n_half_ + 1)) {

        vector<complex<double>> ().swap(t__);
        t__.reserve(n_);

        for(const auto &it : f__) {
            t__.push_back(it);
        }

        for(int i=n_half_; i>0; i--) {
            t__.push_back(t__[i]);
        }

    }
    else {

        cerr << "[ERROR][MyFFT] The number of input data is irregular. : " << f__.size() << endl;
        return false;

    }

    //

    for(auto &&it : t__) { it = conj(it); }

    return true;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::_FFT_CT_seq(vector<complex<double>> &x__) {

    uint_fast32_t n;

    uint_fast32_t N;
    double thetaT;

    {
        unique_lock<std::mutex> wait_lock(mtx_wait_);

        N = n_;
        thetaT = PI_ / N;
    }

    uint_fast32_t k = N;

    complex<double> T;
    complex<double> tmp;
    complex<double> phiT = complex<double>(cos(thetaT), -sin(thetaT));

    //

    while(k > 1) {

        n = k;
        k >>= 1;
        phiT = phiT * phiT;
        T = 1.0L;

        for (uint_fast32_t l = 0; l < k; l++) {

            for (uint_fast32_t a = l; a < N; a += n) {

                uint_fast32_t b = a + k;

                tmp = x__[a] - x__[b];
                x__[a] += x__[b];
                x__[b] = tmp * T;

            }

            T *= phiT;

        }

    }

    //
    // Reverse bits
    //

    complex<double> tmp_swap;

    int i1 = 0;

    {
        unique_lock<std::mutex> wait_lock(mtx_wait_);

        for(const auto &i2 : i_bit_rev_) {
            tmp_swap = x__[i1];
            x__[i1] = x__[i2];
            x__[i2] = tmp_swap;
            i1++;
        }
    }

    return true;

}

//--------------------------------------------------//
///
/// @brief     GSts the window function.
/// @param[in] window_func_name__ : name of window function
///            @arg "hanning"  = Hanning(Hann) window function
///            @arg "hamming"  = Hamming window function
///            @arg "blackman" = Blackman window function
///            @arg "flat-top" = Flat top window function
///            @arg "" or others = rectangular window function
/// @return    None.
///
//--------------------------------------------------//

void MyFFT::SetWindow(const string &window_func_name__, const bool &use_acf__) {

    window_func_name_ = window_func_name__;

    use_acf_ = use_acf__;

    //

    vector<double> ().swap(wf_);
    wf_.resize(n_);

    double coeff = 2 * PI_ / n_;

    double sum = 0;

    int i = 0;

    if(window_func_name_ == "hanning") {

        for(auto &&it : wf_) {
            it = 0.5 - 0.5 * cos(coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else if(window_func_name_ == "hamming") {

        for(auto &&it : wf_) {
            it = 0.54 - 0.46 * cos(coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else if(window_func_name_ == "blackman") {

        for(auto &&it : wf_) {
            it = 0.42 - 0.5 * cos(coeff * i) + 0.08 * cos(2 * coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else if(window_func_name_ == "flat-top") {

        for(auto &&it : wf_) {
            it   = 1 - 1.93 * cos(coeff * i);
            it  +=  1.29  * cos(2 * coeff * i);
            it  += -0.388 * cos(3 * coeff * i);
            it  +=  0.032 * cos(4 * coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else {

        for(auto &&it : wf_) { it = 1.0; i++; }
        acf_ = 1.0;

    }

    return;

}

//--------------------------------------------------//
///
/// @brief      Gets the window function.
/// @param[out] wf__ : the window function array
/// @return     None.
///
//--------------------------------------------------//

void MyFFT::GetWindow(std::vector<double> &wf__) {

    wf__ = wf_;

    return;

}

//--------------------------------------------------//
///
/// @brief      Applys the window function to time data.
/// @param[out] w__ : the window function array
/// @return     None.
///
//--------------------------------------------------//

void MyFFT::ApplyWindow(vector<complex<double>> &t__) {

    uint_fast32_t n = t__.size();

    if(n >= n_) {

        if(use_acf_) {
            for(uint_fast32_t i=0;i<n_;i++) { t__[i] = acf_ * wf_[i] * t__[i]; }
        } else {
            for(uint_fast32_t i=0;i<n_;i++) { t__[i] = wf_[i] * t__[i]; }
        }

        if(t__.size() > n_) {
            t__.resize(n_);
        }

    }

    return;

}

/*
//--------------------------------------------------//
///
/// @brief   Flat top window function
/// @return  None.
///
//--------------------------------------------------//

void MyFFT::GetWF_FlatTop(std::vector<double> &w__) {

    vector<double> ().swap(w__);
    w__.resize(n_);

    double coeff = 2 * PI_ / n_;

    int i = 0;

    for(auto &&it : w__) {
        it  = 1 - 1.93 * cos(coeff * i)
        it +=  1.29  * cos(2 * coeff * i)
        it += -0.388 * cos(3 * coeff * i)
        it += +0.032 * cos(4 * coeff * i)
        i++;
    }

    return;

}
*/

//--------------------------------------------------//
///
/// @brief   Converting data.
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::C2RI(const vector<complex<double>> &c__, vector<double> &c_real__, vector<double> &c_imag__) {

    int n = c__.size();

    if(n == 0 || (n%2) != 0) {
        return false;
    }

    vector<double> ().swap(c_real__);
    vector<double> ().swap(c_imag__);

    c_real__.reserve(n);
    c_imag__.reserve(n);

    for(const auto &it : c__) {
        c_real__.push_back(it.real());
        c_imag__.push_back(it.imag());
    }

    return true;

}

//--------------------------------------------------//

bool MyFFT::RI2C(const vector<double> &c_real__, const vector<double> &c_imag__, vector<complex<double>> &c__) {

    int n_real = c_real__.size();
    int n_imag = c_imag__.size();

    if(n_real == 0 || n_real != n_imag) {
        return false;
    }

    vector<complex<double>> ().swap(c__);

    c__.reserve(n_real);

    for(int i=0;i<n_real;i++) {
        c__.push_back(complex<double> (c_real__[i], c_imag__[i]));
    }

    return true;

}

//--------------------------------------------------//

bool MyFFT::C2AP(const vector<complex<double>> &c__, vector<double> &c_amp__, vector<double> &c_phas__) {

    int n = c__.size();
    int n_half = n / 2;

    if(n == 0 || (n%2) != 0) {
        return false;
    }

    vector<double> ().swap(c_amp__);
    vector<double> ().swap(c_phas__);

    c_amp__.reserve(n_half + 1);
    c_phas__.reserve(n_half + 1);

    for(int i=0;i<=n_half;i++) {
        c_amp__.push_back(abs(c__[i]));
        c_phas__.push_back(arg(c__[i]));
    }

    for(int i=0;i<n_half;i++) {
        c_amp__[n_half-i] += abs(c__[n_half+i]);
    }

    return true;

}

//--------------------------------------------------//

bool MyFFT::AP2C(const vector<double> &c_amp__, const vector<double> &c_phas__, vector<complex<double>> &c__) {

    int n_amp  = c_amp__.size();
    int n_phas = c_phas__.size();

    if(n_amp == 0 || n_amp != n_phas) {
        return false;
    }

    int n = (n_amp -1) * 2;

    vector<complex<double>> ().swap(c__);

    c__.reserve(n);

    c__.push_back(complex<double> (c_amp__[0]*cos(c_phas__[0]), c_amp__[0]*sin(c_phas__[0])));

    for(int i=1;i<n_amp;i++) {
        c__.push_back(complex<double> (0.5 * c_amp__[i]*cos(c_phas__[i]), 0.5 * c_amp__[i]*sin(c_phas__[i])));
    }

    //cout << "00" << endl;

    for(int i=n_amp-2;i>0;i--) {
        c__.push_back(conj(c__[i]));
    }

    //cout << "01 : " << c__.size() << endl;

    return true;

}

//--------------------------------------------------//

};