/**
 * @file
 * This file is part of SeisSol.
 *
 * @author Carsten Uphoff (c.uphoff AT tum.de, http://www5.in.tum.de/wiki/index.php/Carsten_Uphoff,_M.Sc.)
 * @author Sebastian Wolf (wolf.sebastian AT in.tum.de, https://www5.in.tum.de/wiki/index.php/Sebastian_Wolf,_M.Sc.)
 *
 * @section LICENSE
 * Copyright (c) 2017 - 2020, SeisSol Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @section DESCRIPTION
 *
 **/
#include <Eigen/Eigenvalues>
#include <Kernels/precision.hpp>
#include <Initializer/typedefs.hpp>

#include <PUML/PUML.h>
#include <PUML/Downward.h>
#include <PUML/Upward.h>
#include "LtsWeights.h"

#include <Eigen/Dense>

#include <Initializer/ParameterDB.h>
#include <Parallel/MPI.h>

#include <generated_code/tensor.h>
#include <generated_code/init.h>

class FaceSorter {
private:
	std::vector<PUML::TETPUML::face_t> const& m_faces;

public:
	FaceSorter(std::vector<PUML::TETPUML::face_t> const& faces) : m_faces(faces) {}

	bool operator()(unsigned int a, unsigned int b) const {
		return m_faces[a].gid() < m_faces[b].gid();
	}
};

void seissol::initializers::time_stepping::LtsWeights::computeMaxTimesteps( PUML::TETPUML const&  mesh,
                                                                            std::vector<double> const& pWaveVel,
                                                                            std::vector<double>& timestep ) {
  std::vector<PUML::TETPUML::cell_t> const& cells = mesh.cells();
  std::vector<PUML::TETPUML::vertex_t> const& vertices = mesh.vertices();

  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    // Compute insphere radius
    Eigen::Vector3d barycentre(0.,0.,0.);
    Eigen::Vector3d x[4];
    unsigned vertLids[4];
    PUML::Downward::vertices(mesh, cells[cell], vertLids);
    for (unsigned vtx = 0; vtx < 4; ++vtx) {
      for (unsigned d = 0; d < 3; ++d) {
        x[vtx](d) = vertices[ vertLids[vtx] ].coordinate()[d];
      }
    }
    Eigen::Matrix4d A;
    A << x[0](0), x[0](1), x[0](2), 1.0,
         x[1](0), x[1](1), x[1](2), 1.0,
         x[2](0), x[2](1), x[2](2), 1.0,
         x[3](0), x[3](1), x[3](2), 1.0;

    double alpha = A.determinant();
    double Nabc = ( (x[1]-x[0]).cross(x[2]-x[0]) ).norm();
    double Nabd = ( (x[1]-x[0]).cross(x[3]-x[0]) ).norm();
    double Nacd = ( (x[2]-x[0]).cross(x[3]-x[0]) ).norm();
    double Nbcd = ( (x[2]-x[1]).cross(x[3]-x[1]) ).norm();
    double insphere = std::fabs(alpha) / (Nabc + Nabd + Nacd + Nbcd);

    // Compute maximum timestep (CFL=1)
    timestep[cell] = 2.0 * insphere / (pWaveVel[cell] * (2*CONVERGENCE_ORDER-1));
  }
}

int seissol::initializers::time_stepping::LtsWeights::getCluster( double    timestep,
                                                                  double    globalMinTimestep,
                                                                  unsigned  rate ) {
  if (rate == 1) {
    return 0;
  }

  double upper;
  upper = rate * globalMinTimestep;

  int cluster = 0;
  while (upper <= timestep) {
    upper *= rate;
    ++cluster;
  }
  return cluster;
}

int seissol::initializers::time_stepping::LtsWeights::getBoundaryCondition( int const* boundaryCond,
                                                                            unsigned cell,
                                                                            unsigned face ) {
  int bcCurrentFace = ((boundaryCond[cell] >> (face*8)) & 0xFF);
  if (bcCurrentFace > 64) {
     bcCurrentFace = 3;
  }
  return bcCurrentFace;
}

int seissol::initializers::time_stepping::LtsWeights::ipow(int x, int y) {
  assert(y >= 0);

  if (y == 0) {
    return 1;
  }
  int result = x;
  while(--y) {
    result *= x;
  }
  return result;
}

void seissol::initializers::time_stepping::LtsWeights::computeWeights(PUML::TETPUML const& mesh) {
  logInfo(seissol::MPI::mpi.rank()) << "Computing LTS weights.";

  std::vector<PUML::TETPUML::cell_t> const& cells = mesh.cells();
  int const* boundaryCond = mesh.cellData(1);

  std::vector<double> pWaveVel;
  pWaveVel.resize(cells.size());

  seissol::initializers::ElementBarycentreGeneratorPUML queryGen(mesh);
  //up to now we only distinguish between anisotropic elastic any other isotropic material
#ifdef USE_ANISOTROPIC
  std::vector<seissol::model::AnisotropicMaterial> materials(cells.size());
  seissol::initializers::MaterialParameterDB<seissol::model::AnisotropicMaterial> parameterDB;
#else
  std::vector<seissol::model::ElasticMaterial> materials(cells.size());
  seissol::initializers::MaterialParameterDB<seissol::model::ElasticMaterial> parameterDB;
#endif
  parameterDB.setMaterialVector(&materials);
  parameterDB.evaluateModel(m_velocityModel, queryGen);
  for(unsigned cell = 0; cell < cells.size(); ++cell) {
    pWaveVel[cell] = materials[cell].getMaxWaveSpeed();
  }
  std::vector<double> timestep;
  timestep.resize(cells.size());
  computeMaxTimesteps(mesh, pWaveVel, timestep);

  double localMinTimestep = *std::min_element(timestep.begin(), timestep.end());
  double localMaxTimestep = *std::max_element(timestep.begin(), timestep.end());
  double globalMinTimestep;
  double globalMaxTimestep;
#ifdef USE_MPI
  MPI_Allreduce(&localMinTimestep, &globalMinTimestep, 1, MPI_DOUBLE, MPI_MIN, seissol::MPI::mpi.comm());
  MPI_Allreduce(&localMaxTimestep, &globalMaxTimestep, 1, MPI_DOUBLE, MPI_MAX, seissol::MPI::mpi.comm());
#else
  globalMinTimestep = localMinTimestep;
  globalMaxTimestep = localMaxTimestep;
#endif

  int* cluster = new int[cells.size()];
  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    cluster[cell] = getCluster(timestep[cell], globalMinTimestep, m_rate);
  }

  int totalNumberOfReductions = enforceMaximumDifference(mesh, cluster);

  delete[] m_vertexWeights;
  //m_ncon = 2;
  m_ncon = 1;
  m_vertexWeights = new int[cells.size() * m_ncon];
  int maxCluster = getCluster(globalMaxTimestep, globalMinTimestep, m_rate);
  int drToCellRatio = 1;
  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    int dynamicRupture = 0;
    for (unsigned face = 0; face < 4; ++face) {
      dynamicRupture += ( getBoundaryCondition(boundaryCond, cell, face) == 3) ? 1 : 0;
    }

    m_vertexWeights[m_ncon * cell] = (1 + drToCellRatio*dynamicRupture) * ipow(m_rate, maxCluster - cluster[cell]);
    //m_vertexWeights[m_ncon * cell + 1] = (dynamicRupture > 0) ? 1 : 0;
  }

  delete[] cluster;

  logInfo(seissol::MPI::mpi.rank()) << "Computing LTS weights. Done. " << utils::nospace << '(' << totalNumberOfReductions << " reductions.)";
}

int seissol::initializers::time_stepping::LtsWeights::enforceMaximumDifference(PUML::TETPUML const& mesh, int* cluster) {
  int totalNumberOfReductions = 0;
  int globalNumberOfReductions;
  do {
    int localNumberOfReductions = enforceMaximumDifferenceLocal(mesh, cluster);

#ifdef USE_MPI
    MPI_Allreduce(&localNumberOfReductions, &globalNumberOfReductions, 1, MPI_INT, MPI_SUM, seissol::MPI::mpi.comm());
#else
    globalNumberOfReductions = localNumberOfReductions;
#endif // USE_MPI
    totalNumberOfReductions += globalNumberOfReductions;
  } while (globalNumberOfReductions > 0);
  return totalNumberOfReductions;
}

int seissol::initializers::time_stepping::LtsWeights::enforceMaximumDifferenceLocal(PUML::TETPUML const& mesh, int* cluster, int maxDifference) {
  int numberOfReductions = 0;

  std::vector<PUML::TETPUML::cell_t> const& cells = mesh.cells();
  std::vector<PUML::TETPUML::face_t> const& faces = mesh.faces();
	int const* boundaryCond = mesh.cellData(1);

#ifdef USE_MPI
  std::unordered_map<int, std::vector<int>> rankToSharedFaces;
  std::unordered_map<int, int> localFaceIdToLocalCellId;
#endif // USE_MPI

  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    int timeCluster = cluster[cell];

		unsigned int faceids[4];
		PUML::Downward::faces(mesh, cells[cell], faceids);
    for (unsigned f = 0; f < 4; ++f) {
      int difference = maxDifference;
      int boundary = getBoundaryCondition(boundaryCond, cell, f);
      // Continue for regular, dynamic rupture, and periodic boundary cells
      if (boundary == 0 || boundary == 3 || boundary == 6) {
        // We treat MPI neighbours later
        auto const& face = faces[ faceids[f] ];
        if (!face.isShared()) {
          int cellIds[2];
          PUML::Upward::cells(mesh, face, cellIds);

          int neighbourCell = (cellIds[0] == static_cast<int>(cell)) ? cellIds[1] : cellIds[0];
          int otherTimeCluster = cluster[neighbourCell];

          if (boundary == 3) {
            difference = 0;
          }

          if (timeCluster > otherTimeCluster + difference) {
            timeCluster = otherTimeCluster + difference;
            ++numberOfReductions;
          }
        }
#ifdef USE_MPI
        else {
          rankToSharedFaces[ face.shared()[0] ].push_back(faceids[f]);
          localFaceIdToLocalCellId[ faceids[f] ] = cell;
        }
#endif // USE_MPI
      }
    }
    cluster[cell] = timeCluster;
  }

#ifdef USE_MPI
  FaceSorter faceSorter(faces);
  for (auto& sharedFaces: rankToSharedFaces) {
    std::sort(sharedFaces.second.begin(), sharedFaces.second.end(), faceSorter);
  }

  auto numExchanges = rankToSharedFaces.size();
  MPI_Request* requests = new MPI_Request[2*numExchanges];
  int** ghost = new int*[numExchanges];
  int** copy = new int*[numExchanges];
  auto exchange = rankToSharedFaces.begin();
  for (unsigned ex = 0; ex < numExchanges; ++ex) {
    auto exchangeSize = exchange->second.size();
    ghost[ex] = new int[ exchangeSize ];
    copy[ex]  = new int[ exchangeSize ];

    for (unsigned n = 0; n < exchangeSize; ++n) {
      copy[ex][n] = cluster[ localFaceIdToLocalCellId[ exchange->second[n] ] ];
    }
    MPI_Isend( copy[ex], exchangeSize, MPI_INT, exchange->first, 0, seissol::MPI::mpi.comm(), &requests[ex]);
    MPI_Irecv(ghost[ex], exchangeSize, MPI_INT, exchange->first, 0, seissol::MPI::mpi.comm(), &requests[numExchanges + ex]);
    ++exchange;
  }

  MPI_Waitall(2*numExchanges, requests, MPI_STATUSES_IGNORE);

  exchange = rankToSharedFaces.begin();
  for (unsigned ex = 0; ex < numExchanges; ++ex) {
    auto exchangeSize = exchange->second.size();
    for (unsigned n = 0; n < exchangeSize; ++n) {
      int difference = maxDifference;
      int otherTimeCluster = ghost[ex][n];

      int cellIds[2];
      PUML::Upward::cells(mesh, faces[ exchange->second[n] ], cellIds);
      int cell = (cellIds[0] >= 0) ? cellIds[0] : cellIds[1];

      unsigned int faceids[4];
      PUML::Downward::faces(mesh, cells[cell], faceids);
      unsigned f = 0;
      for (; f < 4 && static_cast<int>(faceids[f]) != exchange->second[n]; ++f);
      assert(f != 4);

      int boundary = getBoundaryCondition(boundaryCond, cell, f);
      if (boundary == 3) {
        difference = 0;
      }

      if (cluster[cell] > otherTimeCluster + difference) {
        cluster[cell] = otherTimeCluster + difference;
        ++numberOfReductions;
      }
    }
    ++exchange;
  }

  for (unsigned ex = 0; ex < numExchanges; ++ex) {
    delete[] copy[ex];
    delete[] ghost[ex];
  }
  delete[] copy;
  delete[] ghost;
  delete[] requests;
