/*
 * Copyright [2019] [Christopher Syben]
 *
 * 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.
 *
 * Ray-driven cone-beam projector CUDA kernel using texture inteprolation
 * Implementation adapted from CONRAD
 * PYRO-NN is developed as an Open Source project under the Apache License, Version 2.0.
*/
#if GOOGLE_CUDA
#define EIGEN_USE_GPU
#include "../helper_headers/helper_grid.h"
#include "../helper_headers/helper_math.h"
#include "../helper_headers/helper_eigen.h"
#include "../helper_headers/helper_geometry_gpu.h"
#include <Eigen/QR>
texture<float, 3, cudaReadModeElementType> volume_as_texture;
#define CUDART_INF_F __int_as_float(0x7f800000)

#define BLOCKSIZE_X           16
#define BLOCKSIZE_Y           16

#define gpuErrchk(ans) { gpuAssert((ans), __FILE__, __LINE__); }
inline void gpuAssert(cudaError_t code, const char *file, int line, bool abort=true)
{
   if (code != cudaSuccess)
   {
      fprintf(stderr,"GPUassert: %s %s %d\n", cudaGetErrorString(code), file, line);
      exit(code);
   }
}

__device__ float kernel_project3D_tex_interp(const float3 source_point, const float3 ray_vector, const float step_size, const uint3 volume_size)
{
    float pixel = 0.0f;
    // Step 1: compute alpha value at entry and exit point of the volume
    float min_alpha, max_alpha;
    min_alpha = 0;
    max_alpha = CUDART_INF_F;

    if (0.0f != ray_vector.x)
    {
        float volume_min_edge_point = 0 - 0.5f;
        float volume_max_edge_point = volume_size.x - 0.5f;

        float reci = 1.0f / ray_vector.x;
        float alpha0 = (volume_min_edge_point - source_point.x) * reci;
        float alpha1 = (volume_max_edge_point - source_point.x) * reci;
        min_alpha = fmin(alpha0, alpha1);
        max_alpha = fmax(alpha0, alpha1);
    }

    if (0.0f != ray_vector.y)
    {
        float volume_min_edge_point = 0 - 0.5f;
        float volume_max_edge_point = volume_size.y - 0.5f;

        float reci = 1.0f / ray_vector.y;
        float alpha0 = (volume_min_edge_point - source_point.y) * reci;
        float alpha1 = (volume_max_edge_point - source_point.y) * reci;
        min_alpha = fmax(min_alpha, fmin(alpha0, alpha1));
        max_alpha = fmin(max_alpha, fmax(alpha0, alpha1));
    }

    if (0.0f != ray_vector.z)
    {
        float volume_min_edge_point = 0 - 0.5f;
        float volume_max_edge_point = volume_size.z - 0.5f;

        float reci = 1.0f / ray_vector.z;
        float alpha0 = (volume_min_edge_point - source_point.z) * reci;
        float alpha1 = (volume_max_edge_point - source_point.z) * reci;
        min_alpha = fmax(min_alpha, fmin(alpha0, alpha1));
        max_alpha = fmin(max_alpha, fmax(alpha0, alpha1));
    }
    // we start not at the exact entry point
    // => we can be sure to be inside the volume
    min_alpha += step_size * 0.5f;

    // Step 2: Cast ray if it intersects the volume
    // Trapezoidal rule (interpolating function = piecewise linear func)

    float px, py, pz;

    // Entrance boundary
    // For the initial interpolated value, only a half stepsize is
    //  considered in the computation.
    if (min_alpha < max_alpha)
    {
        px = source_point.x + min_alpha * ray_vector.x;
        py = source_point.y + min_alpha * ray_vector.y;
        pz = source_point.z + min_alpha * ray_vector.z;

        pixel += 0.5f * tex3D(volume_as_texture, px+0.5f, py+0.5f, pz+0.5f );

        min_alpha += step_size;
    }
    // Mid segments
    while (min_alpha < max_alpha)
    {
        px = source_point.x + min_alpha * ray_vector.x;
        py = source_point.y + min_alpha * ray_vector.y;
        pz = source_point.z + min_alpha * ray_vector.z;

        pixel += tex3D(volume_as_texture, px+0.5f, py+0.5f, pz+0.5f );

        min_alpha += step_size;
    }
    // Scaling by stepsize;
    pixel *= step_size;

    // Last segment of the line
    if (pixel > 0.0f)
    {
        pixel -= 0.5f * step_size * tex3D(volume_as_texture, px+0.5f, py+0.5f, pz+0.5f );

        min_alpha -= step_size;
        float last_step_size = max_alpha - min_alpha;

        pixel += 0.5f * last_step_size* tex3D(volume_as_texture, px+0.5f, py+0.5f, pz+0.5f );

        px = source_point.x + max_alpha * ray_vector.x;
        py = source_point.y + max_alpha * ray_vector.y;
        pz = source_point.z + max_alpha * ray_vector.z;

        // The last segment of the line integral takes care of the
        // varying length.
        pixel += 0.5f * last_step_size * tex3D(volume_as_texture, px+0.5f, py+0.5f, pz+0.5f);
    }
    return pixel;
}

__global__ void project_3Dcone_beam_kernel_tex_interp(float *pSinogram, const float *d_inv_AR_matrices, const float3 *d_src_points, const float *sampling_step_size,
                                          const uint3 volume_size, const float *volume_spacing_ptr,
                                          const uint2 detector_size, const int number_of_projections)
{
    uint2 detector_idx = make_uint2( blockIdx.x * blockDim.x + threadIdx.x,  blockIdx.y* blockDim.y + threadIdx.y  );
    uint projection_number = blockIdx.z;
    //Prep: Wrap pointer to float3 for better readable code
    float3 volume_spacing = make_float3(*(volume_spacing_ptr+2), *(volume_spacing_ptr+1), *volume_spacing_ptr);
    if (detector_idx.x >= detector_size.x || detector_idx.y >= detector_size.y || blockIdx.z >= number_of_projections)
    {
        return;
    }
    // //Preparations:
	d_inv_AR_matrices += projection_number * 9;
    float3 source_point = d_src_points[projection_number];

    //Compute ray direction
    const float rx = d_inv_AR_matrices[2] + detector_idx.y * d_inv_AR_matrices[1] + detector_idx.x * d_inv_AR_matrices[0];
    const float ry = d_inv_AR_matrices[5] + detector_idx.y * d_inv_AR_matrices[4] + detector_idx.x * d_inv_AR_matrices[3];
    const float rz = d_inv_AR_matrices[8] + detector_idx.y * d_inv_AR_matrices[7] + detector_idx.x * d_inv_AR_matrices[6];

    float3 ray_vector = make_float3(rx,ry,rz);
    ray_vector = normalize(ray_vector);

    float pixel = kernel_project3D_tex_interp(
        source_point,
        ray_vector,
        *sampling_step_size,
        volume_size);

    pixel *= sqrt((ray_vector.x * volume_spacing.x) * (ray_vector.x * volume_spacing.x) +
            (ray_vector.y * volume_spacing.y) * (ray_vector.y * volume_spacing.y) +
            (ray_vector.z * volume_spacing.z) * (ray_vector.z * volume_spacing.z));

    unsigned sinogram_idx = projection_number * detector_size.y * detector_size.x +  detector_idx.y * detector_size.x + detector_idx.x;

    pSinogram[sinogram_idx] = pixel;
    return;
}