#endif // USE_MPI

  return numberOfReductions;
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_FFT_FFTWBACKEND_HPP
  #define BOOST_MATH_FFT_FFTWBACKEND_HPP

  #include <memory>

  #if defined(__GNUC__)
  #include <fftw3.h>
  #endif
  #include <boost/math/fft/dft_api.hpp>
  #include <boost/math/fft/multiprecision_complex.hpp>

  namespace boost { namespace math {  namespace fft {

  namespace detail {

  #if defined(__GNUC__)
  template<typename T>
  struct fftw_traits_c_interface;

  template<>
  struct fftw_traits_c_interface<float>
  {
    using plan_type = fftwf_plan;

    using real_value_type = float;

    using complex_value_type = real_value_type[2U];

    static plan_type plan_construct(
      int n, complex_value_type* in, complex_value_type* out, int sign, unsigned int flags)
    {
      return ::fftwf_plan_dft_1d(n, in, out, sign, flags);
    }
    static plan_type plan_construct_r2c(
      int n, real_value_type* in, complex_value_type* out, unsigned int flags)
    {
      return ::fftwf_plan_dft_r2c_1d(n, in, out, flags);
    }
    static plan_type plan_construct_c2r(
      int n, complex_value_type* in, real_value_type* out, unsigned int flags)
    {
      return ::fftwf_plan_dft_c2r_1d(n, in, out, flags);
    }
    static plan_type plan_construct_r2r(
      int n, real_value_type* in, real_value_type* out, fftw_r2r_kind kind, unsigned int flags)
    {
      return ::fftwf_plan_r2r_1d(n, in, out, kind, flags);
    }

    static void plan_execute(
      plan_type plan, complex_value_type* in, complex_value_type* out)
    {
      ::fftwf_execute_dft(plan, in, out);
    }
    static void plan_execute_r2c(
      plan_type plan, real_value_type* in, complex_value_type* out)
    {
      ::fftwf_execute_dft_r2c(plan, in, out);
    }
    static void plan_execute_c2r(
      plan_type plan, complex_value_type* in, real_value_type* out)
    {
      ::fftwf_execute_dft_c2r(plan, in, out);
    }
    static void plan_execute_r2r(
      plan_type plan, real_value_type* in, real_value_type* out)
    {
      ::fftwf_execute_r2r(plan, in, out);
    }

    static void plan_destroy(plan_type p) { ::fftwf_destroy_plan(p); }

    static int alignment_of(real_value_type* p) { return ::fftwf_alignment_of(p); }
  };

  template<>
  struct fftw_traits_c_interface<double>
  {
    using plan_type = fftw_plan;

    using real_value_type = double;

    using complex_value_type = real_value_type[2U];

    static plan_type plan_construct(
      int n, complex_value_type* in, complex_value_type* out, int sign, unsigned int flags)
    {
      return ::fftw_plan_dft_1d(n, in, out, sign, flags);
    }
    static plan_type plan_construct_r2c(
      int n, real_value_type* in, complex_value_type* out, unsigned int flags)
    {
      return ::fftw_plan_dft_r2c_1d(n, in, out, flags);
    }
    static plan_type plan_construct_c2r(
      int n, complex_value_type* in, real_value_type* out, unsigned int flags)
    {
      return ::fftw_plan_dft_c2r_1d(n, in, out, flags);
    }
    static plan_type plan_construct_r2r(
      int n, real_value_type* in, real_value_type* out, fftw_r2r_kind kind, unsigned int flags)
    {
      return ::fftw_plan_r2r_1d(n, in, out, kind, flags);
    }

    static void plan_execute(
      plan_type plan, complex_value_type* in, complex_value_type* out)
    {
      ::fftw_execute_dft(plan, in, out);
    }
    static void plan_execute_r2c(
      plan_type plan, real_value_type* in, complex_value_type* out)
    {
      ::fftw_execute_dft_r2c(plan, in, out);
    }
    static void plan_execute_c2r(
      plan_type plan, complex_value_type* in, real_value_type* out)
    {
      ::fftw_execute_dft_c2r(plan, in, out);
    }
    static void plan_execute_r2r(
      plan_type plan, real_value_type* in, real_value_type* out)
    {
      ::fftw_execute_r2r(plan, in, out);
    }

    static void plan_destroy(plan_type p) { ::fftw_destroy_plan(p); }

    static int alignment_of(real_value_type* p) { return ::fftw_alignment_of(p); }
  };

  template<>
  struct fftw_traits_c_interface<long double>
  {
    using plan_type = fftwl_plan;

    using real_value_type = long double;

    using complex_value_type = real_value_type[2U];

    static plan_type plan_construct(
      int n, complex_value_type* in, complex_value_type* out, int sign, unsigned int flags)
    {
      return ::fftwl_plan_dft_1d(n, in, out, sign, flags);
    }
    static plan_type plan_construct_r2c(
      int n, real_value_type* in, complex_value_type* out, unsigned int flags)
    {
      return ::fftwl_plan_dft_r2c_1d(n, in, out, flags);
    }
    static plan_type plan_construct_c2r(
      int n, complex_value_type* in, real_value_type* out, unsigned int flags)
    {
      return ::fftwl_plan_dft_c2r_1d(n, in, out, flags);
    }

    static plan_type plan_construct_r2r(
      int n, real_value_type* in, real_value_type* out, fftw_r2r_kind kind, unsigned int flags)
    {
      return ::fftwl_plan_r2r_1d(n, in, out, kind, flags);
    }

    static void plan_execute(
      plan_type plan, complex_value_type* in, complex_value_type* out)
    {
      ::fftwl_execute_dft(plan, in, out);
    }
    static void plan_execute_r2c(
      plan_type plan, real_value_type* in, complex_value_type* out)
    {
      ::fftwl_execute_dft_r2c(plan, in, out);
    }
    static void plan_execute_c2r(
      plan_type plan, complex_value_type* in, real_value_type* out)
    {
      ::fftwl_execute_dft_c2r(plan, in, out);
    }
    static void plan_execute_r2r(
      plan_type plan, real_value_type* in, real_value_type* out)
    {
      ::fftwl_execute_r2r(plan, in, out);
    }

    static void plan_destroy(plan_type p) { ::fftwl_destroy_plan(p); }

    static int alignment_of(real_value_type* p) { return ::fftwl_alignment_of(p); }
  };
  #endif

  #ifdef BOOST_MATH_USE_FLOAT128
  template<>
  struct fftw_traits_c_interface<boost::multiprecision::float128>
  {
    using plan_type = fftwq_plan;

    // Type casting for fftw:
    using real_value_type = boost::float128_t;

    using complex_value_type = boost::multiprecision::complex128;

    static plan_type plan_construct(
      int n, complex_value_type* in, complex_value_type* out, int sign, unsigned int flags)
    {
      return ::fftwq_plan_dft_1d(n, (real_value_type(*)[2])in, (real_value_type(*)[2])out, sign, flags);
    }
    static plan_type plan_construct_r2c(
      int n, real_value_type* in, complex_value_type* out, unsigned int flags)
    {
      return ::fftwq_plan_dft_r2c_1d(n, in, (real_value_type(*)[2])out, flags);
    }
    static plan_type plan_construct_c2r(
      int n, complex_value_type* in, real_value_type* out, unsigned int flags)
    {
      return ::fftwq_plan_dft_c2r_1d(n, (real_value_type(*)[2])in, out, flags);
    }

    static plan_type plan_construct_r2r(
      int n, real_value_type* in, real_value_type* out, fftw_r2r_kind kind, unsigned int flags)
    {
      return ::fftwq_plan_r2r_1d(n, in, out, kind, flags);
    }

    static void plan_execute(
      plan_type plan, complex_value_type* in, complex_value_type* out)
    {
      ::fftwq_execute_dft(plan, (real_value_type(*)[2])in, (real_value_type(*)[2])out);
    }
    static void plan_execute_r2c(
      plan_type plan, real_value_type* in, complex_value_type* out)
    {
      ::fftwq_execute_dft_r2c(plan, in, (real_value_type(*)[2]) out);
    }
    static void plan_execute_c2r(
      plan_type plan, complex_value_type* in, real_value_type* out)
    {
      ::fftwq_execute_dft_c2r(plan,(real_value_type(*)[2]) in, out);
    }
    static void plan_execute_r2r(
      plan_type plan, real_value_type* in, real_value_type* out)
    {
      ::fftwq_execute_r2r(plan, in, out);
    }

    static void plan_destroy(plan_type p) { ::fftwq_destroy_plan(p); }

    static int alignment_of(real_value_type* p) { return ::fftwq_alignment_of(p); }
  };
  #endif

  #if defined(__GNUC__)
  template<class NativeComplexType, class Allocator_t >
  class fftw_backend
  {
  public:
    using value_type     = NativeComplexType;
    using allocator_type = Allocator_t;

  private:
    using real_value_type    = typename NativeComplexType::value_type;
    using plan_type          = typename detail::fftw_traits_c_interface<real_value_type>::plan_type;
    using complex_value_type = boost::multiprecision::complex<real_value_type>;
    using fftw_real_value_type = typename detail::fftw_traits_c_interface<real_value_type>::real_value_type;

    void execute(plan_type plan, plan_type unaligned_plan, const complex_value_type* in, complex_value_type* out) const
    {
      using local_complex_type = typename detail::fftw_traits_c_interface<real_value_type>::complex_value_type;

      if(in!=out) // We have to copy, because fftw plan is forced to be in-place: from: nullptr, to: nullptr
        std::copy(in,in+size(),out);

      const int out_alignment = detail::fftw_traits_c_interface<real_value_type>::alignment_of(
                reinterpret_cast<fftw_real_value_type*>(out));

      if(out_alignment==ref_alignment)
        detail::fftw_traits_c_interface<real_value_type>::plan_execute
        (
          plan,
          reinterpret_cast<local_complex_type*>(out),
          reinterpret_cast<local_complex_type*>(out)
        );
      else
        detail::fftw_traits_c_interface<real_value_type>::plan_execute
        (
          unaligned_plan,
          reinterpret_cast<local_complex_type*>(out),
          reinterpret_cast<local_complex_type*>(out)
        );
    }

    void free()
    {
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_forward_plan);
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_backward_plan);
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_forward_unaligned_plan);
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_backward_unaligned_plan);
    }
    void alloc()
    {
      my_forward_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct
        (
          size(),
          nullptr,
          nullptr,
          FFTW_FORWARD,
          FFTW_ESTIMATE | FFTW_PRESERVE_INPUT
        );
      my_backward_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct
        (
          size(),
          nullptr,
          nullptr,
          FFTW_BACKWARD,
          FFTW_ESTIMATE | FFTW_PRESERVE_INPUT
        );
      my_forward_unaligned_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct
        (
          size(),
          nullptr,
          nullptr,
          FFTW_FORWARD,
          FFTW_ESTIMATE | FFTW_PRESERVE_INPUT | FFTW_UNALIGNED
        );
      my_backward_unaligned_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct
        (
          size(),
          nullptr,
          nullptr,
          FFTW_BACKWARD,
          FFTW_ESTIMATE | FFTW_PRESERVE_INPUT | FFTW_UNALIGNED
        );
    }

  public:
    fftw_backend(std::size_t n, const allocator_type& = allocator_type{} )
      : my_size{ n },
        ref_alignment{
            detail::fftw_traits_c_interface<real_value_type>::alignment_of(nullptr)}
    {
      // For C++11, this line needs to be constexpr-ified.
      // Then we could restore the constexpr-ness of this constructor.
      alloc();
    }

    ~fftw_backend()
    {
      free();
    }

    void resize(std::size_t new_size)
    {
      if(size()!=new_size)
      {
        free();
        my_size = new_size;
        alloc();
      }
    }

    constexpr std::size_t size() const { return my_size; }

    void forward(const complex_value_type* in, complex_value_type* out) const
    {
      execute(my_forward_plan, my_forward_unaligned_plan, in, out);
    }

    void backward(const complex_value_type* in, complex_value_type* out) const
    {
      execute(my_backward_plan, my_backward_unaligned_plan, in, out);
    }
  private:
    std::size_t my_size;
    const int   ref_alignment;

    plan_type   my_forward_plan;
    plan_type   my_backward_plan;
    plan_type   my_forward_unaligned_plan;
    plan_type   my_backward_unaligned_plan;
  };
  #endif

  #if defined(__GNUC__)
  template<class T, class Allocator_t >
  class fftw_rfft_backend
  {
  public:
    // using value_type     = T;
    using allocator_type = Allocator_t;

  private:
    using real_value_type    = T;
    using plan_type          = typename detail::fftw_traits_c_interface<real_value_type>::plan_type;
    using fftw_real_value_type     = typename detail::fftw_traits_c_interface<real_value_type>::real_value_type;
    using fftw_complex_value_type  = typename detail::fftw_traits_c_interface<real_value_type>::complex_value_type;

    template<class U>
    using vector_t = std::vector<U, typename std::allocator_traits<allocator_type>::template rebind_alloc<U> >;

    void execute(plan_type plan, plan_type unaligned_plan, const real_value_type* in, real_value_type* out) const
    // precondition:
    // size(in)  >= size()
    // size(out) >= size()
    {
      const int out_alignment = detail::fftw_traits_c_interface<real_value_type>::alignment_of(
                reinterpret_cast<fftw_real_value_type*>(out));

      if(in!=out) // We have to copy, because fftw plan is forced to be in-place: from: nullptr, to: nullptr
        std::copy(in,in+size(),out);

      if(out_alignment==ref_alignment)
        detail::fftw_traits_c_interface<real_value_type>::plan_execute_r2r
        (
          plan,
          reinterpret_cast<fftw_real_value_type*>(out),
          reinterpret_cast<fftw_real_value_type*>(out)
        );
      else
        detail::fftw_traits_c_interface<real_value_type>::plan_execute_r2r
        (
          unaligned_plan,
          reinterpret_cast<fftw_real_value_type*>(out),
          reinterpret_cast<fftw_real_value_type*>(out)
        );
    }

    void free()
    {
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_r2hc_plan);
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_hc2r_plan);
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_r2hc_unaligned_plan);
      detail::fftw_traits_c_interface<real_value_type>::plan_destroy(my_hc2r_unaligned_plan);
    }
    void alloc()
    {
      my_r2hc_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct_r2r
        (
          size(),
          nullptr,
          nullptr,
          FFTW_R2HC,
          FFTW_ESTIMATE
        );
      my_hc2r_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct_r2r
        (
          size(),
          nullptr,
          nullptr,
          FFTW_HC2R,
          FFTW_ESTIMATE
        );
      my_r2hc_unaligned_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct_r2r
        (
          size(),
          nullptr,
          nullptr,
          FFTW_R2HC,
          FFTW_ESTIMATE | FFTW_UNALIGNED
        );
      my_hc2r_unaligned_plan =
        detail::fftw_traits_c_interface<real_value_type>::plan_construct_r2r
        (
          size(),
          nullptr,
          nullptr,
          FFTW_HC2R,
          FFTW_ESTIMATE | FFTW_UNALIGNED
        );
    }
    void pack_halfcomplex(real_value_type* out) const
    // precondition:
    // -> size(out) >= N
    {
      const std::size_t N = size();
      for(unsigned int i=1,j=N-1;i<j;++i,--j)
        out[j] = -out[j];
    }
    void unpack_halfcomplex(real_value_type* out) const
    // precondition:
    // -> size(out) >= N
    {
      pack_halfcomplex(out);
    }

  public:
    fftw_rfft_backend(std::size_t n, const allocator_type& A = allocator_type{} )
      : my_size{ n },
        ref_alignment{
            detail::fftw_traits_c_interface<real_value_type>::alignment_of(nullptr)},
        my_allocator{A}
    {
      // For C++11, this line needs to be constexpr-ified.
      // Then we could restore the constexpr-ness of this constructor.
      alloc();
    }

    ~fftw_rfft_backend()
    {
      free();
    }

    void resize(std::size_t new_size)
    {
      if(size()!=new_size)
      {
        free();
        my_size = new_size;
        alloc();
      }
    }

    constexpr std::size_t size() const { return my_size; }
    constexpr std::size_t unique_complex_size() const {return my_size/2 + 1;}

    void real_to_halfcomplex(const real_value_type* in, real_value_type* out) const
    {
      execute(my_r2hc_plan,my_r2hc_unaligned_plan,in,out);
      pack_halfcomplex(out);
    }
    void halfcomplex_to_real(const real_value_type* in, real_value_type* out) const
    {
      std::copy(in,in+size(),out);
      unpack_halfcomplex(out);
      execute(my_hc2r_plan,my_hc2r_unaligned_plan,out,out);
    }

  private:
    std::size_t my_size;
    const int   ref_alignment;
    allocator_type my_allocator;

    plan_type   my_r2hc_plan;
    plan_type   my_hc2r_plan;
    plan_type   my_r2hc_unaligned_plan;
    plan_type   my_hc2r_unaligned_plan;
  };
  #endif

  } // namespace detail

  #if defined(__GNUC__)
  template<class RingType = std::complex<double>, class Allocator_t = std::allocator<RingType> >
  using fftw_dft = detail::complex_dft<detail::fftw_backend,RingType,Allocator_t>;

  template<class T = double, class Allocator_t = std::allocator<T> >
  using fftw_rdft = detail::real_dft<detail::fftw_rfft_backend,T,Allocator_t>;

  using fftw_transform = transform< fftw_dft<> >;
  using fftw_real_transform = transform< fftw_rdft<> >;
  #endif

  } } } // namespace boost::math::fft