/*************** WARNING ******************./
    *
    *   Tensorflow is allocating the whole GPU memory for itself and just leave a small slack memory
    *   using cudaMalloc and cudaMalloc3D will allocate memory in this small slack memory !
    *   Therefore, currently only small volumes can be used (they have to fit into the slack memory which TF does not allocae !)
    *
    *   This is the kernel based on texture interpolation, thus, the allocations are not within the Tensorflow managed memory.
    *   If memory errors occure:
    *    1. start Tensorflow with less gpu memory and allow growth
    *    2. switch to software-based interpolation.
    *
    *   TODO: use context->allocate_tmp and context->allocate_persistent instead of cudaMalloc for the inv_AR_matrix and src_points array
    *       : https://stackoverflow.com/questions/48580580/tensorflow-new-op-cuda-kernel-memory-managment
    *
    */
void Cone_Projection_Kernel_Tex_Interp_Launcher(const float* volume_ptr, float *out, const float *inv_AR_matrix,const float *src_points,
                                    const int number_of_projections, const int volume_width, const int volume_height, const int volume_depth,
                                    const float *volume_spacing, const int detector_width, const int detector_height,const float *step_size)
{
    cudaChannelFormatDesc channelDesc = cudaCreateChannelDesc<float>();
    volume_as_texture.addressMode[0] = cudaAddressModeBorder;
    volume_as_texture.addressMode[1] = cudaAddressModeBorder;
    volume_as_texture.addressMode[2] = cudaAddressModeBorder;
    volume_as_texture.filterMode = cudaFilterModeLinear;
    volume_as_texture.normalized = false;

    // //COPY inv AR matrix to graphics card as float array
    auto matrices_size_b = number_of_projections * 9 * sizeof(float);
    float *d_inv_AR_matrices;
    gpuErrchk(cudaMalloc(&d_inv_AR_matrices, matrices_size_b));
    gpuErrchk(cudaMemcpy(d_inv_AR_matrices, inv_AR_matrix, matrices_size_b, cudaMemcpyHostToDevice));
    //COPY source points to graphics card as float3
    auto src_points_size_b = number_of_projections * sizeof(float3);
    float3 *d_src_points;
    gpuErrchk(cudaMalloc(&d_src_points, src_points_size_b));
    gpuErrchk(cudaMemcpy(d_src_points, src_points, src_points_size_b, cudaMemcpyHostToDevice));

    //COPY volume to graphics card
    //Malloc cuda array for texture
    cudaExtent volume_extent = make_cudaExtent(  volume_width, volume_height, volume_depth );
    cudaExtent volume_extent_byte = make_cudaExtent( sizeof(float)*volume_width, volume_height, volume_depth );

    cudaPitchedPtr d_volumeMem = make_cudaPitchedPtr( const_cast<float*>( volume_ptr ),
                                                volume_width*sizeof(float),
                                                volume_width,
                                                volume_height
                                            );

    cudaArray *volume_array;
    gpuErrchk(cudaMalloc3DArray(&volume_array, &channelDesc, volume_extent));

    cudaMemcpy3DParms copyParams = {0};
    copyParams.srcPtr = d_volumeMem;
    copyParams.dstArray = volume_array;
    copyParams.extent = volume_extent;
    copyParams.kind = cudaMemcpyDeviceToDevice;

    gpuErrchk(cudaMemcpy3D(&copyParams));

    gpuErrchk(cudaBindTextureToArray(volume_as_texture, volume_array, channelDesc))
    uint3 volume_size = make_uint3(volume_width, volume_height, volume_depth);
    uint2 detector_size = make_uint2(detector_width, detector_height);

    const dim3 blocksize = dim3( BLOCKSIZE_X, BLOCKSIZE_Y, 1 );
    const dim3 gridsize = dim3( detector_size.x / blocksize.x + 1, detector_size.y / blocksize.y + 1 , number_of_projections+1);

    project_3Dcone_beam_kernel_tex_interp<<<gridsize, blocksize>>>(out, d_inv_AR_matrices, d_src_points, step_size,
                                        volume_size, volume_spacing, detector_size, number_of_projections);

    cudaDeviceSynchronize();

    // check for errors
    gpuErrchk( cudaPeekAtLastError() );
    gpuErrchk(cudaFreeArray(volume_array));
    gpuErrchk(cudaUnbindTexture(volume_as_texture));
    gpuErrchk(cudaFree(d_inv_AR_matrices));
    gpuErrchk(cudaFree(d_src_points));
}

#endif


/*
 * Copyright (C) 2008-2011, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */

#include <iostream>
#include <boost/lexical_cast.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/numeric/conversion/converter.hpp>

#include <libport/cmath>
#include <libport/cstdlib>
#include <libport/ufloat.hh>

#ifdef LIBPORT_URBI_UFLOAT_LONG_LONG
# include <libport/ull-fixed-point.cc>
#endif

#ifdef LIBPORT_URBI_UFLOAT_FLOATING
# include <libport/uffloat.cc>
#endif

namespace libport
{
#ifdef LIBPORT_URBI_UFLOAT_TABULATED

# ifndef SINTABLE_POWER
  // The tables will containe 2^sintable_power elements.
#  define SINTABLE_POWER 10
# endif

  void buildSinusTable(int powersize);
  class DummyInit
  {
  public:
    DummyInit()
    {
      buildSinusTable(SINTABLE_POWER);
    }
  };

  DummyInit _dummyInit__;

  static ufloat* sinTable=0; //we store [O, PI/2[
  static ufloat* asinTable=0; //we store [O, 1[

  static unsigned long tableSize; //must be a power of two
  static int tableShift;

  void buildSinusTable(int powersize)
  {
    size_t size = 1<<powersize;
    tableShift = powersize;
    //don't use a step-based generation or errors will accumulate
    delete [] sinTable;
    delete [] asinTable;

    sinTable = new ufloat[size];
    asinTable = new ufloat[size];

    tableSize = size;
    for (size_t i=0;i<size;i++)
    {
      double idx = (double)i*(M_PI/2.0)/(double)size;
      float val = ::sin(idx);
      sinTable[i]=val;

      double idx2 = (double)i/(double)size;
      asinTable[i] = ::asin(idx2);
    }
  }

#endif

#if defined LIBPORT_URBI_UFLOAT_TABULATED \
 && !defined LIBPORT_URBI_UFLOAT_FLOATING

  ufloat tabulatedSin(ufloat val)
  {
    ufloat fidx = (val*(ufloat)tableSize / (M_PI/2.0));
    int idx = (int) fidx;
    ufloat rem = fidx -(ufloat)idx;

    idx = idx & (tableSize-1);

    if (fmod(val, M_PI) >= M_PI/2)
      idx = (tableSize-idx-1); //sin(pi/2+x) = sin(pi/2-x)
    ufloat interp = sinTable(idx)*(1.0-rem)+sinTable[(idx+1)%tableSize]*rem;

    return fmod(val, M_PI*2) > M_PI ? -interp : interp;
  }

  ufloat tabulatedCos(ufloat val)
  {
    ufloat fidx = (val*(ufloat)tableSize / (M_PI/2.0));
    int idx = (int) fidx;
    ufloat rem = fidx -(ufloat)idx;

    idx = idx & (tableSize-1);

    if (fmod(val, M_PI) < M_PI/2)
      idx = (tableSize-idx-1); //sin(pi/2+x) = sin(pi/2-x)

    ufloat interp = sinTable(idx)*(1.0-rem)+sinTable[(idx+1)%tableSize]*rem;

    return fmod(val, M_PI*2) > M_PI ? -interp : interp;
  }

  ufloat tabulatedASin(ufloat val)
  {
    ufloat fidx = val *(ufloat)tableSize;
    int idx =(int) fidx;
    ufloat rem = fidx -(ufloat)idx;
    idx = idx & (tableSize-1);
    ufloat interp = asinTable(idx)*(1.0-rem)+asinTable[(idx+1)%tableSize]*rem;

    return val < 0.0 ? -interp : interp;
  }

#endif