#endif // BOOST_MATH_FFT_FFTWBACKEND_HPP


/*
 * MathUtils.cpp
 *
 * Author:
 *       Oleg Kalashev
 *
 * Copyright (c) 2020 Institute for Nuclear Research, RAS
 *
 * 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.
 */

#ifdef USE_BOOST

#include <boost/numeric/odeint/config.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>

#endif

#include "Utils.h"
#include "MathUtils.h"
#include <gsl/gsl_errno.h>
#include "gsl/gsl_sf_erf.h"
#include <gsl/gsl_math.h>
#include "TableFunction.h"
#include "nr/odeint.h"
#include "nr/stepperdopr5.h"

namespace Utils {
#ifdef USE_BOOST
	using namespace boost::numeric::odeint;

	template< class Obj , class Mem >
	class ode_wrapper
	{
		Obj* m_pObj;
		Mem m_mem;

	public:

		ode_wrapper( Obj* obj , Mem mem ) : m_pObj(obj ) , m_mem(mem ) { }

		template< class State , class Deriv , class Time >
		void operator()( const State &x , Deriv &dxdt , Time t )
		{
			((*m_pObj).*m_mem)(x , dxdt , t );
		}
	};

	template< class Obj , class Mem >
	ode_wrapper< Obj , Mem > make_ode_wrapper( Obj* obj , Mem mem )
	{
		return ode_wrapper< Obj , Mem >( obj , mem );
	}

	template< class Obj , class Mem >
	class observer_wrapper
	{
		Obj* m_pObj;
		Mem m_mem;

	public:

		observer_wrapper( Obj* obj , Mem mem ) : m_pObj(obj ) , m_mem(mem ) { }

		template< class State , class Time >
		void operator()( const State &x , Time t )
		{
			((*m_pObj).*m_mem)(x , t );
		}
	};

	template< class Obj , class Mem >
	observer_wrapper< Obj , Mem > make_observer_wrapper( Obj* obj , Mem mem )
	{
		return observer_wrapper< Obj , Mem >( obj , mem );
	}

	template< class X=double >
	class Sampler4 : public ISampler<X> {
		//typedef runge_kutta_dopri5<X> dopri5_type;
		//typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
		//typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;
		//typedef bulirsch_stoer_dense_out<X> dopri5_type;
	public:
		Sampler4(){
			fIntermediateVals=new std::vector<X>;
			fIntermediateTs=new std::vector<X>;
			fIntermediateTs->reserve(256);
			fIntermediateVals->reserve(256);
		}

		~Sampler4(){
			delete fIntermediateVals;
			delete fIntermediateTs;
		}

		void System(const X &x , X &dxdt , X t ){
			dxdt=fFunc->f(t);
		}

		void Output(const X &x , X t ){
			if(fMaxX>0 && x>=fMaxX){
				X lastT = *(fIntermediateTs->end()-1);
				X lastX = *(fIntermediateVals->end()-1);
				if((t-lastT)/(t+lastT)<fRelErr && t-lastT<fAbsErr){
					X result = lastT + (t-lastT)/(x-lastX)*(fMaxX-lastX);
					throw result;
				}
				X totRate = x-lastX;
				throw sample(*fFunc, lastT, t, (fMaxX-lastX)/(x-lastX), totRate, fRelErr, fAbsErr);
			}
			else {
				fIntermediateVals->push_back(x);
				fIntermediateTs->push_back(t);
			}
		}

		/// If total rate is known it should be passed via aTotRate argument
		/// Otherwize aTotRate should be set to 0 (it will be calculated by function)
		X sample(const FunctionX<X>& f, X aTmin, X aTmax, X aRand,
				 X & aTotRate, X aRelErr, X aAbsErr = 1e300){
			fMaxX = aTotRate*aRand;
			fRelErr = aRelErr;
			fAbsErr = aAbsErr;

			size_t nPoints = (size_t)((aTmax-aTmin)/aRelErr+0.5)/2+1;
			X step= (aTmax-aTmin)/nPoints;

			fIntermediateTs->resize(0);//this will not decrease capacity of vectors
			fIntermediateVals->resize(0);

			X curT=aTmin;
			fFunc = &f;
			aTotRate = 0.;

			X dt = (aTmax - aTmin) * aRelErr;

			try {
				//dense_output_dopri5_type dopri5 = make_dense_output( aAbsErr , aRelErr , dopri5_type() );
				bulirsch_stoer_dense_out< X > stepper( aAbsErr , aRelErr);
				integrate_adaptive( stepper , make_ode_wrapper(this,&Sampler4::System) , aTotRate , aTmin , aTmax , dt , make_observer_wrapper(this, &Sampler4::Output) );
				//integrate_const
				//integrate_adaptive(dopri5, make_ode_wrapper(this,&Sampler3::System), aTotRate, aTmin, aTmax, dt,
								   //make_observer_wrapper(this, &Sampler3::Output));
			}catch (X aValue){
				return aValue;
			}
			if(aTotRate<=0){
				return aTmax;
			}
			std::vector<X>& intermediateVals = *fIntermediateVals;
			std::vector<X>& intermediateTs = *fIntermediateTs;
			X searchVal = aTotRate * aRand;
			size_t i2= intermediateVals.size();
			size_t i1=0;
			for(size_t i=(i2+i1)/2; i2-i1>1; i=(i2+i1)/2)
			{
				if(intermediateVals[i] < searchVal)
					i1 = i;
				else
					i2 = i;
			}
			X t1 = intermediateTs[i1];
			X t2 = intermediateTs[i2];
			X x1 = intermediateVals[i1];
			X x2 = intermediateVals[i2];
			if((t2-t1)/(t2+t1)<fRelErr && t2-t1<fAbsErr){
				return t1 + (t2-t1)/(x2-x1)*(searchVal-x1);
			}
			X totRate=x2-x1;
			return sample(*fFunc, t1, t2, (searchVal-x1)/(x2-x1), totRate, fRelErr, fAbsErr);
		}
	private:
		const FunctionX<X>* fFunc;
		std::vector<X>* fIntermediateVals;
		std::vector<X>* fIntermediateTs;
		X      fMaxX;
		X      fRelErr;
		X      fAbsErr;
	};

	template< class X=double >
	class Sampler3 : public ISampler<X> {
		typedef runge_kutta_dopri5<X> dopri5_type;
		typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
		typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;
	public:
		Sampler3(){
			fIntermediateVals=new std::vector<X>;
			fIntermediateTs=new std::vector<X>;
			fIntermediateTs->reserve(256);
			fIntermediateVals->reserve(256);
		}

		~Sampler3(){
			delete fIntermediateVals;
			delete fIntermediateTs;
		}

		void System(const X &x , X &dxdt , X t ){
			dxdt=fFunc->f(t);
		}

		void Output(const X &x , X t ){
			if(fMaxX>0 && x>=fMaxX){
				X lastT = *(fIntermediateTs->end()-1);
				X lastX = *(fIntermediateVals->end()-1);
				if((t-lastT)/(t+lastT)<fRelErr && t-lastT<fAbsErr){
					X result = lastT + (t-lastT)/(x-lastX)*(fMaxX-lastX);
					throw result;
				}
				X totRate = x-lastX;
				throw sample(*fFunc, lastT, t, (fMaxX-lastX)/(x-lastX), totRate, fRelErr, fAbsErr);
			}
			else {
				fIntermediateVals->push_back(x);
				fIntermediateTs->push_back(t);
			}
		}

		/// If total rate is known it should be passed via aTotRate argument
		/// Otherwize aTotRate should be set to 0 (it will be calculated by function)
		X sample(const FunctionX<X>& f, X aTmin, X aTmax, X aRand,
				 X & aTotRate, X aRelErr, X aAbsErr = 1e300){
			fMaxX = aTotRate*aRand;
			fRelErr = aRelErr;
			fAbsErr = aAbsErr;

			size_t nPoints = (size_t)((aTmax-aTmin)/aRelErr+0.5)/2+1;
			X step= (aTmax-aTmin)/nPoints;

			fIntermediateTs->resize(0);//this will not decrease capacity of vectors
			fIntermediateVals->resize(0);

			X curT=aTmin;
			fFunc = &f;
			aTotRate = 0.;
			dense_output_dopri5_type dopri5 = make_dense_output( aAbsErr , aRelErr , dopri5_type() );
			X dt = (aTmax - aTmin) * aRelErr;

			try {
				integrate_adaptive(dopri5, make_ode_wrapper(this,&Sampler3::System), aTotRate, aTmin, aTmax, dt,
								   make_observer_wrapper(this, &Sampler3::Output));
			}catch (X aValue){
				return aValue;
			}
			if(aTotRate<=0){
				return aTmax;
			}
			std::vector<X>& intermediateVals = *fIntermediateVals;
			std::vector<X>& intermediateTs = *fIntermediateTs;
			X searchVal = aTotRate * aRand;
			size_t i2= intermediateVals.size();
			size_t i1=0;
			for(size_t i=(i2+i1)/2; i2-i1>1; i=(i2+i1)/2)
			{
				if(intermediateVals[i] < searchVal)
					i1 = i;
				else
					i2 = i;
			}
			X t1 = intermediateTs[i1];
			X t2 = intermediateTs[i2];
			X x1 = intermediateVals[i1];
			X x2 = intermediateVals[i2];
			if((t2-t1)/(t2+t1)<fRelErr && t2-t1<fAbsErr){
				return t1 + (t2-t1)/(x2-x1)*(searchVal-x1);
			}
			X totRate=x2-x1;
			return sample(*fFunc, t1, t2, (searchVal-x1)/(x2-x1), totRate, fRelErr, fAbsErr);
		}
	private:
		const FunctionX<X>* fFunc;
		std::vector<X>* fIntermediateVals;
		std::vector<X>* fIntermediateTs;
		X      fMaxX;
		X      fRelErr;
		X      fAbsErr;
	};

	template<typename X = double >
	class Sampler : public ISampler<X> {
		typedef runge_kutta_dopri5<X> dopri5_type;
		typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
		typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;
	public:
		Sampler(){
			fIntermediateVals=0;
			fIntermediateTs=0;
		}
		void operator()(const X &x , X &dxdt , const X t ){
			dxdt=fFunc->f(t);
		}
		void operator()(const X &x , const X t ){
			(*fIntermediateVals)[fLastStep++]=x;
			double a=(*fIntermediateVals)[fLastStep-1];
			a=a+1;
		}

		X sample(const FunctionX<X>& f, X aTmin, X aTmax, X aRand,
				 X & aTotRate, X aRelErr, X aAbsErr = 1e300){
			// create a vector with observation time points
			size_t nPoints = (size_t)((aTmax-aTmin)/aRelErr/2+0.5);
			X step= (aTmax-aTmin)/nPoints;
			std::vector<X> intermediateVals(nPoints + 1);
			std::vector<X> intermediateTs(nPoints + 1);
			fIntermediateVals=&intermediateVals;
			fIntermediateTs=&intermediateTs;
			X curT=aTmin;
			for( size_t i=0 ; i<=nPoints ; ++i, curT+=step )
				intermediateTs[i] = curT;

			fFunc = &f;
			aTotRate = 0.;
			dense_output_dopri5_type dopri5 = make_dense_output( aAbsErr , aRelErr , dopri5_type() );
			X dt = (aTmax - aTmin) * aRelErr;

			fLastStep=0;
			integrate_times(dopri5 , (*this) , aTotRate , intermediateTs, dt , (*this) );
			if(aTotRate==0.){
				//aTotRate=0.;
				return aTmax;
			}

			X searchVal = aTotRate * aRand;
			size_t i2= intermediateVals.size();
			size_t i1=0;
			for(size_t i=(i2+i1)/2; i2-i1>1; i=(i2+i1)/2)
			{
				if(intermediateVals[i] < searchVal)
					i1 = i;
				else
					i2 = i;
			}
			X t1 = intermediateTs[i1];
			X t2 = intermediateTs[i2];
			X x1 = intermediateVals[i1];
			X x2 = intermediateVals[i2];
			return t1 + (t2 - t1) / (x2 - x1) * (searchVal - x1);
		}

		const FunctionX<X>* fFunc;
		std::vector<X>* fIntermediateVals;
		std::vector<X>* fIntermediateTs;

		size_t      fLastStep;
	};

	template<typename X = double >
	class LogSampler : public ISampler<X> {
		typedef runge_kutta_dopri5<X> dopri5_type;
		typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
		typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;
	public:
		LogSampler(){
			fIntermediateVals=0;
			fIntermediateTs=0;
		}
		void operator()(const X &x , X &dxdt , const X t ){
			dxdt=fFunc->f(t);
		}
		void operator()(const X &x , const X t ){
			(*fIntermediateVals)[fLastStep++]=x;
			double a=(*fIntermediateVals)[fLastStep-1];
			a=a+1;
		}

		X sample(const FunctionX<X>& f, X aTmin, X aTmax, X aRand,
				 X & aTotRate, X aRelErr, X aAbsErr = 1e300){
			// create a vector with observation time points
			size_t nPoints = (size_t)(log(aTmax/aTmin)/log(1.0+aRelErr)/2+0.5);
			X step= pow(aTmax/aTmin, 1./nPoints);
			std::vector<X> intermediateVals(nPoints + 1);
			std::vector<X> intermediateTs(nPoints + 1);
			fIntermediateVals=&intermediateVals;
			fIntermediateTs=&intermediateTs;
			X curT=aTmin;
			for( size_t i=0 ; i<=nPoints ; ++i, curT*=step )
				intermediateTs[i] = curT;

			fFunc = &f;
			aTotRate = 0.;
			dense_output_dopri5_type dopri5 = make_dense_output( aAbsErr , aRelErr , dopri5_type() );
			X dt = (aTmax - aTmin) * aRelErr;

			fLastStep=0;
			integrate_times(dopri5 , (*this) , aTotRate , intermediateTs, dt , (*this) );
			if(aTotRate==0.){
				//aTotRate=0.;
				return aTmax;
			}

			X searchVal = aTotRate * aRand;
			size_t i2= intermediateVals.size();
			size_t i1=0;
			for(size_t i=(i2+i1)/2; i2-i1>1; i=(i2+i1)/2)
			{
				if(intermediateVals[i] < searchVal)
					i1 = i;
				else
					i2 = i;
			}
			X t1 = intermediateTs[i1];
			X t2 = intermediateTs[i2];
			X x1 = intermediateVals[i1];
			X x2 = intermediateVals[i2];
			return t1 + (t2 - t1) / (x2 - x1) * (searchVal - x1);
		}

		const FunctionX<X>* fFunc;
		std::vector<X>* fIntermediateVals;
		std::vector<X>* fIntermediateTs;

		size_t      fLastStep;
	};

	template<typename X = double >
	class Sampler2 : public ISampler<X> {
		typedef runge_kutta_dopri5<X> dopri5_type;
		typedef controlled_runge_kutta< dopri5_type > controlled_dopri5_type;
		typedef dense_output_runge_kutta< controlled_dopri5_type > dense_output_dopri5_type;
	public:
		Sampler2(){
			fIntermediateVals=0;
			fIntermediateTs=0;
		}
		~Sampler2()
		{
			fIntermediateVals=0;
			fIntermediateTs=0;
		}
		void operator()(const X &x , X &dxdt , const double t ){
			dxdt=fFunc->f(t);
		}
		void operator()(const X &x , const X t ){
			if(fMaxX>0 && x>=fMaxX){
				X lastT = *(fIntermediateTs->end()-1);
				X lastX = *(fIntermediateVals->end()-1);
				if((t-lastT)/(t+lastT)<fRelErr && t-lastT<fAbsErr){
					X result = lastT + (t-lastT)/(x-lastX)*(fMaxX-lastX);
					throw result;
				}
				X totRate = x-lastX;
				throw sample(*fFunc, lastT, t, (fMaxX-lastX)/(x-lastX), totRate, fRelErr, fAbsErr);
			}
			else {
				fIntermediateVals->push_back(x);
				fIntermediateTs->push_back(t);
			}
		}

		/// If total rate is known it should be passed via aTotRate argument
		/// Otherwize aTotRate should be set to 0 (it will be calculated by function)
		X sample(const FunctionX<X>& f, X aTmin, X aTmax, X aRand,
				 X & aTotRate, X aRelErr, X aAbsErr = 1e300){
			fMaxX = aTotRate*aRand;
			//fTmin = aTmin;
			//fTmax = aTmax;
			//fTotRate = aTotRate;
			fRelErr = aRelErr;
			fAbsErr = aAbsErr;

			size_t nPoints = 128;//(size_t)((aTmax-aTmin)/aRelErr+0.5)/2+1;
			//X step= (aTmax-aTmin)/nPoints;
			std::vector<X> intermediateVals;
			std::vector<X> intermediateTs;
			intermediateTs.reserve(nPoints);
			intermediateVals.reserve(nPoints);
			fIntermediateVals=&intermediateVals;
			fIntermediateTs=&intermediateTs;
			X curT=aTmin;
			fFunc = &f;
			aTotRate = 0.;

			///TODO: try different steppers

			dense_output_dopri5_type dopri5 = make_dense_output( aAbsErr , aRelErr , dopri5_type() );
			X dt = (aTmax - aTmin) * aRelErr;

			try {
				integrate_adaptive(dopri5, (*this), aTotRate, aTmin, aTmax, dt,
								   (*this));//this will create two copies of (*this)
				/* //this one is a bit slower than dense_output_dopri5_type
				typedef runge_kutta_cash_karp54< X > error_stepper_type;
				integrate_adaptive( make_controlled< error_stepper_type >( aAbsErr , aRelErr ) ,
									(*this) , aTotRate , aTmin, aTmax, dt,
									(*this));*/

			}catch (X aValue){
				return aValue;
			}
			if(aTotRate<=0){
				return aTmax;
			}

			X searchVal = aTotRate * aRand;
			size_t i2= intermediateVals.size();
			size_t i1=0;
			for(size_t i=(i2+i1)/2; i2-i1>1; i=(i2+i1)/2)
			{
				if(intermediateVals[i] < searchVal)
					i1 = i;
				else
					i2 = i;
			}
			X t1 = intermediateTs[i1];
			X t2 = intermediateTs[i2];
			X x1 = intermediateVals[i1];
			X x2 = intermediateVals[i2];
			if((t2-t1)/(t2+t1)<fRelErr && t2-t1<fAbsErr){
				return t1 + (t2-t1)/(x2-x1)*(searchVal-x1);
			}
			X totRate=x2-x1;
			return sample(*fFunc, t1, t2, (searchVal-x1)/(x2-x1), totRate, fRelErr, fAbsErr);
		}

		const FunctionX<X>* fFunc;
		std::vector<X>* fIntermediateVals;
		std::vector<X>* fIntermediateTs;
		X      fMaxX;
		X      fRelErr;
		X      fAbsErr;
	};

#endif //#ifdef USE_BOOST

	template<typename X = double >
	class NRSampler : public ISampler<X> {
		X sample(const FunctionX<X>& f, X aTmin, X aTmax, X aRand,
				 X & aTotRate, X aRelErr, X aAbsErr = 1e300){
			X x;
			MathUtils::SampleLogDistributionNR(f,aRand,x,aTotRate,aTmin,aTmax,aRelErr);
		}
	};

	double MathUtils::SolveEquation(
			gsl_function F,	double x_lo, double x_hi,
			const double relError, const int max_iter,
			const gsl_root_fsolver_type *T)
{
    double r = 0;
	int status;
	int iter = 0;

	gsl_root_fsolver *solver = gsl_root_fsolver_alloc (T);
	gsl_root_fsolver_set (solver, &F, x_lo, x_hi);
	do
	{
	 	iter++;
	    status = gsl_root_fsolver_iterate (solver);
	    r = gsl_root_fsolver_root (solver);
	    x_lo = gsl_root_fsolver_x_lower (solver);
	    x_hi = gsl_root_fsolver_x_upper (solver);
	    status = gsl_root_test_interval (x_lo, x_hi, 0, relError);
	}
	while (status == GSL_CONTINUE && iter < max_iter);
	ASSERT(status == GSL_SUCCESS);
	gsl_root_fsolver_free (solver);
	if (status != GSL_SUCCESS)
	{
		if(iter >= max_iter)
			Exception::Throw("Failed to solve equation: maximal number of iterations achieved");
	  	else
	  		Exception::Throw("Failed to solve equation: GSL status " + ToString(status));
	}
	return r;
}

double MathUtils::SolveEquation(
		double (*aEquation) (double, void*),
		double x_lo, double x_hi, void* aEquationPars,
		const double relError, const int max_iter,
		const gsl_root_fsolver_type *T)
{
	gsl_function F;
	F.function = aEquation;
	F.params = aEquationPars;
	return SolveEquation(F,	x_lo, x_hi, relError, max_iter, T);
}

MathUtils::MathUtils():
gslQAGintegrator(0)
{

}

MathUtils::~MathUtils()
{
	if(gslQAGintegrator)
		gsl_integration_workspace_free (gslQAGintegrator);
}