  /*------------------------.
  | From string to ufloat.  |
  `------------------------*/

  // MSVC makes it uselessly complex to pass isdigit-like functions by
  // pointer.
  /// \param xdigit  whether hexadecimal instead of decimal.
  static inline
  bool
  is_digit(int c, bool xdigit)
  {
    return xdigit ? isxdigit(c) : isdigit(c);
  }

  /// Remove underscores, allowed only between two digits.
  ///
  /// \param xdigit  whether hexadecimal instead of decimal.
  static
  std::string
  as_ufloat_strip(const std::string& s, bool xdigit)
  {
    std::string res;
    size_t len = s.size();
    for (size_t i = 0; i < len; /* nothing */)
      if (s[i] == '_')
      {
        // Look for the next non-underscore character.
        size_t next = s.find_first_not_of("_", i);
        if (res.empty()
            || !is_digit(res[res.size() - 1], xdigit)
            || !(next < len)
            || !is_digit(s[next], xdigit))
          throw boost::bad_lexical_cast();
        i = next;
      }
      else
      {
        res.append(1, s[i]);
        ++i;
      }
    return res;
  }

  ufloat
  as_ufloat(const std::string& s)
  {
    // Convert.
    if (s.substr(0, 2) == "0x")
    {
      std::string pure = as_ufloat_strip(s, true);
      char* end = 0;
      ufloat res = strtoll(pure.c_str(), &end, 0);
      // Refuse garbage after.
      if (end && *end)
        throw boost::bad_lexical_cast();
      return res;
    }
    else
      return boost::lexical_cast<libport::ufloat>
        (as_ufloat_strip(s, false));
  }

  /*------------------.
  | cast exceptions.  |
  `------------------*/

  const char *bad_numeric_cast::what() const throw()
  {
    return "bad numeric conversion: overflow or non empty fractional part";
  }

  const char *negative_overflow::what() const throw()
  {
    return "bad numeric conversion: negative overflow";
  }

  const char *positive_overflow::what() const throw()
  {
    return "bad numeric conversion: positive overflow";
  }

  /*-------------------.
  | ufloat converter.  |
  `-------------------*/

  template<typename S>
  struct ExactFloat2IntRounderPolicy
  {
    typedef S source_type;
    typedef S argument_type;

    static source_type nearbyint(argument_type s)
    {
      if (s != ceil(s))
	throw boost::numeric::bad_numeric_cast();
      return s;
    }

    typedef boost::mpl::integral_c<std::float_round_style,std::round_to_nearest>
      round_style;
  };

  /*-------------------.
  | numeric_castable.  |
  `-------------------*/

  template <typename T>
  bool
  numeric_castable(ufloat val)
  {
    double int_part;
    if (modf(val, &int_part) != 0)
      return false;

    static boost::numeric::converter
      <T,
      ufloat,
      boost::numeric::conversion_traits<T, ufloat>,
      boost::numeric::def_overflow_handler,
      ExactFloat2IntRounderPolicy<ufloat> > converter;

    return converter.out_of_range(val) == boost::numeric::cInRange;
  }

# define UFLOAT_CAST(Type)                                      \
  template                                                      \
  bool numeric_castable<Type>(ufloat v);

  // Instantiate.
  UFLOAT_CASTS

#undef UFLOAT_CAST

  /*---------------.
  | numeric_cast.  |
  `---------------*/

  template <typename T>
  T
  numeric_cast(ufloat val) throw (bad_numeric_cast)
  {
    try
    {
      static boost::numeric::converter
        <T,
        ufloat,
        boost::numeric::conversion_traits<T, ufloat>,
        boost::numeric::def_overflow_handler,
        ExactFloat2IntRounderPolicy<ufloat> > converter;

      return converter(val);
    }
#define RETHROW(Name)                           \
    catch (boost::numeric::Name&)               \
    {                                           \
      throw Name();                             \
    }
    RETHROW(negative_overflow)
    RETHROW(positive_overflow)
    RETHROW(bad_numeric_cast)
#undef RETHROW
  }

  /*----------------.
  | rounding_cast.  |
  `----------------*/

  template <typename T>
  T
  rounding_cast(ufloat val) throw (bad_numeric_cast)
  {
    try
    {
      static boost::numeric::converter
        <T,
        ufloat,
        boost::numeric::conversion_traits<T, ufloat>,
        boost::numeric::def_overflow_handler,
        boost::numeric::RoundEven<ufloat> > converter;

      return converter(val);
    }
#define RETHROW(Name)                           \
    catch (boost::numeric::Name&)               \
    {                                           \
      throw Name();                             \
    }
    RETHROW(negative_overflow)
    RETHROW(positive_overflow)
    RETHROW(bad_numeric_cast)
#undef RETHROW
  }

# define UFLOAT_CAST(Type)                                      \
  template                                                      \
  Type                                                          \
  numeric_cast<Type>(ufloat v) throw (bad_numeric_cast);        \
                                                                \
  template                                                      \
  Type                                                          \
  rounding_cast<Type>(ufloat v) throw (bad_numeric_cast);

  // Instantiate.
  UFLOAT_CASTS

#undef UFLOAT_CAST

} // namespace libport


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file discrete_distribution.hpp
 * \date 05/25/2014
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * \author Jan Issac (jan.issac@gmail.com)
 */
#pragma once

#include <Eigen/Core>

#include <vector>

#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/assertions.hpp>
#include <fl/distribution/interface/moments.hpp>
#include <fl/distribution/interface/standard_gaussian_mapping.hpp>

namespace fl
{

template <typename Variate, int Locations = Eigen::Dynamic>
class DiscreteDistribution
    : public Moments<typename FirstMomentOf<Variate>::Type>,
      public StandardGaussianMapping<Variate, 1>
{
public:
    typedef Moments<typename FirstMomentOf<Variate>::Type>  MomentsInterface;
    typedef typename MomentsInterface::Variate              Mean;
    typedef typename MomentsInterface::SecondMoment         Covariance;

    typedef Eigen::Array<Real,    Locations, 1> Function;
    typedef Eigen::Array<Variate, Locations, 1> LocationArray;

    typedef StandardGaussianMapping<Variate, 1> StdGaussianMapping;
    typedef typename StdGaussianMapping::StandardVariate StandardVariate;

public:
    /// constructor and destructor *********************************************
    explicit
    DiscreteDistribution(int size = MaxOf<Locations, 1>::value)
    {
        set_uniform(size);
    }

    virtual ~DiscreteDistribution() noexcept { }

    /// non-const functions ****************************************************

    // set ---------------------------------------------------------------------
    virtual void log_unnormalized_prob_mass(const Function& log_prob_mass)
    {
        // rescale for numeric stability
        log_prob_mass_ = log_prob_mass - log_prob_mass.maxCoeff();

        // copy to prob mass
        prob_mass_ = log_prob_mass_.exp();
        Real sum = prob_mass_.sum();

        // normalize
        prob_mass_ /= sum;
        log_prob_mass_ -= std::log(sum);

        // compute cdf
        cumul_distr_.resize(log_prob_mass_.size());

        cumul_distr_[0] = prob_mass_[0];
        for(int i = 1; i < cumul_distr_.size(); i++)
        {
            cumul_distr_[i] = cumul_distr_[i-1] + prob_mass_[i];
        }

        // resize locations
        locations_.resize(log_prob_mass_.size());
    }

    virtual void delta_log_prob_mass(const Function& delta)
    {
        log_unnormalized_prob_mass(log_prob_mass_ + delta);
    }

    virtual void set_uniform(int new_size = -1)
    {
        if (new_size == -1) new_size = size();

        log_unnormalized_prob_mass(Function::Zero(new_size));
    }

    virtual Variate& location(int i)
    {
        return locations_[i];
    }

    template <typename Distribution>
    void from_distribution(const Distribution& distribution, const int& new_size)
    {
        // we first create a local array to sample to. this way, if this
        // is passed as an argument the locations and pmf are not overwritten
        // while sampling
        LocationArray new_locations(new_size);

        for(int i = 0; i < new_size; i++)
        {
            new_locations[i] = distribution.sample();
        }

        set_uniform(new_size);
        locations_ = new_locations;
    }

    /// const functions ********************************************************

    // sampling ----------------------------------------------------------------
    virtual Variate map_standard_normal(const StandardVariate& gaussian_sample,
                                        int& index) const
    {
        StandardVariate scaled_sample = gaussian_sample / std::sqrt(2.0);
        StandardVariate uniform_sample = 0.5 * (1.0 + std::erf(scaled_sample));

        return map_standard_uniform(uniform_sample, index);
    }

    virtual Variate map_standard_uniform(const StandardVariate& uniform_sample,
                                         int& index) const
    {
        index = 0;
        for (index = 0; index < cumul_distr_.size(); ++index)
        {
            if (cumul_distr_[index] >= uniform_sample) break;
        }

        return locations_[index];
    }

    using StdGaussianMapping::sample;

    virtual Variate sample(int& index) const
    {
        return map_standard_normal(this->standard_gaussian_.sample(), index);
    }

    virtual Variate map_standard_normal(const StandardVariate& gaussian_sample) const
    {
        int index;
        return map_standard_normal(gaussian_sample, index);
    }

    virtual Variate map_standard_uniform(const StandardVariate& uniform_sample) const
    {
        int index;
        return map_standard_uniform(uniform_sample, index);
    }

    // get ---------------------------------------------------------------------
    virtual const Variate& location(int i) const
    {
        return locations_[i];
    }

    virtual const LocationArray& locations() const
    {
        return locations_;
    }

    virtual Real log_prob_mass(const int& i) const
    {
        return log_prob_mass_(i);
    }

    virtual Function log_prob_mass() const
    {
        return log_prob_mass_;
    }

    virtual Real prob_mass(const int& i) const
    {
        return prob_mass_(i);
    }

    virtual Function prob_mass() const
    {
        return prob_mass_;
    }

    virtual int size() const
    {
        return locations_.size();
    }

    virtual int dimension() const
    {
        return locations_[0].rows();
    }

    // compute properties ------------------------------------------------------
    virtual const Mean& mean() const
    {
        mu_ = Mean::Zero(dimension());

        for(int i = 0; i < locations_.size(); i++)
        {
            mu_ += prob_mass(i) * locations_[i].template cast<Real>();
        }

        return mu_;
    }

    virtual const Variate& max() const
    {
        int max_index;
        log_prob_mass_.maxCoeff(&max_index);

        return locations_(max_index);
    }

    virtual const Covariance& covariance() const
    {
        Mean mu = mean();
        cov_ = Covariance::Zero(dimension(), dimension());
        for(int i = 0; i < locations_.size(); i++)
        {
            Mean delta = (locations_[i].template cast<Real>()-mu);
            cov_ += prob_mass(i) * delta * delta.transpose();
        }

        return cov_;
    }

    virtual Real entropy() const
    {
        return - log_prob_mass_.cwiseProduct(prob_mass_).sum();
    }

    // implements KL(p||u) where p is this distr, and u is the uniform distr
    virtual Real kl_given_uniform() const
    {
        return std::log(Real(size())) - entropy();
    }

protected:
    /// member variables *******************************************************
    LocationArray locations_;

    Function log_prob_mass_;
    Function prob_mass_;
    Function cumul_distr_;

    mutable Mean mu_;
    mutable Covariance cov_;
};

}


/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Third party includes
#include <boost/math/constants/constants.hpp>

// VOTCA includes
#include <votca/tools/constants.h>

// Local VOTCA includes
#include <votca/xtp/orbitals.h>

// Local private VOTCA includes
#include "spectrum.h"