	template<class X> class GaussDisr : public FunctionX<X>{
	public:
		X mean;
		X sigma;
		GaussDisr(X aMean, X aSigma):mean(aMean),sigma(aSigma){}
		virtual X f(X _x) const{
			X diff = (_x-mean)/sigma;
			return exp(-diff*diff*0.5)/sigma*0.398942280401433;
		}
	};

	template<class X> void ISampler<X>::UnitTest()
	{

		X mean = 1e10;
		X sigma = 1e9;
		X minX=1e9;
		X maxX=1e11;
		X minI = 0.5*(1+gsl_sf_erf((minX-mean)/sigma/sqrt(2.0)));
		X maxI = 0.5*(1+gsl_sf_erf((maxX-mean)/sigma/sqrt(2.0)));
		X totI=maxI-minI;

		GaussDisr<X> gd(mean,sigma);
		X relError = 1e-6;
		int nSteps = 10000;
//gnuplot command: 0.5*(1+erf((x-mean)/sigma/sqrt(2.0))) w l

		for(int i=1; i<nSteps; i++){
			X rand = 1./nSteps*i;
			X totRate = 0;
			X curX = sample(gd, minX, maxX, rand, totRate, relError);
			X exactFrac = (0.5*(1+gsl_sf_erf((curX-mean)/sigma/sqrt(2.0)))-minI)/totI;
			std::cout << curX << "\t" << rand << "\t"  << exactFrac << std::endl;
		}
	}

	template void ISampler<double>::UnitTest();
	template void ISampler<long double>::UnitTest();

	template<typename X> bool MathUtils::SampleLogscaleDistribution(const Function& aDistrib, double aRand, X& aOutputX, X& aOutputIntegral, int nStepsS, X xMin, X xMax, double aRelError)
{
	std::vector<X> sArray,ratesArray;
	double distrXmin = aDistrib.Xmin();
	if(xMin < distrXmin)
		xMin = distrXmin;
	double distrXmax = aDistrib.Xmax();
	if(xMax > distrXmax)
		xMax = distrXmax;
	ASSERT(xMin>0. && xMin<xMax);
	double stepS = pow(xMax/xMin,1./nStepsS);

	sArray.push_back(0);
	ratesArray.push_back(0);

	X taleAccLimit;
	RelAccuracy<X>(taleAccLimit);
	taleAccLimit*=10;
	int maxIntervals = (int)(0.1/aRelError + 10.5);
	aOutputIntegral=0.;
	double deltaLogS = log(stepS);
	double s=xMin;
	for(int iS=1; iS<=nStepsS; iS++)
	{
		double s2=s*stepS;
		X rate = Integration_qag(aDistrib,s,s2,1e-300,aRelError,maxIntervals);
		ASSERT_VALID_NO(rate);
		if(rate==0. && aOutputIntegral==0.)
		{//move Smax
			sArray[0] = deltaLogS*iS;
		}
		else
		{
			if(rate==0 || (aOutputIntegral>0 && rate/aOutputIntegral < taleAccLimit))
				rate = aOutputIntegral*taleAccLimit;//avoiding zero derivative (inverse function must be defined everywhere)
			aOutputIntegral += rate;
			sArray.push_back(deltaLogS*iS);
			ratesArray.push_back(aOutputIntegral);
		}
		s=s2;
	}
	if(aOutputIntegral>0)
	{//sampling s
		ASSERT(sArray.size()>1);
		double randRate = aRand*aOutputIntegral;
		for(int i = ratesArray.size()-1;i>=0; i--)
			ratesArray[i]-=randRate;
		GSLTableFunc func(sArray, ratesArray, 0, 0, gsl_interp_cspline);
		func.SetAutoLimits();
		double logError = aRelError/(nStepsS*deltaLogS);
		if(logError>aRelError)
			logError = aRelError;
		aOutputX = SolveEquation(func, 0, nStepsS*deltaLogS, logError);
		aOutputX = xMin*exp(aOutputX);
		ASSERT(aOutputX>=xMin && aOutputX<=xMax);
		return true;
	}
	return false;
}

class MathUtilsODE
{
public:
	MathUtilsODE(const Function& aDistrib):fDistrib(aDistrib)
	{}
	void operator() (const nr::Doub x, nr::VecDoub_I &y, nr::VecDoub_O &dydx) {
		double val = fDistrib(x);
		if(val!=0)
			dydx[0]= val;
		else
			dydx[0]= 0.;
		ASSERT(val > -1.6e308 && val < 1.6e308);
	}
private:
	const Function& fDistrib;
};

IFunctionCallHandlerX<double>* MathUtils::fLogger = 0;

bool MathUtils::SampleDistribution(const Function& aDistrib, double aRand, double& aOutputX, double& aOutputIntegral, double xMin, double xMax, double aRelError)
{
	const Function* distrib = &aDistrib;
	SafePtr<DebugFunctionX<double> > func;
	if(fLogger)
	{
		func = new DebugFunctionX<double>(aDistrib, *fLogger);
		distrib = func;
	}

	double distrXmin = distrib->Xmin();
	if(xMin < distrXmin)
		xMin = distrXmin;
	double distrXmax = distrib->Xmax();
	if(xMax > distrXmax)
		xMax = distrXmax;
	ASSERT(xMax>xMin);

	double initialStep = aRelError*(xMax-xMin);

	MathUtilsODE d(*distrib);

	const nr::Doub atol=0.;//1.0e-3;
	const nr::Doub hmin=0.0;//minimal step (can be zero)
	nr::VecDoub ystart(1);
	ystart[0]=0.;
	nr::Output out(-1); //output is saved at every integration step
	nr::Odeint<nr::StepperDopr5<MathUtilsODE> > ode(ystart,xMin,xMax,atol,aRelError,initialStep,hmin,out,d);
	ode.integrate();
	aOutputIntegral = ystart[0];
	if(aOutputIntegral<=0)
		return false;
	aRand *= aOutputIntegral;
	int i1=0;
	int i2=out.count-1;
	double* ysave = out.ysave[0];
	for(int i=(i1+i2)/2; i2-i1>1; i=(i1+i2)/2)
	{
		double y = ysave[i];
		if(y<aRand)
			i1 = i;
		else
			i2 = i;
	}
	double y1=ysave[i1];
	double y2=ysave[i2];
	double x1=out.xsave[i1];
	double x2=out.xsave[i2];
	aRand -= y1;
	aOutputX = x1 + (x2-x1)/(y2-y1)*aRand;//make a linear estimate of X
	if(fabs((x2-x1)/aOutputX)<=aRelError)
		return true;
	ystart[0]=0.;
	initialStep = aRelError*(aOutputX>0 ? aOutputX : (x2-x1));
	nr::Output out2(2+(int)fabs((x2-x1)/initialStep));

	nr::Odeint<nr::StepperDopr5<MathUtilsODE> > ode2(ystart,x1,x2,atol,aRelError,initialStep,hmin,out2,d);
	ode2.integrate();
	i1=0;
	i2=out2.count-1;
	ysave = out2.ysave[0];
	for(int i=(i1+i2)/2; i2-i1>1; i=(i1+i2)/2)
	{
		double y = ysave[i];
		if(y<aRand)
			i1 = i;
		else
			i2 = i;
	}
	y1=ysave[i1];
	y2=ysave[i2];
	x1=out2.xsave[i1];
	x2=out2.xsave[i2];
	aOutputX = x1 + (x2-x1)/(y2-y1)*(aRand-y1);//make a linear estimate of X
	return true;
}

class MathUtilsLogODE
{
public:
	MathUtilsLogODE(const Function& aDistrib):fDistrib(aDistrib)
	{}
	void operator() (const nr::Doub x, nr::VecDoub_I &y, nr::VecDoub_O &dydx) {
		double xx = exp(x);
		double val = xx*fDistrib(xx);
		if(val!=0)
			dydx[0]= val;
		else
			dydx[0]= 0.;
		ASSERT(val > -1.6e308 && val < 1.6e308);
	}
private:
	const Function& fDistrib;
};

bool MathUtils::SampleLogDistribution(const Function& aDistrib, double aRand, double& aOutputX, double& aOutputIntegral, double xMin, double xMax, double aRelError)
{
#ifdef USE_BOOST
    return SampleLogDistributionBoost(aDistrib, aRand, aOutputX, aOutputIntegral, xMin, xMax, aRelError);
#else
	return SampleLogDistributionNR(aDistrib, aRand, aOutputX, aOutputIntegral, xMin, xMax, aRelError);
#endif
}

bool MathUtils::SampleLogDistributionNR(const Function& aDistrib, double aRand, double& aOutputX, double& aOutputIntegral, double xMin, double xMax, double aRelError)
{
	ASSERT(aRelError>0 && aRelError<=0.1);

	const Function* distrib = &aDistrib;
	SafePtr<DebugFunctionX<double> > func;
	if(fLogger)
	{
		func = new DebugFunctionX<double>(aDistrib, *fLogger);
		distrib = func;
	}

	double distrXmin = distrib->Xmin();
	if(xMin < distrXmin)
		xMin = distrXmin;
	double distrXmax = distrib->Xmax();
	if(xMax > distrXmax)
		xMax = distrXmax;
	ASSERT(xMax>xMin && xMin>0);
	xMin=log(xMin);
	xMax=log(xMax);

	double initialStep = 0.5*(xMax-xMin);
//	if(aRelError<initialStep)
//		initialStep=aRelError;

	MathUtilsLogODE d(*distrib);

	const nr::Doub atol=0;
	const nr::Doub hmin=0.0;//minimal step (can be zero)
	nr::VecDoub ystart(1);
	ystart[0]=0.;
	nr::Output out(-1); //output is saved at every integration step
	nr::Odeint<nr::StepperDopr5<MathUtilsLogODE> > ode(ystart,xMin,xMax,atol,aRelError,initialStep,hmin,out,d);
	ode.integrate();
	aOutputIntegral = ystart[0];
	if(aOutputIntegral<=0)
		return false;
	double yRand = aRand*aOutputIntegral;
	int i1=0;
	int i2=out.count-1;
	double* ysave = out.ysave[0];
	for(int i=(i1+i2)/2; i2-i1>1; i=(i1+i2)/2)
	{
		double y = ysave[i];
		if(y<yRand)
			i1 = i;
		else
			i2 = i;
	}
	double y1=ysave[i1];
	double y2=ysave[i2];
	double x1=out.xsave[i1];
	double x2=out.xsave[i2];
	double yFrac = (yRand-y1)/(y2-y1);
	if((x2-x1)<aRelError)
	{
		aOutputX = x1 + (x2-x1)*yFrac;//make a linear estimate of X in log scale
		ASSERT(aOutputX>=xMin && aOutputX<=xMax);
	}
	else
	{
		ystart[0]=0.;
		initialStep = 0.5*(x2-x1);//aRelError;
		nr::Output out2(2+(int)fabs((x2-x1)/initialStep));
		nr::Odeint<nr::StepperDopr5<MathUtilsLogODE> > ode2(ystart,x1,x2,atol,aRelError,initialStep,hmin,out2,d);
		ode2.integrate();
		yRand = ystart[0]*yFrac;
		i1=0;
		i2=out2.count-1;
		ysave = out2.ysave[0];
		for(int i=(i1+i2)/2; i2-i1>1; i=(i1+i2)/2)
		{
			double y = ysave[i];
			if(y<yRand)
				i1 = i;
			else
				i2 = i;
		}
		y1=ysave[i1];
		y2=ysave[i2];
		x1=out2.xsave[i1];
		x2=out2.xsave[i2];
		aOutputX = x1 + (x2-x1)/(y2-y1)*(yRand-y1);//make a linear estimate of X in log scale
		ASSERT(aOutputX>=xMin && aOutputX<=xMax);
	}
	aOutputX = exp(aOutputX);
	ASSERT_VALID_NO(aOutputX);
	return true;
}

bool MathUtils::SampleLogDistributionBoost(const Function& aDistrib, double aRand, double& aOutputX, double& aOutputIntegral, double xMin, double xMax, double aRelError)
	{
#ifdef USE_BOOST
		ASSERT(aRelError>0 && aRelError<=0.1);
		ASSERT(xMin>0 && xMax>xMin);
		LogSampler<double> dil;//slower 25 sec
		//Sampler2<double> dil;// 20 sec (todo: fix memory leaks)
		//Sampler3<double> dil;// 20 sec (todo: fix memory leaks)
		aOutputIntegral=0.;

		aOutputX=dil.sample(aDistrib, xMin, xMax, aRand, aOutputIntegral, aRelError);

		ASSERT_VALID_NO(aOutputX);
		return true;
#else
	Exception::Throw("MathUtils::SampleLogDistributionBoost boostlib support is disabled");
	return false;//avoid compiler warning
#endif
	}

template<typename X> void MathUtils::RelAccuracy(X& aOutput)
{
	NOT_IMPLEMENTED
}

template<> void MathUtils::RelAccuracy<double>(double& aOutput)
{
	aOutput = 1e-15;
}

template<> void MathUtils::RelAccuracy<long double>(long double& aOutput)
{
	aOutput = 1e-18L;
}

template bool MathUtils::SampleLogscaleDistribution<double>(const Function& aDistrib, double aRand, double& aOutputX, double& aOutputIntegral, int nStepsS, double xMin, double xMax, double aRelError);
//template bool MathUtils::SampleLogscaleDistribution<long double>(const Function& aDistrib, double aRand, long double& aOutput, int nStepsS, long double xMin, long double xMax, double aRelError);