namespace votca {
namespace xtp {

void Spectrum::Initialize(const tools::Property& user_options) {

  tools::Property options =
      LoadDefaultsAndUpdateWithUserOptions("xtp", user_options);

  _job_name = options.ifExistsReturnElseReturnDefault<std::string>("job_name",
                                                                   _job_name);

  // orbitals file or pure DFT output
  _orbfile = options.ifExistsReturnElseReturnDefault<std::string>(
      ".orbitals", _job_name + ".orb");

  _output_file = options.ifExistsReturnElseReturnDefault<std::string>(
      ".output", _job_name + "_spectrum.dat");

  _n_pt = options.get(".points").as<Index>();
  _lower = options.get(".lower").as<double>();
  _upper = options.get(".upper").as<double>();
  _fwhm = options.get(".fwhm").as<double>();

  _spectrum_type = options.get(".type").as<std::string>();
  _minexc = options.get(".minexc").as<Index>();
  _maxexc = options.get(".maxexc").as<Index>();
  _shiftby = options.get(".shift").as<double>();
}

bool Spectrum::Evaluate() {
  OPENMP::setMaxThreads(_nThreads);
  _log.setReportLevel(Log::current_level);
  _log.setMultithreading(true);

  _log.setCommonPreface("\n... ...");

  XTP_LOG(Log::error, _log)
      << "Calculating absorption spectrum plot " << _orbfile << std::flush;

  Orbitals orbitals;
  // load the QM data from serialized orbitals object
  XTP_LOG(Log::error, _log)
      << " Loading QM data from " << _orbfile << std::flush;
  orbitals.ReadFromCpt(_orbfile);

  // check if orbitals contains singlet energies and transition dipoles
  if (!orbitals.hasBSESinglets()) {
    throw std::runtime_error(
        "BSE singlet energies not stored in QM data file!");
  }

  if (!orbitals.hasTransitionDipoles()) {
    throw std::runtime_error(
        "BSE transition dipoles not stored in QM data file!");
  }

  const Eigen::VectorXd BSESingletEnergies =
      orbitals.BSESinglets().eigenvalues() * tools::conv::hrt2ev;
  const std::vector<Eigen::Vector3d>& TransitionDipoles =
      orbitals.TransitionDipoles();
  Eigen::VectorXd osc = orbitals.Oscillatorstrengths();

  if (_maxexc > Index(TransitionDipoles.size())) {
    _maxexc = Index(TransitionDipoles.size()) - 1;
  }

  Index n_exc = _maxexc - _minexc + 1;
  XTP_LOG(Log::error, _log)
      << " Considering " << n_exc << " excitation with max energy "
      << BSESingletEnergies(_maxexc) << " eV / min wave length "
      << evtonm(BSESingletEnergies[_maxexc - 1]) << " nm" << std::flush;

  /*
   *
   * For a single excitation, broaden by Lineshape function L(v-W)
   *    eps(v) = f * L(v-W)
   *
   * where
   *       v: energy
   *       f: oscillator strength in dipole-length gauge
   *       W: excitation energy
   *
   * Lineshape function depend on FWHM and can be
   *
   *      Gaussian
   *          L(v-W) = 1/(sqrt(2pi)sigma) * exp(-0.5 (v-W)^2/sigma^2
   *
   *
   *            with sigma: derived from FWHM (FWHM/2.3548)
   *
   *     Lorentzian
   *          L(v-W) = 1/pi * 0.5 FWHM/( (v-w)^2 + 0.25*FWHM^2 )
   *
   * Full spectrum is superposition of individual spectra.
   *
   *  Alternatively, one can calculate the imaginary part of the
   *  frequency-dependent dielectric function
   *
   *   IM(eps(v)) ~ 1/v^2 * W^2 * |td|^2 * L(v-W)
   *              = 1/v^2 * W   * f      * L(v-W)
   *
   *
   */

  std::ofstream ofs(_output_file, std::ofstream::out);

  if (_spectrum_type == "energy") {
    ofs << "# E(eV)    epsGaussian    IM(eps)Gaussian   epsLorentz    "
           "Im(esp)Lorentz\n";
    for (Index i_pt = 0; i_pt <= _n_pt; i_pt++) {

      double e = (_lower + double(i_pt) * (_upper - _lower) / double(_n_pt));

      double eps_Gaussian = 0.0;
      double imeps_Gaussian = 0.0;
      double eps_Lorentzian = 0.0;
      double imeps_Lorentzian = 0.0;

      for (Index i_exc = _minexc; i_exc <= _maxexc; i_exc++) {
        eps_Gaussian +=
            osc[i_exc] *
            Gaussian(e, BSESingletEnergies(i_exc) + _shiftby, _fwhm);
        imeps_Gaussian += osc[i_exc] * BSESingletEnergies(i_exc) *
                          Gaussian(e, BSESingletEnergies(i_exc), _fwhm);
        eps_Lorentzian +=
            osc[i_exc] * Lorentzian(e, BSESingletEnergies(i_exc), _fwhm);
        imeps_Lorentzian += osc[i_exc] * BSESingletEnergies(i_exc) *
                            Lorentzian(e, BSESingletEnergies(i_exc), _fwhm);
      }

      ofs << e << "    " << eps_Gaussian << "   " << imeps_Gaussian << "   "
          << eps_Lorentzian << "   " << imeps_Lorentzian << std::endl;
    }

    XTP_LOG(Log::error, _log)
        << " Spectrum in energy range from  " << _lower << " to " << _upper
        << " eV and with broadening of FWHM " << _fwhm
        << " eV written to file  " << _output_file << std::flush;
  }

  if (_spectrum_type == "wavelength") {

    ofs << "# lambda(nm)    epsGaussian    IM(eps)Gaussian   epsLorentz    "
           "Im(esp)Lorentz\n";
    for (Index i_pt = 0; i_pt <= _n_pt; i_pt++) {

      double lambda =
          (_lower + double(i_pt) * (_upper - _lower) / double(_n_pt));
      double eps_Gaussian = 0.0;
      double imeps_Gaussian = 0.0;
      double eps_Lorentzian = 0.0;
      double imeps_Lorentzian = 0.0;

      for (Index i_exc = _minexc; i_exc <= _maxexc; i_exc++) {
        double exc_lambda = nmtoev(BSESingletEnergies(i_exc) + _shiftby);
        eps_Gaussian += osc[i_exc] * Gaussian(lambda, exc_lambda, _fwhm);
        imeps_Gaussian +=
            osc[i_exc] * exc_lambda * Gaussian(lambda, exc_lambda, _fwhm);
        eps_Lorentzian += osc[i_exc] * Lorentzian(lambda, exc_lambda, _fwhm);
        imeps_Lorentzian +=
            osc[i_exc] * exc_lambda * Lorentzian(lambda, exc_lambda, _fwhm);
      }

      ofs << lambda << "    " << eps_Gaussian << "   " << imeps_Gaussian
          << "   " << eps_Lorentzian << "   " << imeps_Lorentzian << std::endl;
    }
    XTP_LOG(Log::error, _log)
        << " Spectrum in wavelength range from  " << _lower << " to " << _upper
        << " nm and with broadening of FWHM " << _fwhm
        << " nm written to file  " << _output_file << std::flush;
  }

  ofs.close();
  return true;
}

double Spectrum::Lorentzian(double x, double center, double fwhm) {
  return 0.5 * fwhm / (std::pow(x - center, 2) + 0.25 * fwhm * fwhm) /
         boost::math::constants::pi<double>();
}

double Spectrum::Gaussian(double x, double center, double fwhm) {
  // FWHM = 2*sqrt(2 ln2) sigma = 2.3548 sigma
  double sigma = fwhm / 2.3548;
  return std::exp(-0.5 * std::pow((x - center) / sigma, 2)) / sigma /
         sqrt(2.0 * boost::math::constants::pi<double>());
}

double Spectrum::evtonm(double eV) { return 1241.0 / eV; }

double Spectrum::evtoinvcm(double eV) { return 8065.73 * eV; }

double Spectrum::nmtoinvcm(double nm) { return 1241.0 * 8065.73 / nm; }

double Spectrum::invcmtonm(double invcm) { return 1.0e7 / invcm; }

double Spectrum::nmtoev(double nm) { return 1241.0 / nm; }

}  // namespace xtp
}  // namespace votca


///////////////////////////////////////////////////////////////////////////////
// weighted_peaks_over_threshold.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. 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_ACCUMULATORS_STATISTICS_WEIGHTED_PEAKS_OVER_THRESHOLD_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_PEAKS_OVER_THRESHOLD_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <numeric>
#include <functional>
#include <boost/throw_exception.hpp>
#include <boost/range.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>
#include <boost/accumulators/statistics/peaks_over_threshold.hpp> // for named parameters pot_threshold_value and pot_threshold_probability
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/tail_variate.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{

    ///////////////////////////////////////////////////////////////////////////////
    // weighted_peaks_over_threshold_impl
    //  works with an explicit threshold value and does not depend on order statistics of weighted samples
    /**
        @brief Weighted Peaks over Threshold Method for Weighted Quantile and Weighted Tail Mean Estimation

        @sa peaks_over_threshold_impl

        @param quantile_probability
        @param pot_threshold_value
    */
    template<typename Sample, typename Weight, typename LeftRight>
    struct weighted_peaks_over_threshold_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Weight, Sample>::result_type weighted_sample;
        typedef typename numeric::functional::average<weighted_sample, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef boost::tuple<float_type, float_type, float_type> result_type;

        template<typename Args>
        weighted_peaks_over_threshold_impl(Args const &args)
          : sign_((is_same<LeftRight, left>::value) ? -1 : 1)
          , mu_(sign_ * numeric::average(args[sample | Sample()], (std::size_t)1))
          , sigma2_(numeric::average(args[sample | Sample()], (std::size_t)1))
          , w_sum_(numeric::average(args[weight | Weight()], (std::size_t)1))
          , threshold_(sign_ * args[pot_threshold_value])
          , fit_parameters_(boost::make_tuple(0., 0., 0.))
          , is_dirty_(true)
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            this->is_dirty_ = true;

            if (this->sign_ * args[sample] > this->threshold_)
            {
                this->mu_ += args[weight] * args[sample];
                this->sigma2_ += args[weight] * args[sample] * args[sample];
                this->w_sum_ += args[weight];
            }
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty_)
            {
                this->is_dirty_ = false;

                this->mu_ = this->sign_ * numeric::average(this->mu_, this->w_sum_);
                this->sigma2_ = numeric::average(this->sigma2_, this->w_sum_);
                this->sigma2_ -= this->mu_ * this->mu_;

                float_type threshold_probability = numeric::average(sum_of_weights(args) - this->w_sum_, sum_of_weights(args));

                float_type tmp = numeric::average(( this->mu_ - this->threshold_ )*( this->mu_ - this->threshold_ ), this->sigma2_);
                float_type xi_hat = 0.5 * ( 1. - tmp );
                float_type beta_hat = 0.5 * ( this->mu_ - this->threshold_ ) * ( 1. + tmp );
                float_type beta_bar = beta_hat * std::pow(1. - threshold_probability, xi_hat);
                float_type u_bar = this->threshold_ - beta_bar * ( std::pow(1. - threshold_probability, -xi_hat) - 1.)/xi_hat;
                this->fit_parameters_ = boost::make_tuple(u_bar, beta_bar, xi_hat);
            }

            return this->fit_parameters_;
        }

    private:
        short sign_;                         // for left tail fitting, mirror the extreme values
        mutable float_type mu_;              // mean of samples above threshold
        mutable float_type sigma2_;          // variance of samples above threshold
        mutable float_type w_sum_;           // sum of weights of samples above threshold
        float_type threshold_;
        mutable result_type fit_parameters_; // boost::tuple that stores fit parameters
        mutable bool is_dirty_;
    };

    ///////////////////////////////////////////////////////////////////////////////
    // weighted_peaks_over_threshold_prob_impl
    //  determines threshold from a given threshold probability using order statistics
    /**
        @brief Peaks over Threshold Method for Quantile and Tail Mean Estimation

        @sa weighted_peaks_over_threshold_impl

        @param quantile_probability
        @param pot_threshold_probability
    */
    template<typename Sample, typename Weight, typename LeftRight>
    struct weighted_peaks_over_threshold_prob_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Weight, Sample>::result_type weighted_sample;
        typedef typename numeric::functional::average<weighted_sample, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef boost::tuple<float_type, float_type, float_type> result_type;

        template<typename Args>
        weighted_peaks_over_threshold_prob_impl(Args const &args)
          : sign_((is_same<LeftRight, left>::value) ? -1 : 1)
          , mu_(sign_ * numeric::average(args[sample | Sample()], (std::size_t)1))
          , sigma2_(numeric::average(args[sample | Sample()], (std::size_t)1))
          , threshold_probability_(args[pot_threshold_probability])
          , fit_parameters_(boost::make_tuple(0., 0., 0.))
          , is_dirty_(true)
        {
        }

        void operator ()(dont_care)
        {
            this->is_dirty_ = true;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty_)
            {
                this->is_dirty_ = false;

                float_type threshold = sum_of_weights(args)
                             * ( ( is_same<LeftRight, left>::value ) ? this->threshold_probability_ : 1. - this->threshold_probability_ );

                std::size_t n = 0;
                Weight sum = Weight(0);

                while (sum < threshold)
                {
                    if (n < static_cast<std::size_t>(tail_weights(args).size()))
                    {
                        mu_ += *(tail_weights(args).begin() + n) * *(tail(args).begin() + n);
                        sigma2_ += *(tail_weights(args).begin() + n) * *(tail(args).begin() + n) * (*(tail(args).begin() + n));
                        sum += *(tail_weights(args).begin() + n);
                        n++;
                    }
                    else
                    {
                        if (std::numeric_limits<float_type>::has_quiet_NaN)
                        {
                            return boost::make_tuple(
                                std::numeric_limits<float_type>::quiet_NaN()
                              , std::numeric_limits<float_type>::quiet_NaN()
                              , std::numeric_limits<float_type>::quiet_NaN()
                            );
                        }
                        else
                        {
                            std::ostringstream msg;
                            msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                            boost::throw_exception(std::runtime_error(msg.str()));
                            return boost::make_tuple(Sample(0), Sample(0), Sample(0));
                        }
                    }
                }

                float_type u = *(tail(args).begin() + n - 1) * this->sign_;

                this->mu_ = this->sign_ * numeric::average(this->mu_, sum);
                this->sigma2_ = numeric::average(this->sigma2_, sum);
                this->sigma2_ -= this->mu_ * this->mu_;

                if (is_same<LeftRight, left>::value)
                    this->threshold_probability_ = 1. - this->threshold_probability_;

                float_type tmp = numeric::average(( this->mu_ - u )*( this->mu_ - u ), this->sigma2_);
                float_type xi_hat = 0.5 * ( 1. - tmp );
                float_type beta_hat = 0.5 * ( this->mu_ - u ) * ( 1. + tmp );
                float_type beta_bar = beta_hat * std::pow(1. - threshold_probability_, xi_hat);
                float_type u_bar = u - beta_bar * ( std::pow(1. - threshold_probability_, -xi_hat) - 1.)/xi_hat;
                this->fit_parameters_ = boost::make_tuple(u_bar, beta_bar, xi_hat);

            }

            return this->fit_parameters_;
        }

    private:
        short sign_;                                // for left tail fitting, mirror the extreme values
        mutable float_type mu_;                     // mean of samples above threshold u
        mutable float_type sigma2_;                 // variance of samples above threshold u
        mutable float_type threshold_probability_;
        mutable result_type fit_parameters_;        // boost::tuple that stores fit parameters
        mutable bool is_dirty_;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_peaks_over_threshold
//
namespace tag
{
    template<typename LeftRight>
    struct weighted_peaks_over_threshold
      : depends_on<sum_of_weights>
      , pot_threshold_value
    {
        /// INTERNAL ONLY
        typedef accumulators::impl::weighted_peaks_over_threshold_impl<mpl::_1, mpl::_2, LeftRight> impl;
    };