	int MathUtils::UnitTest(){
		//LogSampler<double> dil;//slower 25 sec
		//Sampler2<double> dil;// 20 sec (todo: fix memory leaks)
		//Sampler3<double> dil;// 20 sec (todo: fix memory leaks)
		//Sampler4<double> dil;
		NRSampler<double> dil;
		dil.UnitTest();
	}

double MathUtils::Integration_qag (
		gsl_function aFunction,
		double aXmin,
		double aXmax,
		double epsabs,
		double epsrel,
		size_t limit,
		int key)
{
	if(gslQAGintegrator==0)
		gslQAGintegrator = gsl_integration_workspace_alloc (limit);
	else if(gslQAGintegrator->limit < limit)
	{
		gsl_integration_workspace_free (gslQAGintegrator);
		gslQAGintegrator = gsl_integration_workspace_alloc (limit);
	}
	double result, abserr;
	try{
		if(epsabs==0)
			epsabs = std::numeric_limits<double>::min();
		int failed = gsl_integration_qag (&aFunction, aXmin, aXmax, epsabs, epsrel, limit, key, gslQAGintegrator, &result, &abserr);
		if(failed)
		{
			ASSERT(0);
			Exception::Throw("Integration failed with code " + ToString(failed));
		}
	}catch(Exception* ex)
	{
#ifdef _DEBUG
		GslProxyFunction f(aFunction,aXmin,aXmax);
		std::cerr << "\n\n#Integration_qag debug output:" << std::endl;
		bool logscale = aXmin>0 && aXmax/aXmin > 100;
		f.Print(std::cerr, 100, logscale, aXmin, aXmax);
		std::cerr << "\n\n#end of Integration_qag debug output" << std::endl;
#endif
		throw ex;
	}
	return result;
}

} /* namespace Utils */


#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "../../BenchTimer.h"
using namespace Eigen;

#ifndef SCALAR
#error SCALAR must be defined
#endif

typedef SCALAR Scalar;

typedef Matrix<Scalar,Dynamic,Dynamic> Mat;

EIGEN_DONT_INLINE
void gemm(const Mat &A, const Mat &B, Mat &C)
{
  C.noalias() += A * B;
}

EIGEN_DONT_INLINE
double bench(long m, long n, long k)
{
  Mat A(m,k);
  Mat B(k,n);
  Mat C(m,n);
  A.setRandom();
  B.setRandom();
  C.setZero();

  BenchTimer t;

  double up = 1e8*4/sizeof(Scalar);
  double tm0 = 4, tm1 = 10;
  if(NumTraits<Scalar>::IsComplex)
  {
    up /= 4;
    tm0 = 2;
    tm1 = 4;
  }

  double flops = 2. * m * n * k;
  long rep = std::max(1., std::min(100., up/flops) );
  long tries = std::max(tm0, std::min(tm1, up/flops) );

  BENCH(t, tries, rep, gemm(A,B,C));

  return 1e-9 * rep * flops / t.best();
}

int main(int argc, char **argv)
{
  std::vector<double> results;

  std::ifstream settings("gemm_settings.txt");
  long m, n, k;
  while(settings >> m >> n >> k)
  {
    //std::cerr << "  Testing " << m << " " << n << " " << k << std::endl;
    results.push_back( bench(m, n, k) );
  }

  std::cout << RowVectorXd::Map(results.data(), results.size());

  return 0;
}


#define BOOST_TEST_MODULE "test_omp_system_motion_remover"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/SystemMotionRemover.hpp>
#include <mjolnir/omp/SystemMotionRemover.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(SystemMotionRemover_both)
{
    mjolnir::LoggerManager::set_default_logger("test_omp_system_motion_remover.log");

    using traits_type   = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type     = traits_type::real_type;
    using coord_type    = traits_type::coordinate_type;
    using boundary_type = traits_type::boundary_type;
    using system_type   = mjolnir::System<traits_type>;

    constexpr real_type tol = 1e-8;
    constexpr std::size_t N = 100;

    system_type sys(N, boundary_type{});
    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(0.0, 1.0);

    for(std::size_t n=0; n<100; ++n)
    {
        real_type E_kinetic_pre = 0.0;
        auto CoM = mjolnir::math::make_coordinate<coord_type>(0,0,0);
        for(std::size_t i=0; i<N; ++i)
        {
            sys.mass(i)     = 1.0 + uni(mt);
            sys.rmass(i)    = 1.0 / sys.mass(i);
            sys.position(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.velocity(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.force(i)    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

            sys.name(0)  = "X";
            sys.group(0) = "X";

            CoM += sys.mass(i) * sys.position(i);
            E_kinetic_pre += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));
        }
        const system_type sys_pre(sys);

        mjolnir::SystemMotionRemover<traits_type> remover(true, true, true);

        remover.remove(sys);

        auto translation = mjolnir::math::make_coordinate<coord_type>(0,0,0);
        auto rotation    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

        real_type E_kinetic_post = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            const auto  m = sys.mass(i);
            const auto  r = sys.position(i) - CoM;
            const auto& v = sys.velocity(i);

            translation += m * v;
            rotation    += m * mjolnir::math::cross_product(r, v);

            E_kinetic_post += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));

            BOOST_TEST(mjolnir::math::X(sys.position(i)) == mjolnir::math::X(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Y(sys.position(i)) == mjolnir::math::Y(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Z(sys.position(i)) == mjolnir::math::Z(sys_pre.position(i)));
        }

        BOOST_TEST(mjolnir::math::X(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(translation) == 0.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(mjolnir::math::X(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(rotation) == 0.0, boost::test_tools::tolerance(tol));

        // rescaled.
        BOOST_TEST(E_kinetic_pre == E_kinetic_post, boost::test_tools::tolerance(tol));
    }

    for(std::size_t n=0; n<100; ++n)
    {
        real_type E_kinetic_pre = 0.0;
        auto CoM = mjolnir::math::make_coordinate<coord_type>(0,0,0);
        for(std::size_t i=0; i<N; ++i)
        {
            sys.mass(i)     = 1.0 + uni(mt);
            sys.rmass(i)    = 1.0 / sys.mass(i);
            sys.position(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.velocity(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.force(i)    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

            sys.name(0)  = "X";
            sys.group(0) = "X";

            CoM += sys.mass(i) * sys.position(i);
            E_kinetic_pre += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));
        }

        const system_type sys_pre(sys);

        mjolnir::SystemMotionRemover<traits_type> remover(true, true, false);

        remover.remove(sys);

        auto translation = mjolnir::math::make_coordinate<coord_type>(0,0,0);
        auto rotation    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

        real_type E_kinetic_post = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            const auto  m = sys.mass(i);
            const auto  r = sys.position(i) - CoM;
            const auto& v = sys.velocity(i);

            translation += m * v;
            rotation    += m * mjolnir::math::cross_product(r, v);

            E_kinetic_post += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));

            BOOST_TEST(mjolnir::math::X(sys.position(i)) == mjolnir::math::X(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Y(sys.position(i)) == mjolnir::math::Y(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Z(sys.position(i)) == mjolnir::math::Z(sys_pre.position(i)));
        }

        BOOST_TEST(mjolnir::math::X(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(translation) == 0.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(mjolnir::math::X(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(rotation) == 0.0, boost::test_tools::tolerance(tol));

        // not rescaled.
        BOOST_TEST(E_kinetic_pre >= E_kinetic_post);
    }
}

BOOST_AUTO_TEST_CASE(SystemMotionRemover_translation)
{
    mjolnir::LoggerManager::set_default_logger("test_omp_system_motion_remover.log");

    using traits_type   = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type     = traits_type::real_type;
    using coord_type    = traits_type::coordinate_type;
    using boundary_type = traits_type::boundary_type;
    using system_type   = mjolnir::System<traits_type>;

    constexpr real_type tol = 1e-8;
    constexpr std::size_t N = 100;

    system_type sys(N, boundary_type{});
    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(0.0, 1.0);

    for(std::size_t n=0; n<100; ++n)
    {
        real_type E_kinetic_pre = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            sys.mass(i)     = 1.0 + uni(mt);
            sys.rmass(i)    = 1.0 / sys.mass(i);
            sys.position(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.velocity(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.force(i)    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

            sys.name(0)  = "X";
            sys.group(0) = "X";

            E_kinetic_pre += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));
        }
        const system_type sys_pre(sys);

        mjolnir::SystemMotionRemover<traits_type> remover(true, false, true);

        remover.remove(sys);

        auto translation = mjolnir::math::make_coordinate<coord_type>(0,0,0);

        real_type E_kinetic_post = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            const auto  m = sys.mass(i);
            const auto& v = sys.velocity(i);

            translation += m * v;
            E_kinetic_post += 0.5 * m * mjolnir::math::length_sq(v);

            BOOST_TEST(mjolnir::math::X(sys.position(i)) == mjolnir::math::X(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Y(sys.position(i)) == mjolnir::math::Y(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Z(sys.position(i)) == mjolnir::math::Z(sys_pre.position(i)));
        }

        BOOST_TEST(mjolnir::math::X(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(translation) == 0.0, boost::test_tools::tolerance(tol));

        // rescaled.
        BOOST_TEST(E_kinetic_pre == E_kinetic_post, boost::test_tools::tolerance(tol));
    }

    for(std::size_t n=0; n<100; ++n)
    {
        real_type E_kinetic_pre = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            sys.mass(i)     = 1.0 + uni(mt);
            sys.rmass(i)    = 1.0 / sys.mass(i);
            sys.position(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.velocity(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.force(i)    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

            sys.name(0)  = "X";
            sys.group(0) = "X";

            E_kinetic_pre += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));
        }
        const system_type sys_pre(sys);
        mjolnir::SystemMotionRemover<traits_type> remover(true, false, false);

        remover.remove(sys);

        auto translation = mjolnir::math::make_coordinate<coord_type>(0,0,0);

        real_type E_kinetic_post = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            const auto  m = sys.mass(i);
            const auto& v = sys.velocity(i);

            translation += m * v;

            E_kinetic_post += 0.5 * m * mjolnir::math::length_sq(v);

            BOOST_TEST(mjolnir::math::X(sys.position(i)) == mjolnir::math::X(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Y(sys.position(i)) == mjolnir::math::Y(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Z(sys.position(i)) == mjolnir::math::Z(sys_pre.position(i)));
        }

        BOOST_TEST(mjolnir::math::X(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(translation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(translation) == 0.0, boost::test_tools::tolerance(tol));

        // not rescaled.
        BOOST_TEST(E_kinetic_pre >= E_kinetic_post);
    }
}

BOOST_AUTO_TEST_CASE(SystemMotionRemover_rotation)
{
    mjolnir::LoggerManager::set_default_logger("test_omp_system_motion_remover.log");

    using traits_type   = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type     = traits_type::real_type;
    using coord_type    = traits_type::coordinate_type;
    using boundary_type = traits_type::boundary_type;
    using system_type   = mjolnir::System<traits_type>;

    constexpr real_type tol = 1e-8;
    constexpr std::size_t N = 100;

    system_type sys(N, boundary_type{});
    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(0.0, 1.0);

    for(std::size_t n=0; n<100; ++n)
    {
        real_type E_kinetic_pre = 0.0;
        auto CoM = mjolnir::math::make_coordinate<coord_type>(0,0,0);
        for(std::size_t i=0; i<N; ++i)
        {
            sys.mass(i)     = 1.0 + uni(mt);
            sys.rmass(i)    = 1.0 / sys.mass(i);
            sys.position(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.velocity(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.force(i)    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

            sys.name(0)  = "X";
            sys.group(0) = "X";

            CoM += sys.mass(i) * sys.position(i);
            E_kinetic_pre += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));
        }
        const system_type sys_pre(sys);
        mjolnir::SystemMotionRemover<traits_type> remover(true, true, true);

        remover.remove(sys);

        auto rotation    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

        real_type E_kinetic_post = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            const auto  m = sys.mass(i);
            const auto  r = sys.position(i) - CoM;
            const auto& v = sys.velocity(i);

            rotation       += m * mjolnir::math::cross_product(r, v);
            E_kinetic_post += 0.5 * m * mjolnir::math::length_sq(v);

            BOOST_TEST(mjolnir::math::X(sys.position(i)) == mjolnir::math::X(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Y(sys.position(i)) == mjolnir::math::Y(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Z(sys.position(i)) == mjolnir::math::Z(sys_pre.position(i)));
        }

        BOOST_TEST(mjolnir::math::X(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(rotation) == 0.0, boost::test_tools::tolerance(tol));

        // rescaled.
        BOOST_TEST(E_kinetic_pre == E_kinetic_post, boost::test_tools::tolerance(tol));
    }

    for(std::size_t n=0; n<100; ++n)
    {
        real_type E_kinetic_pre = 0.0;
        auto CoM = mjolnir::math::make_coordinate<coord_type>(0,0,0);
        for(std::size_t i=0; i<N; ++i)
        {
            sys.mass(i)     = 1.0 + uni(mt);
            sys.rmass(i)    = 1.0 / sys.mass(i);
            sys.position(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.velocity(i) = mjolnir::math::make_coordinate<coord_type>(uni(mt), uni(mt), uni(mt));
            sys.force(i)    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

            sys.name(0)  = "X";
            sys.group(0) = "X";

            CoM += sys.mass(i) * sys.position(i);
            E_kinetic_pre += 0.5 * sys.mass(i) * mjolnir::math::length_sq(sys.velocity(i));
        }
        const system_type sys_pre(sys);
        mjolnir::SystemMotionRemover<traits_type> remover(true, true, false);

        remover.remove(sys);

        auto rotation    = mjolnir::math::make_coordinate<coord_type>(0,0,0);

        real_type E_kinetic_post = 0.0;
        for(std::size_t i=0; i<N; ++i)
        {
            const auto  m = sys.mass(i);
            const auto  r = sys.position(i) - CoM;
            const auto& v = sys.velocity(i);

            rotation       += m * mjolnir::math::cross_product(r, v);
            E_kinetic_post += 0.5 * m * mjolnir::math::length_sq(v);

            BOOST_TEST(mjolnir::math::X(sys.position(i)) == mjolnir::math::X(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Y(sys.position(i)) == mjolnir::math::Y(sys_pre.position(i)));
            BOOST_TEST(mjolnir::math::Z(sys.position(i)) == mjolnir::math::Z(sys_pre.position(i)));
        }

        BOOST_TEST(mjolnir::math::X(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(rotation) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(rotation) == 0.0, boost::test_tools::tolerance(tol));

        // not rescaled.
        BOOST_TEST(E_kinetic_pre >= E_kinetic_post);
    }
}


/*
 * Copyright 2018 Autoware Foundation. All rights reserved.
 *
 * 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.
 *
 *
 * v1.0 Yukihiro Saito
 */

#include "car_corrector.hpp"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace normal {
bool CarCorrector::correct(
  autoware_perception_msgs::Shape & shape_output, geometry_msgs::Pose & pose_output)
{
  Eigen::Translation<double, 2> trans =
    Eigen::Translation<double, 2>(pose_output.position.x, pose_output.position.y);

  Eigen::Quaterniond quat(
    pose_output.orientation.w, pose_output.orientation.x, pose_output.orientation.y,
    pose_output.orientation.z);
  Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(0, 1, 2);
  const double yaw = euler[2];
  Eigen::Rotation2Dd rotate(yaw);

  Eigen::Affine2d affine_mat;
  affine_mat = trans * rotate.toRotationMatrix();

  /*
   *         ^ x
   *         |
   *        (0)
   * y       |
   * <--(1)--|--(3)--
   *         |
   *        (2)
   *         |
   */
  std::vector<Eigen::Vector2d> v_point;
  v_point.push_back(Eigen::Vector2d(shape_output.dimensions.x / 2.0, 0.0));
  v_point.push_back(Eigen::Vector2d(0.0, shape_output.dimensions.y / 2.0));
  v_point.push_back(Eigen::Vector2d(-shape_output.dimensions.x / 2.0, 0.0));
  v_point.push_back(Eigen::Vector2d(0.0, -shape_output.dimensions.y / 2.0));

  size_t first_most_distant_index;
  {
    double distance = 0.0;
    for (size_t i = 0; i < v_point.size(); ++i) {
      if (distance < (affine_mat * v_point.at(i)).norm()) {
        distance = (affine_mat * v_point.at(i)).norm();
        first_most_distant_index = i;
      }
    }
  }
  size_t second_most_distant_index;
  {
    double distance = 0.0;
    for (size_t i = 0; i < v_point.size(); ++i) {
      if (distance < (affine_mat * v_point.at(i)).norm() && i != first_most_distant_index) {
        distance = (affine_mat * v_point.at(i)).norm();
        second_most_distant_index = i;
      }
    }
  }
  size_t third_most_distant_index;
  {
    double distance = 0.0;
    for (size_t i = 0; i < v_point.size(); ++i) {
      if (
        (distance < (affine_mat * v_point.at(i)).norm()) && i != first_most_distant_index &&
        i != second_most_distant_index) {
        distance = (affine_mat * v_point.at(i)).norm();
        third_most_distant_index = i;
      }
    }
  }

  // rule based correction
  Eigen::Vector2d correction_vector = Eigen::Vector2d::Zero();
  constexpr double min_width = 1.2;
  constexpr double max_width = 2.2;
  constexpr double min_length = 3.0;
  constexpr double max_length = 5.0;

  if ((int)std::abs((int)first_most_distant_index - (int)second_most_distant_index) % 2 == 0) {
    if (
      min_width < (v_point.at(first_most_distant_index) * 2.0).norm() &&
      (v_point.at(first_most_distant_index) * 2.0).norm() < max_width) {
      if ((v_point.at(third_most_distant_index) * 2.0).norm() < max_length) {
        correction_vector = v_point.at(third_most_distant_index);
        if (correction_vector.x() == 0.0)
          correction_vector.y() =
            std::max(std::abs(correction_vector.y()), ((min_length + max_length) / 2.0) / 2.0) *
              (correction_vector.y() < 0.0 ? -1.0 : 1.0) -
            correction_vector.y();
        else if (correction_vector.y() == 0.0)
          correction_vector.x() =
            std::max(std::abs(correction_vector.x()), ((min_length + max_length) / 2.0) / 2.0) *
              (correction_vector.x() < 0.0 ? -1.0 : 1.0) -
            correction_vector.x();
        else
          return false;
      } else {
        return false;
      }
    } else if (
      min_length < (v_point.at(first_most_distant_index) * 2.0).norm() &&
      (v_point.at(first_most_distant_index) * 2.0).norm() < max_length) {
      if ((v_point.at(third_most_distant_index) * 2.0).norm() < max_width) {
        correction_vector = v_point.at(third_most_distant_index);
        if (correction_vector.x() == 0.0)
          correction_vector.y() =
            std::max(std::abs(correction_vector.y()), ((min_width + max_width) / 2.0) / 2.0) *
              (correction_vector.y() < 0.0 ? -1.0 : 1.0) -
            correction_vector.y();
        else if (correction_vector.y() == 0.0)
          correction_vector.x() =
            std::max(std::abs(correction_vector.x()), ((min_width + max_width) / 2.0) / 2.0) *
              (correction_vector.x() < 0.0 ? -1.0 : 1.0) -
            correction_vector.x();
        else
          return false;
      } else {
        return false;
      }
    } else {
      return false;
    }
  }
  // fit width
  else if (
    (min_width < (v_point.at(first_most_distant_index) * 2.0).norm() &&
     (v_point.at(first_most_distant_index) * 2.0).norm() < max_width) &&
    (min_width < (v_point.at(second_most_distant_index) * 2.0).norm() &&
     (v_point.at(second_most_distant_index) * 2.0).norm() < max_width)) {
    correction_vector = v_point.at(first_most_distant_index);
    if (correction_vector.x() == 0.0)
      correction_vector.y() =
        std::max(std::abs(correction_vector.y()), ((min_length + max_length) / 2.0) / 2.0) *
          (correction_vector.y() < 0.0 ? -1.0 : 1.0) -
        correction_vector.y();
    else if (correction_vector.y() == 0.0)
      correction_vector.x() =
        std::max(std::abs(correction_vector.x()), ((min_length + max_length) / 2.0) / 2.0) *
          (correction_vector.x() < 0.0 ? -1.0 : 1.0) -
        correction_vector.x();
    else
      return false;
  } else if (
    min_width < (v_point.at(first_most_distant_index) * 2.0).norm() &&
    (v_point.at(first_most_distant_index) * 2.0).norm() < max_width) {
    correction_vector = v_point.at(second_most_distant_index);
    if (correction_vector.x() == 0.0)
      correction_vector.y() =
        std::max(std::abs(correction_vector.y()), ((min_length + max_length) / 2.0) / 2.0) *
          (correction_vector.y() < 0.0 ? -1.0 : 1.0) -
        correction_vector.y();
    else if (correction_vector.y() == 0.0)
      correction_vector.x() =
        std::max(std::abs(correction_vector.x()), ((min_length + max_length) / 2.0) / 2.0) *
          (correction_vector.x() < 0.0 ? -1.0 : 1.0) -
        correction_vector.x();
    else
      return false;
  } else if (
    min_width < (v_point.at(second_most_distant_index) * 2.0).norm() &&
    (v_point.at(second_most_distant_index) * 2.0).norm() < max_width) {
    correction_vector = v_point.at(first_most_distant_index);

    if (correction_vector.x() == 0.0)
      correction_vector.y() =
        std::max(std::abs(correction_vector.y()), ((min_length + max_length) / 2.0) / 2.0) *
          (correction_vector.y() < 0.0 ? -1.0 : 1.0) -
        correction_vector.y();
    else if (correction_vector.y() == 0.0)
      correction_vector.x() =
        std::max(std::abs(correction_vector.x()), ((min_length + max_length) / 2.0) / 2.0) *
          (correction_vector.x() < 0.0 ? -1.0 : 1.0) -
        correction_vector.x();
    else
      return false;
  }
  // fit length
  else if (
    (min_length < (v_point.at(first_most_distant_index) * 2.0).norm() &&
     (v_point.at(first_most_distant_index) * 2.0).norm() < max_length) &&
    (v_point.at(second_most_distant_index) * 2.0).norm() < max_width) {
    correction_vector = v_point.at(second_most_distant_index);

    if (correction_vector.x() == 0.0)
      correction_vector.y() =
        std::max(std::abs(correction_vector.y()), ((min_width + max_width) / 2.0) / 2.0) *
          (correction_vector.y() < 0.0 ? -1.0 : 1.0) -
        correction_vector.y();
    else if (correction_vector.y() == 0.0)
      correction_vector.x() =
        std::max(std::abs(correction_vector.x()), ((min_width + max_width) / 2.0) / 2.0) *
          (correction_vector.x() < 0.0 ? -1.0 : 1.0) -
        correction_vector.x();
    else
      return false;
  } else if (
    (min_length < (v_point.at(second_most_distant_index) * 2.0).norm() &&
     (v_point.at(second_most_distant_index) * 2.0).norm() < max_length) &&
    (v_point.at(first_most_distant_index) * 2.0).norm() < max_width) {
    correction_vector = v_point.at(first_most_distant_index);

    if (correction_vector.x() == 0.0)
      correction_vector.y() =
        std::max(std::abs(correction_vector.y()), ((min_width + max_width) / 2.0) / 2.0) *
          (correction_vector.y() < 0.0 ? -1.0 : 1.0) -
        correction_vector.y();
    else if (correction_vector.y() == 0.0)
      correction_vector.x() =
        std::max(std::abs(correction_vector.x()), ((min_width + max_width) / 2.0) / 2.0) *
          (correction_vector.x() < 0.0 ? -1.0 : 1.0) -
        correction_vector.x();
    else
      return false;
  } else {
    return false;
  }

  shape_output.dimensions.x += std::abs(correction_vector.x()) * 2.0;
  shape_output.dimensions.y += std::abs(correction_vector.y()) * 2.0;
  pose_output.position.x += (rotate.toRotationMatrix() * correction_vector).x();
  pose_output.position.y += (rotate.toRotationMatrix() * correction_vector).y();

  // correct to set long length is x, short length is y
  if (shape_output.dimensions.x < shape_output.dimensions.y) {
    double roll, pitch, yaw;
    tf2::Quaternion quaternion;
    tf2::fromMsg(pose_output.orientation, quaternion);
    tf2::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    double yaw_90_rotated = yaw + M_PI_2;
    tf2::Quaternion corrected_quaternion;
    corrected_quaternion.setRPY(roll, pitch, yaw_90_rotated);
    pose_output.orientation = tf2::toMsg(corrected_quaternion);
    double temp = shape_output.dimensions.x;
    shape_output.dimensions.x = shape_output.dimensions.y;
    shape_output.dimensions.y = temp;
  }

  return true;
}
}


#include "fixed.h"

#include <Eigen/Eigen>

Fixed::Fixed(const Eigen::MatrixXd& V, const Eigen::VectorXi& indices, double k)
	: Element(V, indices, Eigen::VectorXi()), _k(k)
{
}

Fixed::~Fixed()
{

}

void Fixed::compute(const Eigen::MatrixXd& V, const Eigen::MatrixXd& ext)
{
	Eigen::MatrixXd x = extract(V);
	_energy = energy(x);
	_G = computeGradient(x);

#ifdef COMPUTE_FINITE_DIFFERENCE
	_fG = computeGradient(x);
#endif
}

double Fixed::diffTest(const Eigen::MatrixXd& V, double h)
{
	Eigen::MatrixXd x = extract(V);
	Eigen::MatrixXd G = computeGradient(x);
	const double e = energy(x);

	Eigen::MatrixXd hG = Eigen::MatrixXd::Zero(x.rows(), x.cols());
	for (int i = 0; i < x.rows(); i++)
	{
		for (int j = 0; j < x.cols(); j++)
		{
			Eigen::MatrixXd hX = x;
			hX(i, j) += h;

			hG(i, j) = (energy(hX) - e) / h;
		}
	}

	return (hG - G).squaredNorm();
}

/////////////////////////////////////////////////////

Eigen::MatrixXd Fixed::computeGradient(const Eigen::MatrixXd& x) const
{
	return _k * (x - _X);
}

double Fixed::energy(const Eigen::MatrixXd& x) const
{
	return 0.5 * _k * (x - _X).rowwise().squaredNorm().sum();
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/basicTypedefs.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"

#include "Tudat/Mathematics/Filters/linearKalmanFilter.h"
#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h"

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_linear_kalman_filter )

// Test implementation of linear Kalman filter class. Tested by comparison with code by https://github.com/hmartiro/kalman-cpp.
BOOST_AUTO_TEST_CASE( testLinearKalmanFilter )
{
    using namespace tudat::filters;

    // Set initial conditions
    const double initialTime = 0;
    const double timeStep = 1.0 / 30.0;
    const unsigned int numberOfTimeSteps = 45;

    Eigen::Vector3d initialStateVector = Eigen::Vector3d::Zero( );
    initialStateVector[ 0 ] = 1.04202710058;
    initialStateVector[ 2 ] = -9.81;

    Eigen::Vector3d initialEstimatedStateVector = Eigen::Vector3d::Zero( );
    initialEstimatedStateVector[ 0 ] = 1.04202710058;
    initialEstimatedStateVector[ 2 ] = -9.81;

    Eigen::Matrix3d initialEstimatedStateCovarianceMatrix = Eigen::Matrix3d::Zero( );
    initialEstimatedStateCovarianceMatrix( 0, 0 ) = 0.1;
    initialEstimatedStateCovarianceMatrix( 0, 1 ) = 0.1;
    initialEstimatedStateCovarianceMatrix( 0, 2 ) = 0.1;
    initialEstimatedStateCovarianceMatrix( 1, 0 ) = 0.1;
    initialEstimatedStateCovarianceMatrix( 1, 1 ) = 10000.0;
    initialEstimatedStateCovarianceMatrix( 1, 2 ) = 10.0;
    initialEstimatedStateCovarianceMatrix( 2, 0 ) = 0.1;
    initialEstimatedStateCovarianceMatrix( 2, 1 ) = 10.0;
    initialEstimatedStateCovarianceMatrix( 2, 2 ) = 100.0;

    // Set system dynamics and measurement
    Eigen::Matrix3d stateTransitionMatrix = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d controlMatrix = Eigen::Matrix3d::Zero( );
    Eigen::RowVector3d measurementMatrix = Eigen::RowVector3d::Zero( );
    stateTransitionMatrix( 0, 0 ) = 1.0;
    stateTransitionMatrix( 0, 1 ) = timeStep;
    stateTransitionMatrix( 1, 1 ) = 1.0;
    stateTransitionMatrix( 1, 2 ) = timeStep;
    stateTransitionMatrix( 2, 2 ) = 1.0;
    measurementMatrix[ 0 ] = 1.0;

    // Set system and measurement uncertainty
    Eigen::Matrix3d systemUncertainty = Eigen::Matrix3d::Zero( );
    Eigen::Vector1d measurementUncertainty = Eigen::Vector1d::Zero( );
    systemUncertainty( 0, 0 ) = 0.05;
    systemUncertainty( 0, 1 ) = 0.05;
    systemUncertainty( 1, 0 ) = 0.05;
    systemUncertainty( 1, 1 ) = 0.05;
    measurementUncertainty[ 0 ] = 0.5;

    // Create linear Kalman filter object
    KalmanFilterDoublePointer linearFilter = std::make_shared< LinearKalmanFilterDouble >(
                [ & ]( const double, const Eigen::Vector3d& ){ return stateTransitionMatrix; },
                [ & ]( const double, const Eigen::Vector3d& ){ return controlMatrix; },
                [ & ]( const double, const Eigen::Vector3d& ){ return measurementMatrix; },
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix );

    // Load noise from file
    Eigen::MatrixXd systemNoise = input_output::readMatrixFromFile( tudat::input_output::getTudatRootPath( ) +
                                                                    "/Mathematics/Filters/UnitTests/noiseData/lkfSystemNoise1.dat" );
    Eigen::MatrixXd measurementNoise = input_output::readMatrixFromFile( tudat::input_output::getTudatRootPath( ) +
                                                                         "/Mathematics/Filters/UnitTests/noiseData/lkfMeasurementNoise1.dat" );

    // Loop over each time step
    const bool showProgress = false;
    double currentTime = initialTime;
    Eigen::Vector3d currentStateVector = initialStateVector;
    Eigen::Vector3d currentControlVector = Eigen::Vector3d::Zero( );
    Eigen::Vector1d currentMeasurementVector;
    std::map< double, Eigen::Vector3d > actualStateVectorHistory;
    std::map< double, Eigen::Vector1d > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentStateVector = stateTransitionMatrix * currentStateVector + controlMatrix * currentControlVector +
                systemNoise.col( i );
        currentMeasurementVector = measurementMatrix * currentStateVector + measurementNoise.col( i );

        // Update filter
        linearFilter->updateFilter( currentMeasurementVector );
        currentTime = linearFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << linearFilter->getCurrentStateEstimate( ).transpose( ) << std::endl;
        }
    }

    // Check that final state is as expected
    Eigen::Vector3d expectedFinalState;
    expectedFinalState << -7.415393533447765, -12.421405468923618, -8.9114310345206711;
    for ( unsigned int i = 0; i < expectedFinalState.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( linearFilter->getCurrentStateEstimate( )[ i ] - expectedFinalState[ i ], 1.0e-10 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#pragma once
#include <memory> // shared_ptr

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

#include <opt/constrained_problem.hpp>
#include <opt/optimization_problem.hpp>
#include <opt/optimization_results.hpp>
#include <solvers/lcp_solver.hpp>
#include <solvers/optimization_solver.hpp>

namespace ccd {
namespace opt {

    /**
     * @brief Method for updating candidate solution during NCP iterations
     *
     * Update: \f$ x_{i+1} = x_{i} + \Delta x \f$
     */
    enum class NCPUpdate {
        /// \f$\Delta x = A^{-1} \nabla g(x_i)^T \lambda_i\f$
        G_GRADIENT,
        /// \f$\Delta x = A^{-1} \nabla g(x_i)^T \lambda_i + A^{-1}b - x_i\f$
        LINEAR
    };

    class NCPSolver : public OptimizationSolver {
    public:
        NCPSolver();
        virtual ~NCPSolver() = default;

        /// Initialize the state of the solver using the settings saved in JSON
        virtual void settings(const nlohmann::json& json) override;
        /// Export the state of the solver using the settings saved in JSON
        virtual nlohmann::json settings() const override;

        /// An identifier for the solver class
        static std::string solver_name() { return "ncp_solver"; }
        /// An identifier for this solver
        virtual std::string name() const override
        {
            return NCPSolver::solver_name();
        }

        void set_problem(OptimizationProblem& problem) override
        {
            assert(problem.is_constrained_problem());
            problem_ptr = dynamic_cast<ConstrainedProblem*>(&problem);
        }

        /// Initialize the solver state for a new solve
        void init_solve(const Eigen::VectorXd& x0) override;
        /// Solve the saved optimization problem to completion
        virtual OptimizationResults solve(const Eigen::VectorXd& x0) override;
        OptimizationResults
        solve(const Eigen::VectorXd& x0, const bool use_grad);
        /// Perform a single step of solving the optimization problem
        virtual OptimizationResults step_solve() override;

        // --------------------------------------------------------------------
        // Configuration
        // --------------------------------------------------------------------
        bool do_line_search;
        bool solve_for_active_cstr;
        double convergence_tolerance;
        NCPUpdate update_type;
        LCPSolver lcp_solver;
        int max_iterations;

        // --------------------------------------------------------------------
        // Optimization Status
        // --------------------------------------------------------------------
        std::shared_ptr<Eigen::SparseLU<Eigen::SparseMatrix<double>>> Asolver;
        Eigen::VectorXd g_xi;
        Eigen::MatrixXd jac_g_xi;

        // --------------------------------------------------------------------
        // Optimization results
        // --------------------------------------------------------------------
        Eigen::VectorXd xi;
        Eigen::VectorXd lambda_i;

    protected:
        void compute_linear_system(ConstrainedProblem& problem_ptr_);
        void compute_initial_solution();

        /**
         * @brief Linearize the problem and solve for primal variables (xᵢ₊₁)
         * and dual variables (λᵢ).
         *
         * Linearization:
         * \f{aligned}{
         *      A x_{i+1} = b + \nabla g(x_i)^T \lambda_i \\
         *      0 \leq \lambda_i \perp g(x_i) + \nabla g(x_i) Δx \geq 0
         * \f}
         * Update:
         * \f{aligned}{
         *      x_{i+1} = x_i + \Delta x
         * \f}
         * \f$\Delta x\f$:
         * * g_gradient update: \f$\Delta x = A^{-1} [\nabla g(x_i)]^T \lambda_i
         *      \f$
         * * linearized update: \f$\Delta x = A^{-1} [\nabla g(x_i)]^T \lambda_i
         *      + A^{-1}b - x_i\f$
         *
         * We want to take our problem to the form
         * \f{aligned}{
         *      s = q + N (M\lambda_i + p) \\
         *      0 \leq \lambda_i \perp s \geq 0
         * \f}
         * where
         * \f{aligned}{
         *      q &= g(x_i) \\
         *      N &= \nabla g(x_i) \\
         *      M &= A^{-1} [\nabla g(x_i)]^{T} \\
         *      p &= \Delta x - A^{-1} [\nabla g(x_i)]^{T} \lambda_i
         * \f}
         */
        Eigen::VectorXd solve_lcp();

        /**
         * @brief Compute \f$A^{-1}x\f$.
         *
         * Uses a precomputed decomposition of A to solve the system in
         * \f$O(n^2)\f$.
         */
        Eigen::VectorXd Ainv(const Eigen::VectorXd& x) const;

        // --------------------------------------------------------------------
        // Fields
        // --------------------------------------------------------------------
        Eigen::SparseMatrix<double> A;
        Eigen::VectorXd b;
        ConstrainedProblem* problem_ptr;

        /// @brief Current number of outer iterations completed.
        int num_outer_iterations_;
        std::string name_;
        bool m_use_gradient = true;
    };

    void
    zero_out_fixed_dof(const Eigen::VectorXb& is_fixed, Eigen::MatrixXd& jac);
} // namespace opt
} // namespace ccd