    template<typename LeftRight>
    struct weighted_peaks_over_threshold_prob
      : depends_on<sum_of_weights, tail_weights<LeftRight> >
      , pot_threshold_probability
    {
        /// INTERNAL ONLY
        typedef accumulators::impl::weighted_peaks_over_threshold_prob_impl<mpl::_1, mpl::_2, LeftRight> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_peaks_over_threshold
//
namespace extract
{
    extractor<tag::abstract_peaks_over_threshold> const weighted_peaks_over_threshold = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_peaks_over_threshold)
}

using extract::weighted_peaks_over_threshold;

// weighted_peaks_over_threshold<LeftRight>(with_threshold_value) -> weighted_peaks_over_threshold<LeftRight>
template<typename LeftRight>
struct as_feature<tag::weighted_peaks_over_threshold<LeftRight>(with_threshold_value)>
{
    typedef tag::weighted_peaks_over_threshold<LeftRight> type;
};

// weighted_peaks_over_threshold<LeftRight>(with_threshold_probability) -> weighted_peaks_over_threshold_prob<LeftRight>
template<typename LeftRight>
struct as_feature<tag::weighted_peaks_over_threshold<LeftRight>(with_threshold_probability)>
{
    typedef tag::weighted_peaks_over_threshold_prob<LeftRight> type;
};

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


// VMDモーションを間引く

#include <vector>
#include <Eigen/Core>
#include "VMD.h"
#include "interpolate.h"
#include "reducevmd.h"

#define _USE_MATH_DEFINES
#include <math.h>

// head番目からtail番目のボーンキーフレームのうち、残すべきものを再帰的に探して返す。
// ただし返値にtail番目のボーンは含まれないので、コール元で追加する必要がある。
vector<VMD_Frame> reduce_bone_frame_recursive(const vector<VMD_Frame>& v, int head, int tail, float threshold_pos, float threshold_rot, bool bezier)
{
  float max_pos_err = 0.0;
  float max_rot_err = 0.0;
  int max_idx_pos = 0;
  int max_idx_rot = 0;
  VMD_Frame head_frame = v[head];
  VMD_Frame tail_frame = v[tail];
  const int bezier_interpolation_limit = 60;
  if (bezier && tail - head < bezier_interpolation_limit) {
    optimize_bezier_parameter(tail_frame, v, head, tail);
  }

  // headフレームの値とtailフレームの値によって決まる補完曲線(直線)から最も離れた(誤差の大きい)フレームを探す
  for (int i = head + 1; i < tail; i++) {
    VMD_Frame f = interpolate_frame(head_frame, tail_frame, i, bezier);
    float pos_err = (f.position - v[i].position).norm();
    if (pos_err > max_pos_err) {
      max_idx_pos = i;
      max_pos_err = pos_err;
    }
    float rot_err = fabs(f.rotation.angularDistance(v[i].rotation) * 180 / M_PI);
    if (rot_err > max_rot_err) {
      max_idx_rot = i;
      max_rot_err = rot_err;
    }
  }

  // 補間曲線から最も離れたフレームの誤差が閾値を超えていたら、そのフレーム(max_idx_*)を残し、
  // [head, max_idx_*] と [max_idx_*, tail] のそれぞれの区間を再帰的に探す。
  vector<VMD_Frame> v1;
  if (max_pos_err > threshold_pos) {
    v1 = reduce_bone_frame_recursive(v, head, max_idx_pos, threshold_pos, threshold_rot, bezier);
    vector<VMD_Frame> v2 = reduce_bone_frame_recursive(v, max_idx_pos, tail, threshold_pos, threshold_rot, bezier);
    v1.insert(v1.end(), v2.begin(), v2.end());
  } else {
    if (max_rot_err > threshold_rot) {
      v1 = reduce_bone_frame_recursive(v, head, max_idx_rot, threshold_pos, threshold_rot, bezier);
      vector<VMD_Frame> v2 = reduce_bone_frame_recursive(v, max_idx_rot, tail, threshold_pos, threshold_rot, bezier);
      v1.insert(v1.end(), v2.begin(), v2.end());
    } else {
      v1.push_back(tail_frame);
    }
  }
  return v1;
}

// head番目からtail番目のボーンキーフレームのうち、残すべきものを探して返す。
vector<VMD_Frame> reduce_bone_frame(const vector<VMD_Frame>& v, int head, int tail, float threshold_pos, float threshold_rot, bool bezier)
{
  if (threshold_pos < 0 || threshold_rot < 0) {
    vector<VMD_Frame> v1(v);
    return v1;
  }

  vector<VMD_Frame> v1 = reduce_bone_frame_recursive(v, head, tail, threshold_pos, threshold_rot, bezier);
  v1.insert(v1.begin(), v.front());

  return v1;
}

// head番目からtail番目の表情キーフレームのうち、残すべきものを再帰的に探して返す。
// ただし返値にtail番目のフレームは含まれないので、コール元で追加する必要がある。
vector<VMD_Morph> reduce_morph_frame_recursive(const vector<VMD_Morph>& v, int head, int tail, float threshold)
{
  float max = 0.0;
  int max_idx = 0;
  int total = tail - head;
  for (int i = head + 1; i < tail; i++) {
    float iv = v[head].weight + (v[tail].weight - v[head].weight) * (i - head) / total;
    float e = abs(iv - v[i].weight);
    if (e > max) {
      max_idx = i;
      max = e;
    }
  }

  vector<VMD_Morph> v1;
  if (max > threshold) {
    v1 = reduce_morph_frame_recursive(v, head, max_idx, threshold);
    vector<VMD_Morph> v2 = reduce_morph_frame_recursive(v, max_idx, tail, threshold);
    v1.insert(v1.end(), v2.begin(), v2.end());
  } else {
    v1.push_back(v[head]);
  }
  return v1;
}

// head番目からtail番目の表情キーフレームのうち、残すべきものを探して返す。
vector<VMD_Morph> reduce_morph_frame(const vector<VMD_Morph>& v, int head, int tail, float threshold)
{
  if (threshold < 0) {
    vector<VMD_Morph> v1(v);
    return v1;
  }

  vector<VMD_Morph> v1 = reduce_morph_frame_recursive(v, head, tail, threshold);
  v1.push_back(v.back());
  return v1;
}


//
// VariationalBayesEstimatorOnATM.hpp
//
// Copyright (c) 2017 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#ifndef VBATM
#define VBATM

#include<stdlib.h>
#include<math.h>
#include<cmath>
#include<iostream>
#include<vector>
#include<numeric>
#include<memory>
#include<random>
#include<iomanip>
#include<fstream>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include"BOWFileParser.hpp"
#include"AuthorFileParser.hpp"
#include"utils.hpp"

enum BetaUpdateManner{
    SYMMETRY, ASYMMETRY
};

class VariationalBayesEstimatorOnATM{
protected:
    const std::vector<std::vector<unsigned int> > &_frequencyMatrix;
    const std::vector<std::vector<unsigned int> > &_docVoca;
    const unsigned int _K, _V, _D, _M;
    const std::vector<std::vector<unsigned int> > &_docAuth;
    double _convergenceDiterminationRate;
    std::vector<std::vector<std::vector<std::vector<double> > > > _qzy;
    std::vector<std::vector<double> > _thetaEx, _phiEx;
    std::vector<double> _alpha, _beta;
    double _alphaSum, _betaSum;
    std::vector<std::vector<double> > _alphaTimeSeries, _betaTimeSeries;
    std::vector<double> _nm, _nk;
    std::vector<std::vector<double> > _nmk, _nkv;
    double _variationalLowerBound;
    std::vector<double> _VLBTimeSeries;
public:
    VariationalBayesEstimatorOnATM(const BOWFileParser &parser, const AuthorFileParser &aParser, const unsigned int K, const double convergenceDiterminationRate);
    virtual ~VariationalBayesEstimatorOnATM();
    virtual void initializeParam();
    virtual void initializeHyperParam();
    virtual void calculateEx();
    virtual void calculateHyperParamSum();
    virtual void updateQzy();
    virtual void updateNEx();
    virtual void updateBeta(BetaUpdateManner manner);
    virtual void updateHyperParameters();
    virtual double calculateVariationalLowerBound()const;
    virtual void writeParameter(std::string thetaFilename, std::string phiFilename, std::string alphaFilename, std::string betaFilename)const;
    virtual void writeVariationalLowerBound(std::string VLBFilename, std::string VLBTimeSeriesFilename)const;
    virtual void runIteraions();
};

#endif


/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   covariance.cpp
 * @brief  Covariance estimation methods for ICP results
 * @author Jose Luis Blanco Claraco
 * @date   Jun 9, 2020
 */

#include <mp2p_icp/covariance.h>
#include <mp2p_icp/errorTerms.h>
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/math/num_jacobian.h>

#include <Eigen/Dense>

using namespace mp2p_icp;

mrpt::math::CMatrixDouble66 mp2p_icp::covariance(
    const Pairings& in, const mrpt::poses::CPose3D& finalAlignSolution,
    const CovarianceParameters& param)
{
    // If we don't have pairings, we can't provide an estimation:
    if (in.empty())
    {
        mrpt::math::CMatrixDouble66 cov;
        cov.setDiagonal(1e6);
        return cov;
    }

    mrpt::math::CMatrixDouble61 xInitial;
    xInitial[0] = finalAlignSolution.x();
    xInitial[1] = finalAlignSolution.y();
    xInitial[0] = finalAlignSolution.x();
    xInitial[3] = finalAlignSolution.yaw();
    xInitial[4] = finalAlignSolution.pitch();
    xInitial[5] = finalAlignSolution.roll();

    mrpt::math::CMatrixDouble61 xIncrs;
    for (int i = 0; i < 3; i++) xIncrs[i] = param.finDif_xyz;
    for (int i = 0; i < 3; i++) xIncrs[3 + i] = param.finDif_angles;

    struct LambdaParams
    {
    };

    LambdaParams lmbParams;

    auto errorLambda = [&](const mrpt::math::CMatrixDouble61& x,
                           const LambdaParams&,
                           mrpt::math::CVectorDouble& err) {
        mrpt::poses::CPose3D pose;
        pose.setFromValues(x[0], x[1], x[2], x[3], x[4], x[5]);

        const auto nPt2Pt = in.paired_pt2pt.size();
        const auto nPt2Ln = in.paired_pt2ln.size();
        const auto nPt2Pl = in.paired_pt2pl.size();
        const auto nPl2Pl = in.paired_pl2pl.size();
        const auto nLn2Ln = in.paired_ln2ln.size();

        const auto nErrorTerms =
            (nPt2Pt + nPl2Pl + nPt2Ln + nPt2Pl) * 3 + nLn2Ln * 4;
        ASSERT_(nErrorTerms > 0);
        err.resize(nErrorTerms);

        // Point-to-point:
        for (size_t idx_pt = 0; idx_pt < nPt2Pt; idx_pt++)
        {
            // Error:
            const auto&                       p = in.paired_pt2pt[idx_pt];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_point2point(p, pose);
            err.block<3, 1>(idx_pt * 3, 0) = ret.asEigen();
        }
        auto base_idx = nPt2Pt * 3;

        // Point-to-line
        for (size_t idx_pt = 0; idx_pt < nPt2Ln; idx_pt++)
        {
            // Error
            const auto&                       p = in.paired_pt2ln[idx_pt];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_point2line(p, pose);
            err.block<3, 1>(base_idx + idx_pt * 3, 0) = ret.asEigen();
        }
        base_idx += nPt2Ln * 3;

        // Line-to-Line
        // Minimum angle to approach zero
        for (size_t idx_ln = 0; idx_ln < nLn2Ln; idx_ln++)
        {
            const auto&                       p = in.paired_ln2ln[idx_ln];
            mrpt::math::CVectorFixedDouble<4> ret =
                mp2p_icp::error_line2line(p, pose);
            err.block<4, 1>(base_idx + idx_ln * 4, 0) = ret.asEigen();
        }
        base_idx += nLn2Ln;

        // Point-to-plane:
        for (size_t idx_pl = 0; idx_pl < nPt2Pl; idx_pl++)
        {
            // Error:
            const auto&                       p = in.paired_pt2pl[idx_pl];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_point2plane(p, pose);
            err.block<3, 1>(idx_pl * 3 + base_idx, 0) = ret.asEigen();
        }
        base_idx += nPt2Pl * 3;

        // Plane-to-plane (only direction of normal vectors):
        for (size_t idx_pl = 0; idx_pl < nPl2Pl; idx_pl++)
        {
            // Error term:
            const auto&                       p = in.paired_pl2pl[idx_pl];
            mrpt::math::CVectorFixedDouble<3> ret =
                mp2p_icp::error_plane2plane(p, pose);
            err.block<3, 1>(idx_pl * 3 + base_idx, 0) = ret.asEigen();
        }
    };

    // Do NOT use "Eigen::MatrixXd", it may have different alignment
    // requirements than MRPT matrices:
    mrpt::math::CMatrixDouble jacob;
    mrpt::math::estimateJacobian(
        xInitial,
        std::function<void(
            const mrpt::math::CMatrixDouble61&, const LambdaParams&,
            mrpt::math::CVectorDouble&)>(errorLambda),
        xIncrs, lmbParams, jacob);

    const mrpt::math::CMatrixDouble66 hessian(
        jacob.asEigen().transpose() * jacob.asEigen());

    const mrpt::math::CMatrixDouble66 cov = hessian.inverse_LLt();

    return cov;
}

// other ideas?
// See: http://censi.mit.edu/pub/research/2007-icra-icpcov-slides.pdf


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/poses/CPose2DInterpolator.h>
#include <mrpt/serialization/stl_serialization.h>
#include <Eigen/Dense>
#include "CPoseInterpolatorBase.hpp"  // templ impl

using namespace mrpt::poses;

IMPLEMENTS_SERIALIZABLE(CPose2DInterpolator, CSerializable, mrpt::poses)

uint8_t CPose2DInterpolator::serializeGetVersion() const { return 0; }
void CPose2DInterpolator::serializeTo(mrpt::serialization::CArchive& out) const
{
	out << m_path;
}
void CPose2DInterpolator::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			in >> m_path;
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

namespace mrpt::poses
{
// Specialization for DIM=2
template <>
void CPoseInterpolatorBase<2>::impl_interpolation(
	const TTimePosePair& p1, const TTimePosePair& p2, const TTimePosePair& p3,
	const TTimePosePair& p4, const TInterpolatorMethod method,
	const mrpt::Clock::time_point& t, pose_t& out_interp) const
{
	using mrpt::math::TPose2D;
	using doubleDuration = std::chrono::duration<double>;
	doubleDuration durationT(t.time_since_epoch());
	double td = durationT.count();
	mrpt::math::CVectorFixedDouble<4> ts;
	ts[0] =
		std::chrono::duration_cast<doubleDuration>(p1.first.time_since_epoch())
			.count();
	ts[1] =
		std::chrono::duration_cast<doubleDuration>(p2.first.time_since_epoch())
			.count();
	ts[2] =
		std::chrono::duration_cast<doubleDuration>(p3.first.time_since_epoch())
			.count();
	ts[3] =
		std::chrono::duration_cast<doubleDuration>(p4.first.time_since_epoch())
			.count();

	mrpt::math::CVectorFixedDouble<4> X, Y, yaw;
	X[0] = p1.second.x;
	Y[0] = p1.second.y;
	yaw[0] = p1.second.phi;
	X[1] = p2.second.x;
	Y[1] = p2.second.y;
	yaw[1] = p2.second.phi;
	X[2] = p3.second.x;
	Y[2] = p3.second.y;
	yaw[2] = p3.second.phi;
	X[3] = p4.second.x;
	Y[3] = p4.second.y;
	yaw[3] = p4.second.phi;

	unwrap2PiSequence(yaw);

	// Target interpolated values:
	switch (method)
	{
		case imSpline:
		{
			// ---------------------------------------
			//    SPLINE INTERPOLATION
			// ---------------------------------------
			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
			out_interp.phi = math::spline(td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imLinear2Neig:
		{
			out_interp.x =
				math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
			out_interp.y =
				math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
			out_interp.phi = math::interpolate2points(
				td, ts[1], yaw[1], ts[2], yaw[2], true);  // Wrap 2pi
		}
		break;

		case imLinear4Neig:
		{
			out_interp.x =
				math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, X);
			out_interp.y =
				math::leastSquareLinearFit<double, decltype(ts), 4>(td, ts, Y);
			out_interp.phi =
				math::leastSquareLinearFit<double, decltype(ts), 4>(
					td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imSSLLLL:
		{
			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
			out_interp.phi =
				math::leastSquareLinearFit<double, decltype(ts), 4>(
					td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imSSLSLL:
		{
			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
			out_interp.phi = math::spline(td, ts, yaw, true);  // Wrap 2pi
		}
		break;

		case imLinearSlerp:
		{
			const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
			const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
			out_interp.phi = yaw[1] + ratio * Aang;

			out_interp.x =
				math::interpolate2points(td, ts[1], X[1], ts[2], X[2]);
			out_interp.y =
				math::interpolate2points(td, ts[1], Y[1], ts[2], Y[2]);
		}
		break;

		case imSplineSlerp:
		{
			const double ratio = (td - ts[1]) / (ts[2] - ts[1]);
			const double Aang = mrpt::math::angDistance(yaw[1], yaw[2]);
			out_interp.phi = yaw[1] + ratio * Aang;

			out_interp.x = math::spline(td, ts, X);
			out_interp.y = math::spline(td, ts, Y);
		}
		break;

		default:
			THROW_EXCEPTION("Unknown value for interpolation method!");
	};  // end switch
}

// Explicit instantations:
template class CPoseInterpolatorBase<2>;
}  // namespace mrpt::poses