#ifndef TRIAL_ONLINE_QUANTILE_PSQUARE_HPP
#define TRIAL_ONLINE_QUANTILE_PSQUARE_HPP

///////////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2016 Bjorn Reese <breese@users.sourceforge.net>
//
// 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)
//
///////////////////////////////////////////////////////////////////////////////

// Jain and Chlamtac, "The [Piecewise-parabolic Prediction]-Square Algorithm
//   for Dynamic Calculation of Percentiles and Histograms without Storing
//   Observations", Communications of the ACM, 28(10), pp. 1076-1086, 1985.
//
// Raatikainen, "Sequential Procedure for Simultaneous Estimation of Several
//   Percentiles", Transactions of the Society for Computer Simulations, 7(1),
//   pp. 21-44, 1990.

#include <cstddef> // std::size_t
#include <vector>
#include <array>
#include <ratio>
#include <boost/mp11/list.hpp>
#include <boost/mp11/algorithm.hpp>
#include <trial/online/detail/type_traits.hpp>

namespace trial
{
namespace online
{
namespace quantile
{

using minimum_ratio = std::ratio<0, 1>;
using maximum_ratio = std::ratio<1, 1>;

template <typename T, typename... Quantiles>
class psquare
{
    static_assert(std::is_floating_point<T>::value, "T must be a floating-point type");
    static_assert((sizeof...(Quantiles) > 0), "There must be at least one quantile");

    using QuantileList = boost::mp11::mp_sort<boost::mp11::mp_list<minimum_ratio, Quantiles..., maximum_ratio>, std::ratio_less>;
    static_assert(boost::mp11::mp_all_of<QuantileList, detail::is_ratio>::value, "Quantiles must be ratios");

public:
    using value_type = T;
    using size_type = std::size_t;

    psquare() noexcept;
    psquare(const psquare&) noexcept = default;
    psquare(psquare&&) noexcept = default;
    psquare& operator= (const psquare&) noexcept = default;
    psquare& operator= (psquare&&) noexcept = default;

    void clear() noexcept;
    bool empty() const noexcept;
    size_type size() const noexcept;

    void push(value_type) noexcept;

    // Get value by type.
    // Select middle quantile parameter by default.
    template < typename Q = boost::mp11::mp_at_c<QuantileList, 1 + sizeof...(Quantiles) / 2> >
    value_type value() const noexcept;

    // Get value by index.
    template <std::size_t Index = 1 + sizeof...(Quantiles) / 2>
    value_type get() const noexcept;

    struct parameter_type
    {
        parameter_type(size_type position,
                       value_type height) noexcept;

        bool operator== (const parameter_type& other) const noexcept
        {
            return ((position == other.position) &&
                    (height == other.height));
        }

        size_type position;
        value_type height;
    };
    std::vector<parameter_type> parameters() const;
    void parameters(const std::vector<parameter_type>&) noexcept;

private:
    void initialize() noexcept;
    value_type linear(size_type, int) const noexcept;
    value_type parabolic(size_type, int) const noexcept;

private:
    static constexpr size_type quantile_length = sizeof...(Quantiles);
    static constexpr size_type parameter_length = 2 * quantile_length + 3;
    static constexpr value_type quantiles[quantile_length] = { (Quantiles::num / value_type(Quantiles::den))... };

    size_type count {0};
    std::array<size_type, parameter_length> positions;
    std::array<value_type, parameter_length> heights;
    std::array<value_type, parameter_length> desired_positions;
    std::array<value_type, parameter_length> constant_deltas;
};

// Convenience types
using median_ratio = std::ratio<1, 2>;
using lower_quartile_ratio = std::ratio<1, 4>;
using upper_quartile_ratio = std::ratio<3, 4>;

template <typename T> using psquare_median = psquare<T, median_ratio>;
template <typename T> using psquare_quartile = psquare<T, lower_quartile_ratio, median_ratio, upper_quartile_ratio>;

} // namespace quantile
} // namespace online
} // namespace trial

#include <trial/online/quantile/detail/psquare.ipp>

#endif // TRIAL_ONLINE_QUANTILE_PSQUARE_HPP


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      102511    D. Dirkx          First version of file.
 *      110120    D. Dirkx          Finalized for code check.
 *      110208    K. Kumar          Updated file header; corrected Doxygen comments; minor changes.
 *      110209    D. Dirkx          Minor changes.
 *      110209    K. Kumar          Minor changes.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120323    D. Dirkx          Removed set functions; moved functionality to constructor.
 *
 *    References
 *      E.H. Hirschel and C. Weiland, Selected Aerothermodynamic Design Problems of Hypersonic
 *          Flight Vehicles (chapter 5), Springer/AIAA, 2009.
 *      D. Dirkx, Continuous Shape Optimization of Entry Vehicles, MSc thesis, Delft University
 *          of Technology, 2011 (Unpublished).
 *
 *    Notes
 *
 */

#include <cmath>
#include <iostream>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/GeometricShapes/capsule.h"
#include "Tudat/Mathematics/GeometricShapes/conicalFrustum.h"
#include "Tudat/Mathematics/GeometricShapes/sphereSegment.h"
#include "Tudat/Mathematics/GeometricShapes/torus.h"

namespace tudat
{
namespace geometric_shapes
{

//! Default constructor.
Capsule::Capsule( const double noseRadius,
                  const double middleRadius,
                  const double rearLength,
                  const double rearAngle,
                  const double sideRadius )
{
    using std::sin;
    using std::cos;
    using mathematical_constants::PI;

    // Call set functions for number of single and composite surface geometries
    // with predetermined values.
    setNumberOfCompositeSurfaceGeometries( 0 );
    setNumberOfSingleSurfaceGeometries( 4 );

    // Set member shape variables
    noseRadius_ = noseRadius;
    middleRadius_ = middleRadius;
    rearLength_ = rearLength;
    rearAngle_ = rearAngle;
    sideRadius_ = sideRadius;

    // Determine and set extent of spherical nose part.
    double noseSphereAngle_ = asin( ( middleRadius_ - sideRadius_ )
                                    / ( noseRadius_ - sideRadius_ ) );

    // Create nose sphere.
    boost::shared_ptr< SphereSegment > noseSphere_ = boost::make_shared< SphereSegment >(
                noseRadius_, 0, 2 * PI, 0, noseSphereAngle_ );

    // Declare translation vector.
    Eigen::VectorXd translationVector_ = Eigen::VectorXd( 3 );
    translationVector_( 2 ) = 0.0;
    translationVector_( 1 ) = 0.0;
    translationVector_( 0 ) = - noseRadius_ * cos( noseSphereAngle_ );

    // Set nose translation vector.
    noseSphere_->setOffset( translationVector_ );

    // Set noseSphere_ in singleSurfaceList_.
    setSingleSurfaceGeometry( noseSphere_, 0 );

    // Create rear cone, fully revolved.
    boost::shared_ptr< ConicalFrustum > cone_ = boost::make_shared< ConicalFrustum >(
                rearAngle_, middleRadius_ - sideRadius_ * ( 1.0 - cos( rearAngle_ ) ),
                rearLength_ );

    // Set translation vector of cone.
    translationVector_( 0 ) = -sideRadius_ * ( sin( PI / 2.0 - noseSphereAngle_ )
                                               + sin ( -rearAngle_ ) );
    cone_->setOffset( translationVector_ );

    // Set cone in singleSurfaceList_.
    setSingleSurfaceGeometry( cone_, 1 );

    // Calculate end radius of cone.
    double endRadius_ = cone_->getStartRadius( ) + rearLength_ * tan( rearAngle_ );

    // Calculate rear sphere radius.
    double rearNoseRadius_ = endRadius_ / cos( -rearAngle_ );

    // Create rear sphere ( "end cap" ), fully revolved.
    boost::shared_ptr< SphereSegment > rearSphere_ = boost::make_shared< SphereSegment >(
                rearNoseRadius_, 0.0, 2.0 * PI, PI / 2.0 - rearAngle_, PI );

    // Set translation vector of rear sphere.
    translationVector_( 0 ) =  ( rearNoseRadius_ * sin( -rearAngle_ ) ) - rearLength_
            - ( sideRadius_ * ( sin( PI / 2.0 - noseSphereAngle_ ) + sin ( -rearAngle_ ) ) );
    rearSphere_->setOffset( translationVector_ );
    setSingleSurfaceGeometry( rearSphere_, 2 );

    // Create torus section of capsule.
    double torusMajorRadius_ = ( noseRadius_ - sideRadius_ ) * sin( noseSphereAngle_ );
    boost::shared_ptr< Torus > torus_ = boost::make_shared< Torus >(
       torusMajorRadius_, sideRadius_, 0.0, 2.0 * PI, PI / 2.0 - noseSphereAngle_, rearAngle_ );

    // Set translation vector of rear sphere.
    translationVector_( 0 ) = -cos( noseSphereAngle_ ) * sideRadius_;
    torus_->setOffset( translationVector_ );
    setSingleSurfaceGeometry( torus_, 3 );

    // Set rotation matrix fo each part to be compatible with flow direction in
    // aerodynamic analysis.
    Eigen::MatrixXd rotationMatrix = Eigen::MatrixXd( 3, 3 );
    double angle_ = PI / 2.0;
    rotationMatrix( 0, 0 ) = cos( angle_ );
    rotationMatrix( 0, 1 ) = 0.0;
    rotationMatrix( 0, 2 ) = sin( angle_ );
    rotationMatrix( 1, 0 ) = 0.0;
    rotationMatrix( 1, 1 ) = 1.0;
    rotationMatrix( 1, 2 ) = 0.0;
    rotationMatrix( 2, 0 ) = -sin( angle_ );
    rotationMatrix( 2, 1 ) = 0.0;
    rotationMatrix( 2, 2 ) = cos( angle_ );

    // Set rotation matrix for single surface geometries.
    for ( unsigned i = 0; i < numberOfSingleSurfaceGeometries_ ; i++ )
    {
        singleSurfaceGeometryList_[ i ]->setRotationMatrix( rotationMatrix );
    }
}

//! Overload ostream to print class information.
std::ostream &operator<<( std::ostream &stream, Capsule& capsule )
{
    using std::endl;

    stream << "This is a capsule." << endl;
    stream << "The defining parameters are: "<< endl
           << "Nose radius: " << capsule.getNoseRadius( ) << endl
           << "Mid radius: " << capsule.getMiddleRadius( ) << endl
           << "Rear length: " << capsule.getRearLength( ) << endl
           << "Rear angle: " << capsule.getRearAngle( ) << endl
           << "Side radius: " << capsule.getSideRadius( )<< endl;

    return stream;
}

} // namespace geometric_shapes
} // namespace tudat


/*
 * Copyright Nick Thompson, John Maddock 2020
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MATH_SPECIAL_DAUBECHIES_SCALING_HPP
#define BOOST_MATH_SPECIAL_DAUBECHIES_SCALING_HPP
#include <vector>
#include <array>
#include <cmath>
#include <thread>
#include <future>
#include <iostream>
#include <boost/math/special_functions/detail/daubechies_scaling_integer_grid.hpp>
#include <boost/math/filters/daubechies.hpp>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>
#include <boost/math/interpolators/detail/quintic_hermite_detail.hpp>
#include <boost/math/interpolators/detail/septic_hermite_detail.hpp>

namespace boost::math {

template<class Real, int p, int order>
std::vector<Real> daubechies_scaling_dyadic_grid(int64_t j_max)
{
    using std::isnan;
    using std::sqrt;
    auto c = boost::math::filters::daubechies_scaling_filter<Real, p>();
    Real scale = sqrt(static_cast<Real>(2))*(1 << order);
    for (auto & x : c)
    {
        x *= scale;
    }

    auto phik = detail::daubechies_scaling_integer_grid<Real, p, order>();

    // Maximum sensible j for 32 bit floats is j_max = 22:
    if (std::is_same_v<Real, float>)
    {
        if (j_max > 23)
        {
            throw std::logic_error("Requested dyadic grid more dense than number of representables on the interval.");
        }
    }
    std::vector<Real> v(2*p + (2*p-1)*((1<<j_max) -1), std::numeric_limits<Real>::quiet_NaN());
    v[0] = 0;
    v[v.size()-1] = 0;
    for (int64_t i = 0; i < static_cast<int64_t>(phik.size()); ++i) {
        v[i*(1uLL<<j_max)] = phik[i];
    }

    for (int64_t j = 1; j <= j_max; ++j)
    {
        int64_t k_max = v.size()/(int64_t(1) << (j_max-j));
        for (int64_t k = 1; k < k_max;  k += 2)
        {
            // Where this value will go:
            int64_t delivery_idx = k*(1uLL << (j_max-j));
            // This is a nice check, but we've tested this exhaustively, and it's an expensive check:
            //if (delivery_idx >= static_cast<int64_t>(v.size())) {
            //    std::cerr << "Delivery index out of range!\n";
            //    continue;
            //}
            Real term = 0;
            for (int64_t l = 0; l < static_cast<int64_t>(c.size()); ++l)
            {
                int64_t idx = k*(int64_t(1) << (j_max - j + 1)) - l*(int64_t(1) << j_max);
                if (idx < 0)
                {
                    break;
                }
                if (idx < static_cast<int64_t>(v.size()))
                {
                    term += c[l]*v[idx];
                }
            }
            // Again, another nice check:
            //if (!isnan(v[delivery_idx])) {
            //    std::cerr << "Delivery index already populated!, = " << v[delivery_idx] << "\n";
            //    std::cerr << "would overwrite with " << term << "\n";
            //}
            v[delivery_idx] = term;
        }
    }
    return v;
}

namespace detail {

template<class RandomAccessContainer>
class matched_holder {
public:
    using Real = typename RandomAccessContainer::value_type;

    matched_holder(RandomAccessContainer && y, RandomAccessContainer && dydx, int grid_refinements, Real x0) : x0_{x0}, y_{std::move(y)}, dy_{std::move(dydx)}
    {
        inv_h_ = (1 << grid_refinements);
        Real h = 1/inv_h_;
        for (auto & dy : dy_)
        {
            dy *= h;
        }
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        using std::sqrt;
        // This is the exact Holder exponent, but it's pessimistic almost everywhere!
        // It's only exactly right at dyadic rationals.
        //Real const alpha = 2 - log(1+sqrt(Real(3)))/log(Real(2));
        // We're gonna use alpha = 1/2, rather than 0.5500...
        Real s = (x-x0_)*inv_h_;
        Real ii = floor(s);
        auto i = static_cast<decltype(y_.size())>(ii);
        Real t = s - ii;
        Real dphi = dy_[i+1];
        Real diff = y_[i+1] - y_[i];
        return y_[i] + (2*dphi - diff)*t + 2*sqrt(t)*(diff-dphi);
    }

    int64_t bytes() const
    {
        return 2*y_.size()*sizeof(Real) + sizeof(this);
    }

private:
    Real x0_;
    Real inv_h_;
    RandomAccessContainer y_;
    RandomAccessContainer dy_;
};

template<class RandomAccessContainer>
class matched_holder_aos {
public:
    using Point = typename RandomAccessContainer::value_type;
    using Real = typename Point::value_type;

    matched_holder_aos(RandomAccessContainer && data, int grid_refinements, Real x0) : x0_{x0}, data_{std::move(data)}
    {
        inv_h_ = Real(1uLL << grid_refinements);
        Real h = 1/inv_h_;
        for (auto & datum : data_)
        {
            datum[1] *= h;
        }
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        using std::sqrt;
        Real s = (x-x0_)*inv_h_;
        Real ii = floor(s);
        auto i = static_cast<decltype(data_.size())>(ii);
        Real t = s - ii;
        Real y0 = data_[i][0];
        Real y1 = data_[i+1][0];
        Real dphi = data_[i+1][1];
        Real diff = y1 - y0;
        return y0 + (2*dphi - diff)*t + 2*sqrt(t)*(diff-dphi);
    }

    int64_t bytes() const
    {
        return data_.size()*data_[0].size()*sizeof(Real) + sizeof(this);
    }

private:
    Real x0_;
    Real inv_h_;
    RandomAccessContainer data_;
};

template<class RandomAccessContainer>
class linear_interpolation {
public:
    using Real = typename RandomAccessContainer::value_type;

    linear_interpolation(RandomAccessContainer && y, RandomAccessContainer && dydx, int grid_refinements) : y_{std::move(y)}, dydx_{std::move(dydx)}
    {
        s_ = (1 << grid_refinements);
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        Real y = x*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return (1-t)*y_[kk] + t*y_[kk+1];
    }

    inline Real prime(Real x) const
    {
        using std::floor;
        Real y = x*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return (1-t)*dydx_[kk] + t*dydx_[kk+1];
    }

    int64_t bytes() const
    {
        return (1 + y_.size() + dydx_.size())*sizeof(Real) + sizeof(y_) + sizeof(dydx_);
    }

private:
    Real s_;
    RandomAccessContainer y_;
    RandomAccessContainer dydx_;
};

template<class RandomAccessContainer>
class linear_interpolation_aos {
public:
    using Point = typename RandomAccessContainer::value_type;
    using Real = typename Point::value_type;

    linear_interpolation_aos(RandomAccessContainer && data, int grid_refinements, Real x0) : x0_{x0}, data_{std::move(data)}
    {
        s_ = Real(1uLL << grid_refinements);
    }

    inline Real operator()(Real x) const
    {
        using std::floor;
        Real y = (x-x0_)*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return (t != 0) ? (1-t)*data_[kk][0] + t*data_[kk+1][0] : data_[kk][0];
    }

    inline Real prime(Real x) const
    {
        using std::floor;
        Real y = (x-x0_)*s_;
        Real k = floor(y);

        int64_t kk = static_cast<int64_t>(k);
        Real t = y - k;
        return t != 0 ? (1-t)*data_[kk][1] + t*data_[kk+1][1] : data_[kk][1];
    }

    int64_t bytes() const
    {
        return sizeof(this) + data_.size()*data_[0].size()*sizeof(Real);
    }

private:
    Real x0_;
    Real s_;
    RandomAccessContainer data_;
};

template <class T>
struct daubechies_eval_type
{
   typedef T type;

   static const std::vector<T>& vector_cast(const std::vector<T>& v) { return v; }

};
template <>
struct daubechies_eval_type<float>
{
   typedef double type;

   inline static std::vector<float> vector_cast(const std::vector<double>& v)
   {
      std::vector<float> result(v.size());
      for (unsigned i = 0; i < v.size(); ++i)
         result[i] = static_cast<float>(v[i]);
      return result;
   }
};
template <>
struct daubechies_eval_type<double>
{
   typedef long double type;

   inline static std::vector<double> vector_cast(const std::vector<long double>& v)
   {
      std::vector<double> result(v.size());
      for (unsigned i = 0; i < v.size(); ++i)
         result[i] = static_cast<double>(v[i]);
      return result;
   }
};

struct null_interpolator
{
   template <class T>
   T operator()(const T&)
   {
      return 1;
   }
};

} // namespace detail

template<class Real, int p>
class daubechies_scaling {
   //
   // Some type manipulation so we know the type of the interpolator, and the vector type it requires:
   //
   typedef std::vector<std::array<Real, p < 6 ? 2 : p < 10 ? 3 : 4>> vector_type;
   //
   // List our interpolators:
   //
   typedef std::tuple<
      detail::null_interpolator, detail::matched_holder_aos<vector_type>, detail::linear_interpolation_aos<vector_type>,
      interpolators::detail::cardinal_cubic_hermite_detail_aos<vector_type>, interpolators::detail::cardinal_quintic_hermite_detail_aos<vector_type>,
      interpolators::detail::cardinal_septic_hermite_detail_aos<vector_type> > interpolator_list;
   //
   // Select the one we need:
   //
   typedef std::tuple_element_t<
      p == 1 ? 0 :
      p == 2 ? 1 :
      p == 3 ? 2 :
      p <= 5 ? 3 :
      p <= 9 ? 4 : 5, interpolator_list> interpolator_type;

public:
   daubechies_scaling(int grid_refinements = -1)
   {
      static_assert(p < 20, "Daubechies scaling functions are only implemented for p < 20.");
      static_assert(p > 0, "Daubechies scaling functions must have at least 1 vanishing moment.");
      if constexpr (p == 1)
      {
         return;
      }
      else {
         if (grid_refinements < 0)
         {
            if (std::is_same_v<Real, float>)
            {
               if (grid_refinements == -2)
               {
                  // Control absolute error:
                  //                          p= 2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
                  std::array<int, 20> r{ -1, -1, 18, 19, 16, 11,  8,  7,  7,  7,  5,  5,  4,  4,  4,  4,  3,  3,  3,  3 };
                  grid_refinements = r[p];
               }
               else
               {
                  // Control relative error:
                  //                          p= 2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
                  std::array<int, 20> r{ -1, -1, 21, 21, 21, 17, 16, 15, 14, 13, 12, 11, 11, 11, 11, 11, 11, 11, 11, 11 };
                  grid_refinements = r[p];
               }
            }
            else if (std::is_same_v<Real, double>)
            {
               //                          p= 2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
               std::array<int, 20> r{ -1, -1, 21, 21, 21, 21, 21, 21, 21, 21, 20, 20, 19, 19, 18, 18, 18, 18, 18, 18 };
               grid_refinements = r[p];
            }
            else
            {
               grid_refinements = 21;
            }
         }

         // Compute the refined grid:
         // In fact for float precision I know the grid must be computed in double precision and then cast back down, or else parts of the support are systematically inaccurate.
         std::future<std::vector<Real>> t0 = std::async(std::launch::async, [&grid_refinements]() {
            // Computing in higher precision and downcasting is essential for 1ULP evaluation in float precision:
            auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 0>(grid_refinements);
            return detail::daubechies_eval_type<Real>::vector_cast(v);
            });
         // Compute the derivative of the refined grid:
         std::future<std::vector<Real>> t1 = std::async(std::launch::async, [&grid_refinements]() {
            auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 1>(grid_refinements);
            return detail::daubechies_eval_type<Real>::vector_cast(v);
            });

         // if necessary, compute the second and third derivative:
         std::vector<Real> d2ydx2;
         std::vector<Real> d3ydx3;
         if constexpr (p >= 6) {
            std::future<std::vector<Real>> t3 = std::async(std::launch::async, [&grid_refinements]() {
               auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 2>(grid_refinements);
               return detail::daubechies_eval_type<Real>::vector_cast(v);
               });

            if constexpr (p >= 10) {
               std::future<std::vector<Real>> t4 = std::async(std::launch::async, [&grid_refinements]() {
                  auto v = daubechies_scaling_dyadic_grid<typename detail::daubechies_eval_type<Real>::type, p, 3>(grid_refinements);
                  return detail::daubechies_eval_type<Real>::vector_cast(v);
                  });
               d3ydx3 = t4.get();
            }
            d2ydx2 = t3.get();
         }

         auto y = t0.get();
         auto dydx = t1.get();

         if constexpr (p >= 2)
         {
            vector_type data(y.size());
            for (size_t i = 0; i < y.size(); ++i)
            {
               data[i][0] = y[i];
               data[i][1] = dydx[i];
               if constexpr (p >= 6)
                  data[i][2] = d2ydx2[i];
               if constexpr (p >= 10)
                  data[i][3] = d3ydx3[i];
            }
            if constexpr (p <= 3)
               m_interpolator = std::make_shared<interpolator_type>(std::move(data), grid_refinements, Real(0));
            else
               m_interpolator = std::make_shared<interpolator_type>(std::move(data), Real(0), Real(1) / (1 << grid_refinements));
         }
         else
            m_interpolator = std::make_shared<detail::null_interpolator>();
      }
   }

    inline Real operator()(Real x) const
    {
        if (x <= 0 || x >= 2*p-1)
        {
            return 0;
        }
        return (*m_interpolator)(x);
    }

    inline Real prime(Real x) const
    {
        static_assert(p > 2, "The 3-vanishing moment Daubechies scaling function is the first which is continuously differentiable.");
        if (x <= 0 || x >= 2*p-1)
        {
            return 0;
        }
        return m_interpolator->prime(x);
    }

    inline Real double_prime(Real x) const
    {
        static_assert(p >= 6, "Second derivatives require at least 6 vanishing moments.");
        if (x <= 0 || x >= 2*p - 1)
        {
            return Real(0);
        }
        return m_interpolator->double_prime(x);
    }

    std::pair<Real, Real> support() const
    {
        return {Real(0), Real(2*p-1)};
    }

    int64_t bytes() const
    {
       return m_interpolator->bytes() + sizeof(m_interpolator);
    }

private:
   std::shared_ptr<interpolator_type> m_interpolator;
};

}
#endif


/* ----------------------------------------------------------------------------
 * Copyright 2018, Ross Hartley <m.ross.hartley@gmail.com>
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   landmarks.cpp
 *  @author Ross Hartley
 *  @brief  Example of invariant filtering for landmark-aided inertial navigation
 *  @date   September 25, 2018
 **/

#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>
#include "inekf/inekf.hpp"

#define DT_MIN 1e-6
#define DT_MAX 1

double stod98(const std::string &s) {
    return atof(s.c_str());
}

int stoi98(const std::string &s) {
    return atoi(s.c_str());
}

using namespace std;
using namespace inekf;

int main() {
    //  ---- Initialize invariant extended Kalman filter ----- //
    RobotState initial_state;

    // Initialize state mean
    Eigen::Matrix3d R0;
    Eigen::Vector3d v0, p0, bg0, ba0;
    R0 << 1, 0, 0, // initial orientation
          0, -1, 0, // IMU frame is rotated 90deg about the x-axis
          0, 0, -1;
    v0 << 0,0,0; // initial velocity
    p0 << 0,0,0; // initial position
    bg0 << 0,0,0; // initial gyroscope bias
    ba0 << 0,0,0; // initial accelerometer bias
    initial_state.setRotation(R0);
    initial_state.setVelocity(v0);
    initial_state.setPosition(p0);
    initial_state.setGyroscopeBias(bg0);
    initial_state.setAccelerometerBias(ba0);

    // Initialize state covariance
    NoiseParams noise_params;
    noise_params.setGyroscopeNoise(0.01);
    noise_params.setAccelerometerNoise(0.1);
    noise_params.setGyroscopeBiasNoise(0.00001);
    noise_params.setAccelerometerBiasNoise(0.0001);

    // Initialize filter
    InEKF filter(initial_state, noise_params);
    cout << "Noise parameters are initialized to: \n";
    cout << filter.getNoiseParams() << endl;
    cout << "Robot's state is initialized to: \n";
    cout << filter.getState() << endl;

    // --- Optionally initialize prior landmarks --- //
    mapIntVector3d prior_landmarks;
    Eigen::Vector3d p_wl;
    int id;

    // // Landmark 1
    // id = 1;
    // p_wl << 0,-1,0;
    // prior_landmarks.insert(pair<int,Eigen::Vector3d> (id, p_wl));

    // Landmark 2
    // id = 2;
    // p_wl << 1,1,-0.5;
    // prior_landmarks.insert(pair<int,Eigen::Vector3d> (id, p_wl));

    // // Landmark 3
    // id = 3;
    // p_wl << 2,-1,0.5;
    // prior_landmarks.insert(pair<int,Eigen::Vector3d> (id, p_wl));

    // Store landmarks for localization
    filter.setPriorLandmarks(prior_landmarks);

    // Open data file
    ifstream infile("../data/imu_landmark_measurements.txt");
    string line;
    Eigen::Matrix<double,6,1> imu_measurement = Eigen::Matrix<double,6,1>::Zero();
    Eigen::Matrix<double,6,1> imu_measurement_prev = Eigen::Matrix<double,6,1>::Zero();
    double t = 0;
    double t_prev = 0;

    // Loop through data file and read in measurements line by line
    while (getline(infile, line)){
        vector<string> measurement;
        boost::split(measurement,line,boost::is_any_of(" "));
        // Handle measurements
        if (measurement[0].compare("IMU")==0){
            cout << "Received IMU Data, propagating state\n";
            assert((measurement.size()-2) == 6);
            t = stod98(measurement[1]);
            imu_measurement << stoi98(measurement[2]),
                               stoi98(measurement[3]),
                               stoi98(measurement[4]),
                               stoi98(measurement[5]),
                               stoi98(measurement[6]),
                               stoi98(measurement[7]);

            // Propagate using IMU data
            double dt = t - t_prev;
            if (dt > DT_MIN && dt < DT_MAX) {
                filter.Propagate(imu_measurement_prev, dt);
            }
        }
        else if (measurement[0].compare("LANDMARK")==0){
            cout << "Received LANDMARK observation, correcting state\n";
            assert((measurement.size()-2)%4 == 0);
            t = stod98(measurement[1]);
            vectorLandmarks measured_landmarks;
            for (int i=2; i<measurement.size(); i+=4) {
                int id = stoi98(measurement[i]);
                Eigen::Vector3d p_bl;
                p_bl << stoi98(measurement[i+1]),
                        stoi98(measurement[i+2]),
                        stoi98(measurement[i+3]);
                Eigen::Matrix3d cov = 0.01*Eigen::Matrix3d::Identity();
                Landmark landmark(id, p_bl, cov);
                measured_landmarks.push_back(landmark);
            }

            // Correct state using landmark measurements
            filter.CorrectLandmarks(measured_landmarks);
        }

        // Store previous timestamp
        t_prev = t;
        imu_measurement_prev = imu_measurement;
    }

    // Print final state
    cout << filter.getState() << endl;

    return 0;
}


/*
 * Copyright 2020-2021 Telecom Paris

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include "transport_channel.h"

#include <cmath>

#include <algorithm>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup/common_attributes.hpp>
#include <boost/log/utility/setup/file.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <limits>
#include <thread>
#include <vector>

#include "../../asn1c/nr_rrc/BCCH-DL-SCH-Message.h"
#include "../../utils/common_utils/common_utils.h"
#include "../../utils/sequence_generator/sequence_generator.h"
#include "../../variables/common_matrices/common_matrices.h"
#include "../../variables/common_variables/common_variables.h"
#include "../../variables/ldpc_matrices/ldpc_matrices.h"
#include "../libphy/libphy.h"

using namespace std;

auto free5GRAN::phy::transport_channel::compute_N_polar_code(int E,
                                                             int K,
                                                             int nmax) -> int {
  /**
   * \fn compute_N_polar_code
   * \brief Compute polar coding out sequence size
   * \standard TS 38.212 V15.2.0 Section 5.3.1
   *
   * \param[in] E: Rate matching output sequence length
   * \param[in] K: Polar coding input sequence length (including CRC)
   * \param[in] nmax: Maximum value of n
   *
   * \return Polar coding out sequence size
   */
  int n1, n2;
  float rmin = 1.0 / 8.0;
  if (E <= (9.0 / 8.0) * pow(2, ceil(log2(E)) - 1) &&
      (float)K / (float)E < 9.0 / 16.0) {
    n1 = ceil(log2(E)) - 1;
  } else {
    n1 = ceil(log2(E));
  }
  n2 = ceil(log2((float)K / rmin));
  if (nmax < 5 && n1 < 5 && n2 < 5) {
    return 5;
  } else {
    if (n1 < n2 && n1 < nmax) {
      return n1;
    } else if (n2 < n1 && n2 < nmax) {
      return n2;
    } else {
      return nmax;
    }
  }
}

void free5GRAN::phy::transport_channel::rate_recover(int* input_bits,
                                                     int* output_bits,
                                                     int i_bil,
                                                     int E,
                                                     int N,
                                                     int K) {
  /**
   * \fn rate_recover
   * \brief Rate recovering for polar coding.
   * \standard TS 38.212 V15.2.0 Section 5.4.1
   *
   * \partial_imp Code block de-interleaving not implemented
   * \param[in] input_bits: Input bits sequence
   * \param[out] output_bits: Output bits sequence
   * \param[in] i_bil: Coded blocks interleaving indicator (0: No interleaving,
   * 1: Interleaving) \param[in] E: Rate matching output sequence length
   * \param[in] N: Rate matching input sequence length
   */
  int e[E], y[N];
  /*
   * Coded bit-interleaving/de-interleaving TS38.212 5.4.1.3
   * No coded-bits interleaving
   */
  if (i_bil == 0) {
    for (int n = 0; n < E; n++) {
      e[n] = input_bits[n];
    }
  }
  /*
   * Bit de-selection TS38.212 5.4.1.2
   */
  if (E >= N) {
    for (int n = 0; n < N; n++) {
      y[n] = e[n];
    }
  } else {
    if ((float)K / (float)E <= 7.0 / 16.0) {
      for (int n = 0; n < N - E; n++) {
        y[n] = 0;
      }
      for (int n = 0; n < E; n++) {
        y[n + N - E] = e[n];
      }
    }
  }

  /*
   * Sub-block de-interleaving TS38.212 5.4.1.1
   */
  for (int n = 0; n < N; n++) {
    int i = floor(32 * (double)n / (double)N);
    int j_n =
        free5GRAN::SUB_BLOCK_INTERLEAVER_PATTERN[i] * N / 32 + n % (N / 32);
    output_bits[j_n] = y[n];
  }
}

void free5GRAN::phy::transport_channel::polar_decode(int* input_bits,
                                                     int* output_bits,
                                                     int N,
                                                     int K,
                                                     int nmax,
                                                     int i_il,
                                                     int n_pc,
                                                     int n_wm_pc,
                                                     int E) {
  /**
   * \fn polar_decode
   * \brief Polar decoding
   * \standard TS 38.212 V15.2.0 Section 5.3.1
   *
   * \partial_imp Only n_wm_pc=0 is implemented
   * \details
   * Details:
   * - Polar decoding using Gn inverse matrix
   * - Computing required sets
   * - Recover c'
   * - De-interleave c' to recover output
   *
   * \param[in] input_bits: Input bits sequence
   * \param[out] output_bits: Output bits sequence
   * \param[in] N: Rate matching input sequence length
   * \param[in] K: Polar coding input sequence length (including CRC)
   * \param[in] nmax: Maximum value of n
   * \param[in] i_il: Interleaving indicator (0: No interleaving, 1:
   * Interleaving) \param[in] n_pc: Number of parity check bits \param[in]
   * n_wm_pc: Number of other parity check bits
   */
  int q_0_n_1[N], count_seq, q_i_n[K + n_pc], c_p[K], pi_seq[K], j_n, u[N];
  int K_max = 164;
  bool found;

  /*
   * polar decoding using Gn inverse matrix (TS38.212 5.3.1.2)
   */
  switch (N) {
    case 32: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G5_INV[p][n]);
        }
      }
    };
    case 64: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G6_INV[p][n]);
        }
      }
    };
    case 128: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G7_INV[p][n]);
        }
      }
    };
    case 256: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G8_INV[p][n]);
        }
      }
    };
    case 512: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G9_INV[p][n]);
        }
      }
    };
    case 1024: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G10_INV[p][n]);
        }
      }
    };
  }

  count_seq = 0;
  vector<int> q_ftmp_n, q_itmp_n;

  /*
   * Computing q_0_n_1 (TS38.212 5.3.1.2)
   */
  for (int n : free5GRAN::POLAR_SEQUENCE_QNMAX_AND_RELIABILITY) {
    if (n < N) {
      q_0_n_1[count_seq] = n;
      count_seq++;
    }
  }

  if (E < N) {
    if ((float)K / (float)E <= 7.0 / 16.0) {
      for (int n = 0; n < N - E; n++) {
        int i = floor(32 * (double)n / (double)N);
        j_n =
            free5GRAN::SUB_BLOCK_INTERLEAVER_PATTERN[i] * N / 32 + n % (N / 32);
        q_ftmp_n.push_back(j_n);
      }
      if (E >= 3.0 * (float)N / 4.0) {
        for (int n = 0; n < ceil(3.0 * (float)N / 4.0 - (float)E / 2.0); n++) {
          q_ftmp_n.push_back(n);
        }
      } else {
        for (int n = 0; n < ceil(9.0 * (float)N / 16.0 - (float)E / 4.0); n++) {
          q_ftmp_n.push_back(n);
        }
      }
    }
  }

  for (int n = 0; n < N; n++) {
    found = false;
    for (int x : q_ftmp_n) {
      if (q_0_n_1[n] == x) {
        found = true;
        break;
      }
    }
    if (!found) {
      q_itmp_n.push_back(q_0_n_1[n]);
    }
  }
  /*
   * Computing q_i_n (TS38.212 5.3.1.2)
   */
  for (int n = 0; n < K + n_pc; n++) {
    q_i_n[n] = q_itmp_n[q_itmp_n.size() - (K + n_pc) + n];
  }
  count_seq = 0;
  /*
   * Recovering c' from u_n (TS38.212 5.3.1.2)
   */
  for (int n = 0; n < N; n++) {
    found = false;
    for (int p = 0; p < K + n_pc; p++) {
      if (q_i_n[p] == n) {
        found = true;
        break;
      }
    }
    if (found) {
      c_p[count_seq] = u[n];
      count_seq++;
    }
  }
  count_seq = 0;
  /*
   * generating pi sequence (TS38.212 5.3.1.1)
   */
  for (int m = 0; m < K_max; m++) {
    if (free5GRAN::INTERLEAVING_PATTERN[m] >= K_max - K) {
      pi_seq[count_seq] = free5GRAN::INTERLEAVING_PATTERN[m] - (K_max - K);
      count_seq++;
    }
  }
  /*
   * de-interleaving c' to recover output sequence (TS38.212 5.3.1.1)
   */
  for (int k = 0; k < K; k++) {
    output_bits[pi_seq[k]] = c_p[k];
  }
}

void free5GRAN::phy::transport_channel::crc_validate(int* input_bits,
                                                     int* crc_polynom,
                                                     int* remainder,
                                                     int length_input,
                                                     int length_crc) {
  /**
   * \fn crc_validate
   * \brief CRC validation
   * \details
   * Computes the remainder of the division of the input sequence by the
   * polynom. If the remainder is equal to 0, CRC is validated
   *
   * \param[in] input_bits: Input bits sequence
   * \param[in] crc_polynom: Polynom used for CRC computation
   * \param[out] remainder: Output remainder of the division
   * \param[in] length_input: Input size
   * \param[in] length_crc: Polynom size
   */
  int num_steps, seq1[length_crc];

  /*
   * Getting index of first 1 in input_bits
   */
  int index_0 = -1;
  for (int i = 0; i < length_input; i++) {
    if (input_bits[i] == 1) {
      index_0 = i;
      break;
    }
  }
  /*
   * Creating a new input called temp_input by removing all 0 from the beginning
   * of input_bits
   */
  int temp_input[length_input - index_0];
  for (int i = 0; i < length_input - index_0; i++) {
    temp_input[i] = input_bits[index_0 + i];
  }
  length_input = length_input - index_0;
  num_steps = length_input - length_crc + 1;

  /*
   * seq1 is the variable that will be XORed with crc_polynom step after step
   * Initializing seq1 variable to the first bits of temp_input
   */
  for (int i = 0; i < length_crc; i++) {
    seq1[i] = temp_input[i];
  }

  int i = 0;

  /*
   * Iterate until the polynom reaches the end of temp_input
   */
  while (i < num_steps) {
    /*
     * XORing seq1 and crc_polynom
     */
    for (int j = 0; j < length_crc; j++) {
      remainder[j] = seq1[j] ^ crc_polynom[j];
    }
    /*
     * Checing if the remainder is equal to 0. If true, algorithm ends
     */
    bool validated = true;
    for (int j = 1; j < length_crc; j++) {
      if (remainder[j] == 1) {
        validated = false;
        break;
      }
    }
    if (validated) {
      break;
    }
    /*
     * Searching first 1 index in remainder
     */
    int index_1 = -1;
    for (int j = 1; j < length_crc; j++) {
      if (remainder[j] == 1) {
        index_1 = j;
        break;
      }
    }
    /*
     * seq1 is updated to be the remainder shifted until finding the first 1
     */
    for (int j = 0; j < length_crc - index_1; j++) {
      seq1[j] = remainder[j + index_1];
    }
    /*
     * Adding new entries from temp_input to fill seq1
     */
    for (int j = 0; j < index_1; j++) {
      seq1[length_crc - index_1 + j] = temp_input[length_crc + i + j];
    }
    i += index_1;
  }
}

void free5GRAN::phy::transport_channel::compute_crc(int* input_bits,
                                                    int* crc_polynom,
                                                    int* remainder,
                                                    int length_input,
                                                    int length_crc) {
  /**
   * \fn compute_crc
   * \brief CRC computation
   * \details
   * Compute the CRC for a given input and polynom
   *
   * \param[in] input_bits: Input bits sequence
   * \param[in] crc_polynom: Polynom used for CRC computation
   * \param[out] remainder: Output CRC
   * \param[in] length_input: Input size
   * \param[in] length_crc: Polynom size
   */
  int num_steps, *seq1;

  /*
   * Getting index of first 1 in input_bits
   */
  int index_0 = -1;
  for (int i = 0; i < length_input; i++) {
    if (input_bits[i] == 1) {
      index_0 = i;
      break;
    }
  }
  /*
   * Creating a new input called temp_input by removing all 0 from the beginning
   * of input_bits Adding (length_crc - 1) zeros to complete temp_input
   */
  int temp_input[length_input - index_0 + length_crc - 1];
  for (int i = 0; i < length_input - index_0; i++) {
    temp_input[i] = input_bits[index_0 + i];
  }
  for (int i = 0; i < length_crc - 1; i++) {
    temp_input[length_input - index_0 + i] = 0;
  }
  length_input = length_input - index_0;
  num_steps = length_input;

  /*
   * Iterate until the polynom reaches the end of temp_input
   */
  int i = 0;
  while (i < num_steps) {
    /*
     * XORing temp_input and crc_polynom
     */
    for (int j = 0; j < length_crc; j++) {
      temp_input[i + j] ^= crc_polynom[j];
    }
    /*
     * Checing if temp_input (for the first length_input bits) is equal to 0. If
     * true, algorithm ends
     */
    bool finished = true;
    for (int j = 0; j < length_input; j++) {
      if (temp_input[j] == 1) {
        finished = false;
        break;
      }
    }
    if (finished) {
      break;
    }
    /*
     * Shifting until finding a 1 (at least once)
     */
    i++;
    while (temp_input[i] != 1) {
      i++;
    }
  }
  /*
   * Remainder corresponds to the (length_crc - 1) last bits of temp_input
   */
  for (int j = 0; j < length_crc - 1; j++) {
    remainder[j] = temp_input[length_input + j];
  }
}

void free5GRAN::phy::transport_channel::compute_ldpc_base_graph(int A,
                                                                float R,
                                                                int& graph) {
  /**
   * \fn compute_ldpc_base_graph
   * \brief Choose LDPC base graph
   * \standard TS 38.212 V15.2.0 Section 7.2.2
   *
   * \param[in] A: Transport block size
   * \param[in] R: Code rate
   * \param[out] graph: Output graph
   */
  if (A <= 292 || (A <= 3824 && R <= 0.67) || R <= 0.25) {
    graph = 2;
  } else {
    graph = 1;
  }
}

void free5GRAN::phy::transport_channel::compute_transport_block_size(
    int n_re,
    float R,
    int mod_order,
    int num_layers,
    int nrb,
    int& tbs) {
  /**
   * \fn compute_transport_block_size
   * \brief Transport block size computation
   * \standard TS 38.214 V15.2.0 Section 5.1.3.2
   *
   * \partial_imp n_info > 3824 not implemented
   *
   * \param[in] n_re: Number of PDSCH RE per RB
   * \param[in] R: Code rate
   * \param[in] mod_order: Modulation order
   * \param[in] num_layers: Number of transport layers
   * \param[in] nrb: Number of RB in PDSCH allocation
   * \param[out] tbs: Transport block size
   */
  long nre = (long)min(156, n_re) * nrb;
  double n_info = (double)nre * R * mod_order * num_layers;
  if (n_info <= 3824) {
    long n = (int)max(3.0, floor(log2(n_info)) - 6);
    long n_p_info = max(24.0, pow(2, n) * floor((double)n_info / pow(2, n)));
    for (int i : free5GRAN::TS_38_214_TABLE_5_1_3_2_1) {
      if (i >= n_p_info) {
        tbs = i;
        break;
      }
    }
  } else {
    cout << "N INFO not supported !" << endl;
  }
}

/*
 * TS 38 212 5.2.2
 */
void free5GRAN::phy::transport_channel::compute_Zc_dl_sch(int kb,
                                                          float k_p,
                                                          int& Zc,
                                                          int& i_ls) {
  /**
   * \fn compute_Zc_dl_sch
   * \brief Compute LDPC lifting size for DL-SCH encoding/decoding
   * \standard TS 38.212 V15.2.0 Section 5.2.2
   *
   * \param[in] kb: Intermediate value for Zc computation
   * \param[in] k_p: K' value
   * \param[out] Zc: Returned value
   * \param[out] i_ls: Zc set index
   */
  Zc = 512;
  for (int i = 0; i < 8; i++) {
    for (int j = 0; j < 8; j++) {
      if (TS_38_212_TABLE_5_3_2_1[i][j] < Zc &&
          TS_38_212_TABLE_5_3_2_1[i][j] >= (k_p / (float)kb)) {
        Zc = TS_38_212_TABLE_5_3_2_1[i][j];
        i_ls = i;
      }
    }
  }
}

void free5GRAN::phy::transport_channel::
    compute_code_block_segmentation_info_ldpc(int graph,
                                              int B,
                                              int& Zc,
                                              int& K,
                                              int& i_ls,
                                              int& L,
                                              int& C,
                                              int& N,
                                              int& K_p) {
  /**
   * \fn compute_code_block_segmentation_info_ldpc
   * \brief Compute code block segmentation informations for LDPC
   * \standard TS 38.212 V15.2.0 Section 5.2.2
   *
   * \param[in] graph: LDPC base graph
   * \param[in] B: Code block segmentation input size (inlcuding CRC)
   * \param[out] Zc: Returned LDPC lifting size
   * \param[out] K: Code block segmentation output size
   * \param[out] i_ls: Zc set index
   * \param[out] L: Code block CRC length
   * \param[out] C: Number of code blocks
   * \param[out] N: LDPC output sequence
   * \param[out] K_p: K' intermediate K value
   */
  int k_b, B_p;
  int k_cb = (graph == 1) ? 8448 : 3840;
  if (B <= k_cb) {
    L = 0;
    C = 1;
    B_p = B;
  } else {
    L = 24;
    C = ceil(B / (k_cb - L));
    B_p = B + C * L;
  }
  float k_p = (float)B_p / (float)C;
  if (graph == 1) {
    k_b = 22;
  } else {
    if (B > 640) {
      k_b = 10;
    } else if (B > 560) {
      k_b = 9;
    } else if (B > 192) {
      k_b = 8;
    } else {
      k_b = 6;
    }
  }
  compute_Zc_dl_sch(k_b, k_p, Zc, i_ls);
  K = (graph == 1) ? 22 * Zc : 10 * Zc;
  N = (graph == 1) ? 66 * Zc : 50 * Zc;
  K_p = (int)k_p;
}

void free5GRAN::phy::transport_channel::rate_recover_ldpc(
    int* input_bits,
    int N,
    int i_lbrm,
    int E,
    int id_rv,
    int mod_order,
    int C,
    int Zc,
    int graph,
    int K,
    int K_p,
    int* output_sequence) {
  /**
   * \fn rate_recover_ldpc
   * \brief Rate recovering for LDPC (hard bits)
   * \standard TS 38.212 V15.2.0 Section 5.4.2
   *
   * \param[in] input_bits: DL-SCH input bits
   * \param[in] N: Output sequence length
   * \param[in] i_lbrm: Indicator for N_cb computation
   * \param[in] E: DL-SCH input sequence length
   * \param[in] id_rv: Redundancy version
   * \param[in] mod_order: Modulation order
   * \param[in] C: Number of code blocks
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] K: Code block length
   * \param[in] K_p: K' code block intermediate length
   * \param[out] output_sequence: Output bits
   */
  int N_cb = (i_lbrm == 0) ? N : min(N, 25344);
  int e[E];
  int E_Q = (int)((float)E / (float)mod_order);
  /*
   * Input de-interleaving
   */
  for (int j = 0; j < E_Q; j++) {
    for (int i = 0; i < mod_order; i++) {
      e[i * (E_Q) + j] = input_bits[i + j * mod_order];
    }
  }
  /*
   * Compute k0
   */
  int k0, k, j;
  if (id_rv == 0) {
    k0 = 0;
  } else if (id_rv == 1) {
    k0 = (graph == 1) ? floor((17.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((13.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 2) {
    k0 = (graph == 1) ? floor((33.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((25.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 3) {
    k0 = (graph == 1) ? floor((56.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((43.0 * N_cb) / (50.0 * Zc)) * Zc;
  }

  /*
   * rate recovering
   */
  j = 0;
  k = 0;
  while (k < E) {
    int index = (k0 + j) % N_cb;
    if (index >= K_p - 2 * Zc && index < K - 2 * Zc) {
      output_sequence[index] = -1;
    } else {
      output_sequence[index] = e[k];
      k++;
    }
    j++;
  }
}

void free5GRAN::phy::transport_channel::rate_recover_ldpc(
    double* input_bits,
    int N,
    int i_lbrm,
    int E,
    int id_rv,
    int mod_order,
    int C,
    int Zc,
    int graph,
    int K,
    int K_p,
    double* output_sequence) {
  /**
   * \fn rate_recover_ldpc
   * \brief Rate recovering for LDPC (soft bits)
   * \standard TS 38.212 V15.2.0 Section 5.4.2
   *
   * \param[in] input_bits: DL-SCH input soft bits
   * \param[in] N: Output sequence length
   * \param[in] i_lbrm: Indicator for N_cb computation
   * \param[in] E: DL-SCH input sequence length
   * \param[in] id_rv: Redundancy version
   * \param[in] mod_order: Modulation order
   * \param[in] C: Number of code blocks
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] K: Code block length
   * \param[in] K_p: K' code block intermediate length
   * \param[out] output_sequence: Output soft bits
   */
  int N_cb = (i_lbrm == 0) ? N : min(N, 25344);
  double e[E];
  int E_Q = (int)((float)E / (float)mod_order);
  /*
   * Input de-interleaving
   */
  for (int j = 0; j < E_Q; j++) {
    for (int i = 0; i < mod_order; i++) {
      e[i * (E_Q) + j] = input_bits[i + j * mod_order];
    }
  }

  int k0, k, index, j;
  if (id_rv == 0) {
    k0 = 0;
  } else if (id_rv == 1) {
    k0 = (graph == 1) ? floor((17.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((13.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 2) {
    k0 = (graph == 1) ? floor((33.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((25.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 3) {
    k0 = (graph == 1) ? floor((56.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((43.0 * N_cb) / (50.0 * Zc)) * Zc;
  }

  vector<int> seen_indexes;
  /*
   * rate recovering
   */
  j = 0;
  k = 0;
  while (k < E) {
    index = (k0 + j) % N_cb;
    if (find(seen_indexes.begin(), seen_indexes.end(), index) ==
        seen_indexes.end()) {
      seen_indexes.push_back(index);
      if (index >= K_p - 2 * Zc && index < K - 2 * Zc) {
        output_sequence[index] = numeric_limits<double>::infinity();
        ;
      } else {
        output_sequence[index] = e[k];
        k++;
      }
    }
    j++;
  }
}

void free5GRAN::phy::transport_channel::compute_circular_permutation_matrix(
    int size,
    int offset,
    int** matrix) {
  /**
   * \fn compute_circular_permutation_matrix
   * \brief Compute offset times right shifted identity matrix of size n
   * \param[in] size: Identity matrix size
   * \param[in] offset: Number of times to shift matrix
   * \param[out] matrix: Output matrix
   */
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < size; j++) {
      matrix[i][j] = 0;
    }
    matrix[i][(i + offset) % size] = 1;
  }
}

void free5GRAN::phy::transport_channel::ldpc_decode_one_bit(
    vector<vector<int>> R,
    double* soft_bits,
    int i,
    double& new_bit) {
  /**
   * \fn ldpc_decode_one_bit
   * \brief LDPC correct one bit value (using belief propagation algorithm)
   * \param[in] R: Input set of rows connected to bit i
   * \param[in] soft_bits: Input soft bits
   * \param[in] i: Bit index
   * \param[out] new_bit: New bit value
   */
  double r_p1[R.size()], r_prop, q_p1, q_m1, n_q_p1, n_q_m1;
  /*
   * Compute rj for each element in R
   */
  for (int j = 0; j < R.size(); j++) {
    r_p1[j] = 1;
    for (int i_p = 0; i_p < R[j].size(); i_p++) {
      r_prop = 1 / (1 + exp(2 * soft_bits[R[j][i_p]]));
      r_p1[j] *= 1 - 2 * r_prop;
    }
    r_p1[j] = 0.5 + 0.5 * r_p1[j];
  }
  /*
   * Compute updated bit probabilities
   */
  q_m1 = 1;
  q_p1 = 1;
  for (int j = 0; j < R.size(); j++) {
    q_p1 *= r_p1[j];
    q_m1 *= (1 - r_p1[j]);
  }
  r_prop = 1 / (1 + exp(2 * soft_bits[i]));
  q_m1 = r_prop * q_m1;
  q_p1 = (1 - r_prop) * q_p1;
  /*
   * Normalization
   */
  n_q_p1 = q_p1 / (q_p1 + q_m1);
  n_q_m1 = q_m1 / (q_p1 + q_m1);
  /*
   * Update bit value depending on max probability
   */
  if (n_q_p1 > n_q_m1) {
    new_bit = -0.5 * log((1 / n_q_p1) - 1);
  } else {
    new_bit = 0.5 * log((1 / n_q_m1) - 1);
  }
}

void free5GRAN::phy::transport_channel::compute_H_matrix_ldpc(
    int Zc,
    int graph,
    int i_ls,
    vector<vector<int>>& matrix,
    int& size_i,
    int& size_j) {
  /**
   * \fn compute_H_matrix_ldpc
   * \brief Compute Hbg matrix for LDPC
   * \standard TS 38.212 V15.2.0 Section 5.3.2
   *
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] i_ls: Zc set index
   * \param[out] matrix: H matrix
   * \param[out] size_i: H matrix number of rows
   * \param[out] size_i: H matrix number of columns
   */
  if (graph == 1) {
    size_i = 46;
    size_j = 68;
  } else {
    size_i = 42;
    size_j = 52;
  }

  vector<int*> ldpc_table;
  if (graph == 1) {
    ldpc_table = free5GRAN::TS_38_212_TABLE_5_3_2_2;
  } else {
    ldpc_table = free5GRAN::TS_38_212_TABLE_5_3_2_3;
  }
  for (auto& p : ldpc_table) {
    int index_i = p[0];
    int index_j = p[1];
    int v_ij = p[i_ls + 2];
    matrix[index_i][index_j] = v_ij;
  }
}

void free5GRAN::phy::transport_channel::ldpc_decode(double* input_bits,
                                                    int N,
                                                    int Zc,
                                                    int graph,
                                                    int K,
                                                    int i_ls,
                                                    int* output_sequence) {
  /**
   * \fn compute_H_matrix_ldpc
   * \brief Compute H matrix for LDPC using Belief propagation algorithm
   * \standard TS 38.212 V15.2.0 Section 5.3.2
   *
   * \details
   * Details:
   * - Compute H matrix
   * - Recover LDPC input bits from input sequence
   * - Compute columns and row bit relations
   * - Iterate 10 time:
   * -# Try to correct each input bit value
   * -# If H times corrected bits value equals 0, algorithm ends, otherwise it
   * continues
   *
   * \param[in] input_bits: Input soft bits sequence
   * \param[in] N: LDPC input bit sequence length
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] K: Code block length
   * \param[in] i_ls: Zc set index
   * \param[out] output_sequence: Corrected output bits sequence
   */
  /*
   * H matrix generation
   */
  int size_i, size_j;
  size_i = 52;
  size_j = 42;
  vector<vector<int>> H(size_j, vector<int>(size_i, -1));
  compute_H_matrix_ldpc(Zc, graph, i_ls, H, size_j, size_i);

  double ldpc_input_bits[N + 2 * Zc];
  for (int i = 0; i < 2 * Zc; i++) {
    ldpc_input_bits[i] = 0;
  }
  for (int k = 2 * Zc; k < N + 2 * Zc; k++) {
    ldpc_input_bits[k] = input_bits[k - 2 * Zc];
  }
  double new_bits[N + 2 * Zc];
  vector<vector<int>> R[N + 2 * Zc], R_tot;
  vector<int> new_vec, new_vec_tot;

  /*
   * Generate rows and columns matrices
   */
  for (int j = 0; j < size_j * Zc; j++) {
    new_vec_tot.clear();
    for (int i = 0; i < size_i; i++) {
      if (H[j / Zc][i] != -1) {
        new_vec_tot.push_back(((j % Zc + H[j / Zc][i]) % Zc) + i * Zc);
      }
    }
    R_tot.push_back(new_vec_tot);
  }
  int index_l, inter_ind;
  for (int i = 0; i < N + 2 * Zc; i++) {
    for (int j = 0; j < size_j; j++) {
      if (H[j][i / Zc] != -1) {
        new_vec.clear();
        inter_ind = ((i % Zc - H[j][i / Zc]) % Zc);
        if (inter_ind < 0) {
          inter_ind = Zc + inter_ind;
        }
        index_l = inter_ind + j * Zc;

        for (int& p : R_tot[index_l]) {
          if (p != i) {
            new_vec.push_back(p);
          }
        }
        R[i].push_back(new_vec);
      }
    }
  }
  int rest, final_bit[N + 2 * Zc];
  /*
   * Looping over iteration
   */
  for (int iter = 0; iter < 10; iter++) {
    /*
     * Decode bits
     */
    for (int i = 0; i < N + 2 * Zc; i++) {
      ldpc_decode_one_bit(R[i], &ldpc_input_bits[0], i, ref(new_bits[i]));
    }
    for (int i = 0; i < N + 2 * Zc; i++) {
      ldpc_input_bits[i] = new_bits[i];
      final_bit[i] = (ldpc_input_bits[i] < 0) ? 1 : 0;
      if (i < K) {
        output_sequence[i] = final_bit[i];
      }
    }

    bool validated = true;
    for (int j = 0; j < size_j * Zc; j++) {
      rest = 0;
      for (int i : R_tot[j]) {
        rest ^= final_bit[i];
      }
      if (rest == 1) {
        validated = false;
        break;
      }
    }
    if (validated) {
      BOOST_LOG_TRIVIAL(trace) << "LDPC VALIDATED";
      break;
    }
  }
}

void free5GRAN::phy::transport_channel::decode_bch(int* bch_bits,
                                                   bool& crc_validated,
                                                   int* mib_bits,
                                                   int pci) {
  /**
   * \fn decode_bch
   * \brief Decode broadcast channel
   * \standard TS 38.212 V15.2.0 Section 7.1
   *
   * \details
   * Details:
   * - Rate recovering
   * - Polar decoding
   * - CRC validation
   * - Scrambling
   * - Payload de-interleaving
   *
   * \param[in] bch_bits: Input bits sequence
   * \param[out] crc_validated: Indicator for CRC validation
   * \param[out] mib_bits: Output MIB bits
   * \param[in] pci: Cell PCI
   */
  int n = free5GRAN::phy::transport_channel::compute_N_polar_code(
      free5GRAN::SIZE_SSB_PBCH_SAMPLES * 2, free5GRAN::SIZE_PBCH_POLAR_DECODED,
      9);
  int N = pow(2, n);
  int rate_recovered_bits[N],
      polar_decoded_bits[free5GRAN::SIZE_PBCH_POLAR_DECODED],
      remainder[free5GRAN::BCH_CRC_LENGTH + 1],
      bch_payload[free5GRAN::BCH_PAYLOAD_SIZE],
      bch_crc_recomputed[free5GRAN::BCH_CRC_LENGTH],
      bch_crc[free5GRAN::BCH_CRC_LENGTH], crc_masq[free5GRAN::BCH_CRC_LENGTH];
  // Rate recover bch_bits to rate_recovered_bits
  free5GRAN::phy::transport_channel::rate_recover(
      bch_bits, rate_recovered_bits, 0, free5GRAN::SIZE_SSB_PBCH_SAMPLES * 2, N,
      free5GRAN::SIZE_PBCH_POLAR_DECODED);
  // Polar decode rate_recovered_bits to polar_decoded_bits
  free5GRAN::phy::transport_channel::polar_decode(
      rate_recovered_bits, polar_decoded_bits, N,
      free5GRAN::SIZE_PBCH_POLAR_DECODED, 9, 1, 0, 0,
      free5GRAN::SIZE_SSB_PBCH_SAMPLES * 2);
  // Validate polar_decoded_bits CRC (compute the remainder and check that t is
  // equal to 0)
  free5GRAN::phy::transport_channel::crc_validate(
      polar_decoded_bits, free5GRAN::G_CRC_24_C, remainder,
      free5GRAN::SIZE_PBCH_POLAR_DECODED, free5GRAN::BCH_CRC_LENGTH + 1);
  crc_validated = true;
  for (int i = 1; i < free5GRAN::BCH_CRC_LENGTH + 1; i++) {
    if (remainder[i] == 1) {
      crc_validated = false;
      break;
    }
  }
  // Split polar_decoded_bits into bch_payload and bch_crc
  for (int i = 0; i < free5GRAN::BCH_PAYLOAD_SIZE; i++) {
    bch_payload[i] = polar_decoded_bits[i];
  }
  for (int i = 0; i < free5GRAN::BCH_CRC_LENGTH; i++) {
    bch_crc[i] = polar_decoded_bits[free5GRAN::BCH_PAYLOAD_SIZE + i];
  }
  // Re-compute the bch_payload CRC
  free5GRAN::phy::transport_channel::compute_crc(
      bch_payload, free5GRAN::G_CRC_24_C, bch_crc_recomputed, 32, 25);

  // XOR recomputed CRC with received CRC to determine CRC masq
  for (int i = 0; i < free5GRAN::BCH_CRC_LENGTH; i++) {
    crc_masq[i] = bch_crc[i] ^ bch_crc_recomputed[i];
  }

  /*
   * PBCH payload recovering (TS38.212 7.1.1)
   */
  int A = free5GRAN::BCH_PAYLOAD_SIZE;
  int A_bar = free5GRAN::BCH_PAYLOAD_SIZE - 8;
  int M = A - 3;
  int s_sequence[A], bch_descrambled[free5GRAN::BCH_PAYLOAD_SIZE];
  int sfn_bits[4][2] = {{0, 0}, {0, 1}, {1, 0}, {1, 1}};
  // Find the correct value of v
  for (int v = 0; v < 4; v++) {
    // Generate de-scrambling sequence
    int c_seq[free5GRAN::BCH_PAYLOAD_SIZE + v * M];
    free5GRAN::utils::sequence_generator::generate_c_sequence(
        pci, free5GRAN::BCH_PAYLOAD_SIZE + v * M, c_seq, 0);
    int j = 0;
    // Generate s sequence
    for (int i = 0; i < A; i++) {
      if (i == 0 || i == 6 || i == 24) {
        s_sequence[i] = 0;
      } else {
        s_sequence[i] = c_seq[j + v * M];
        j++;
      }
    }
    // De-scramble bch_payload to bch_descrambled
    free5GRAN::utils::common_utils::scramble(bch_payload, s_sequence,
                                             bch_descrambled,
                                             free5GRAN::BCH_PAYLOAD_SIZE, 0);

    // BCH payload de-interleaving
    int j_sfn = 0;
    int j_hrf = 10;
    int j_ssb = 11;
    int j_other = 14;
    for (int i = 0; i < 32; i++) {
      if (i == 24 || i == 25 || i == 26 || i == 27 || i == 1 || i == 2 ||
          i == 3 || i == 4 || i == 5 || i == 6) {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_sfn]];
        j_sfn++;
      } else if (i == 28) {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_hrf]];
      } else if (i >= A_bar + 5 && i <= A_bar + 7) {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_ssb]];
        j_ssb++;
      } else {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_other]];
        j_other++;
      }
    }
    // If final bits correspond to 3rd and 2nd LSB of SFN, correct v was found
    if (sfn_bits[v][0] == mib_bits[25] && sfn_bits[v][1] == mib_bits[26]) {
      break;
    }
  }
}

void free5GRAN::phy::transport_channel::decode_dci(
    int* dci_bits,
    int E,
    int K,
    int* rnti,
    bool& validated,
    vector<int>& decoded_dci_bits) {
  /**
   * \fn decode_dci
   * \brief DCI decoding
   * \standard TS 38.212 V15.2.0 Section 7.3
   *
   * \partial_imp Only DCI Format 1_0 has been tested
   * \details
   * Details:
   * - Rate recovering
   * - Polar decoding
   * - RNTI scrambling
   * - CRC validation
   *
   * \param[in] dci_bits: Input bits sequence
   * \param[in] E: PDCCH payload size
   * \param[in] K: DCI payload size (including CRC)
   * \param[in] rnti: RNTI for identifying DCI Format
   * \param[out] validated: Indicator for CRC validation
   * \param[out] decoded_dci_bits: Output bits sequence
   */
  int n = compute_N_polar_code(E, K, 9);
  int N = pow(2, n);

  BOOST_LOG_TRIVIAL(trace) << "(n, N) = (" + to_string(n) + ", " +
                                  to_string(N) + ")";
  BOOST_LOG_TRIVIAL(trace) << "E = " + to_string(E);
  BOOST_LOG_TRIVIAL(trace) << "K = " + to_string(K);

  int rate_recovered[N], polar_decoded[K], remainder[25], descrambled[K + 24];

  int A = K - 24;
  /*
   * rate recovering
   */
  rate_recover(dci_bits, rate_recovered, 0, E, N, K);
  /*
   * Polar decoding
   */
  polar_decode(rate_recovered, polar_decoded, N, K, 9, 1, 0, 0, E);
  /*
   * RNTI de-masking and CRC validation
   */
  for (int i = 0; i < 24; i++) {
    descrambled[i] = 1;
  }
  for (int i = 0; i < K; i++) {
    if (i < A + 8) {
      descrambled[i + 24] = polar_decoded[i];
    } else {
      descrambled[i + 24] = (polar_decoded[i] + rnti[i - A - 8]) % 2;
    }
  }
  crc_validate(descrambled, free5GRAN::G_CRC_24_C, remainder, K + 24, 25);
  validated = true;
  for (int i : remainder) {
    if (i == 1) {
      validated = false;
      break;
    }
  }
  BOOST_LOG_TRIVIAL(trace) << "## CRC "
                           << ((validated) ? "validated" : "not validated");

  for (int i = 0; i < A; i++) {
    decoded_dci_bits[i] = polar_decoded[i];
  }
}

auto free5GRAN::phy::transport_channel::decode_dl_sch(
    double* dl_sch_bits,
    int n_re,
    float R,
    int nrb,
    int E,
    bool& validated,
    free5GRAN::dci_1_0_si_rnti dci_1_0_si_rnti) -> vector<int> {
  /**
   * \fn decode_dl_sch
   * \brief DL-SCH decoding
   * \standard TS 38.212 V15.2.0 Section 7.2
   *
   * \partial_imp Implemented for one code block (C=1)
   * \details
   * Details:
   * - Rate recovering
   * - LDPC decoding
   * - CRC validation
   *
   * \param[in] dl_sch_bits: Input soft bits sequence
   * \param[in] n_re: Number of RE per PDSCH RB
   * \param[in] R: Code Rate
   * \param[in] nrb: Number of PDSCH allocated RB
   * \param[in] E: PDSCH output sequence length
   * \param[out] validated: CRC validation indicator
   * \param[in] dci_1_0_si_rnti: Input DCI Format 1_0 object
   */

  /*
   * Compute code block segmentation information
   */
  int graph, A, N, K, Zc, i_ls, L_cb, C, B, L, K_p;
  compute_transport_block_size(n_re, R, 2, 1, nrb, A);
  compute_ldpc_base_graph(A, R, graph);
  L = (A > 3824) ? 24 : 16;
  B = A + L;
  compute_code_block_segmentation_info_ldpc(graph, B, Zc, K, i_ls, L_cb, C, N,
                                            K_p);

  BOOST_LOG_TRIVIAL(trace) << "(TBS, graph) = " << A << ", " << graph;
  BOOST_LOG_TRIVIAL(trace) << "B = " << B;
  BOOST_LOG_TRIVIAL(trace) << "Zc = " << Zc;
  BOOST_LOG_TRIVIAL(trace) << "K = " << K;
  BOOST_LOG_TRIVIAL(trace) << "i_ls = " << i_ls;
  BOOST_LOG_TRIVIAL(trace) << "L = " << L;
  BOOST_LOG_TRIVIAL(trace) << "C = " << C;
  BOOST_LOG_TRIVIAL(trace) << "N = " << N;
  BOOST_LOG_TRIVIAL(trace) << "L_cb = " << L_cb;
  BOOST_LOG_TRIVIAL(trace) << "K_p = " << K_p;
  BOOST_LOG_TRIVIAL(trace) << "E = " << E;

  /*
   * Rate recovering
   */
  auto* rate_recovered = new double[N];
  rate_recover_ldpc(dl_sch_bits, N, 1, E, dci_1_0_si_rnti.rv, 2, C, Zc, graph,
                    K, K_p, rate_recovered);

  /*
   * LDPC decoding
   */
  int ldpc_decoded[K];
  auto start = chrono::steady_clock::now();
  ldpc_decode(rate_recovered, N, Zc, graph, K, i_ls, ldpc_decoded);
  auto end = chrono::steady_clock::now();
  auto diff = end - start;
  BOOST_LOG_TRIVIAL(trace) << "## LDPC execution time "
                           << chrono::duration<double, milli>(diff).count()
                           << "ms";

  int desegmented[B];
  for (int i = 0; i < B; i++) {
    desegmented[i] = ldpc_decoded[i];
  }

  /*
   * CRC validation
   */
  int remainder[L];
  if (L == 24) {
    crc_validate(desegmented, free5GRAN::G_CRC_24_C, remainder, B, L + 1);
  } else {
    crc_validate(desegmented, free5GRAN::G_CRC_16, remainder, B, L + 1);
  }

  validated = true;
  for (int i = 0; i < L + 1; i++) {
    if (remainder[i] == 1) {
      validated = false;
      break;
    }
  }
  vector<int> output_bits(A);

  for (int i = 0; i < A; i++) {
    output_bits[i] = desegmented[i];
  }

  BOOST_LOG_TRIVIAL(trace) << "## CRC "
                           << ((validated) ? "validated" : "not validated");

  return output_bits;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_LOG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_LOG_HPP_INCLUDED

#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/fma.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/simd/if_nan_else.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/if_else_zero.hpp>
#include <boost/simd/function/simd/is_ltz.hpp>
#include <boost/simd/arch/common/detail/generic/f_log_kernel.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/log2_em1.hpp>
#include <boost/simd/constant/log10_ehi.hpp>
#include <boost/simd/constant/log10_elo.hpp>
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/constant/log10_2lo.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/simd/is_nan.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#endif
#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/is_less.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/constant/mlogtwo2nmb.hpp>
#include <boost/simd/constant/mlog2two2nmb.hpp>
#include <boost/simd/constant/mlog10two2nmb.hpp>
#endif

  //////////////////////////////////////////////////////////////////////////////
  // how to compute the various logarithms
  //////////////////////////////////////////////////////////////////////////////
  // The method is mainly taken from the cephes library:
  // first reduce the the data
  // a0 is supposed > 0
  // the input a0 is split into a mantissa and an exponent
  // the mantissa m is between sqrt(0.5) and sqrt(2) and the corresponding exponent is e
  // a0 = m*2^e
  // then the log? calculus is split in two parts (? being nothing: natural logarithm,  2: base 2 logarithm,
  //10 base ten logarithm)
  // as log?(a) = log?(2^e)+log?(m)
  // 1) computing log?(m)
  //   first put x = m-1 (so -0.29 <  x < 0.414)
  //   write log(m)   = log(1+x)   = x + x*x/2 + x*x*x*g(x)
  //   write log2(m)  = log2(1+x)  = C2*log(x),
  //     C2 = log(2)  the multiplication have to be taken seriously as C2 is not exact
  //   write log10(m) = log10(1+x) = C10*log(x),
  //     C10= log(10) the multiplication have to be taken seriously as C10 is not exact
  // then g(x) has to be approximated
  // g is ((log(1+x)/x-1)/x-1/2)/x
  // It is not a good idea to approximate directly log(1+x) instead of g,  because this will lead to bad precision around 1.
  //
  // in this approximation one can choose a best approximation rational function given by remez algorithm.
  // there exist a classical solution which is a polynomial p8 one of degree 8 that gives 0.5ulps everywhere
  // this is what is done in the kernel_t::log impl;
  // Now,  it is possible to choose a rational fraction or a polynomial of lesser degree to approximate g
  // providing faster but less accurate logs.
  // 2) computing log?(2^e)
  // see the explanations relative to each case
  // 3) finalize
  // This is simply treating invalid entries
  // 4) For denormal we use the fact that log(x) =  log?(x*y)-log?(y) and that if y is
  // the constant two2nmb if x is denormal x*y and y are not.
  //////////////////////////////////////////////////////////////////////////////

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd = boost::dispatch;
    template < class A0 >
    struct logarithm< A0, tag::simd_type, float>
    {
      using int_type = bd::as_integer_t<A0, signed>;
      using sA0 = bd::scalar_of_t<A0>;
      using  kernel_t = kernel<A0, tag::simd_type, float>;

      static inline A0 log(const A0& a0) BOOST_NOEXCEPT
      {
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        auto denormal = is_less(bs::abs(z), Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        t = if_else_zero(denormal, Mlogtwo2nmb<A0>());
#endif
        //log(2.0) in double is 6.931471805599453e-01
        //double(0.693359375f)+double(-0.00021219444f)  is  6.931471805600000e-01 at 1.0e-14 of log(2.0)
        // let us call Log_2hi 0.693359375f anf Log_2lo -0.00021219444f
        // We use thi to correct the sum where this could matter a lot
        // log(a0) = fe*Log_2hi+ (0.5f*x*x +(fe*Log_2lo+y))
        // These operations are order dependent: the parentheses do matter
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = bs::fma(fe, Log_2lo<A0>(), y);
        y = bs::fma(Mhalf<A0>(), x2, y);
#ifdef BOOST_SIMD_NO_DENORMALS
        return finalize(a0, bs::fma(Log_2hi<A0>(), fe, x+y));
#else
        return finalize(a0, bs::fma(Log_2hi<A0>(), fe, x+y+t));
#endif
      }

      static inline A0 log2(const A0& a0) BOOST_NOEXCEPT
      {
        A0 z =  a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = lt(bs::abs(z), Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 t = if_else_zero(denormal, Mlog2two2nmb<A0>());
#endif
        //here let l2em1 = log2(e)-1, the computation is done as:
        //log2(a0) = ((l2em1*x+(l2em1*(y+x*x/2)))+(y+x*x/2)))+x+fe for best results
        // once again the order is very important.
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = bs::fma(Mhalf<A0>(),x2, y);
        z = bs::fma(x,Log2_em1<A0>(),y*Log2_em1<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
        return finalize(a0, ((z+y)+x)+fe);
#else
        return finalize(a0, ((z+y)+x)+fe+t);
#endif
      }

      static inline A0 log10(const A0& a0) BOOST_NOEXCEPT
      {
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = lt(bs::abs(z), Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 t = if_else_zero(denormal, Mlog10two2nmb<A0>());
#endif
        // here there are two multiplication: log of fraction by log10(e) and base 2 exponent by log10(2)
        // and we have to split log10(e) and log10(2) in two parts to get extra precision when needed
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = bs::fma(x2, Mhalf<A0>(), y);
        z = (x+y* Log10_elo<A0>());
        z = bs::fma(y, Log10_ehi<A0>(), z);
        z = bs::fma( x, Log10_ehi<A0>(), z);
        z = bs::fma(fe, Log10_2hi<A0>(), z);
#ifdef BOOST_SIMD_NO_DENORMALS
        return finalize(a0, bs::fma(fe, Log10_2lo<A0>(), z));
#else
        return finalize(a0, bs::fma(fe, Log10_2lo<A0>(), z+t));
#endif
      }
    private:
      static inline A0 finalize(const A0& a0, const A0& y) BOOST_NOEXCEPT
      {
#ifdef BOOST_SIMD_NO_NANS
        auto test = bs::is_ltz(a0);
#else
        auto test = bs::logical_or(bs::is_ltz(a0), bs::is_nan(a0));
#endif
        A0 y1 = bs::if_nan_else(test, y);
#ifndef BOOST_SIMD_NO_INFINITIES
        y1 = if_else(bs::is_equal(a0, bs::Inf<A0>()), a0, y1);
#endif
        return if_else(is_eqz(a0), bs::Minf<A0>(), y1);
      }
    };
  }
} }

#endif


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::response::right_truncated::mean_event.hpp   		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_MEAN_EVENT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_MEAN_EVENT_HPP_ER_2009
#include <stdexcept>
#include <boost/type_traits.hpp>
#include <boost/range.hpp>
#include <boost/format.hpp>
#include <boost/operators.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace response{

    // This class can be used to accumulate the proportion of failures and
    // mean event time in a sequence of events.
    //
    // In the case of the exponential, a collection of mean_events can
    // form a sufficient statistic,  assuming a discretized domain for
    // the covariate: domain(x) = {x[k]:k=0,...,K-1}
    //
    // $\log L_i(\beta) = \nu_i \eta_i - t_i \exp(\eta_i)$
    // where $\eta_i = \langle x_i,beta \rangle$
    // log L(D,\beta) = sum{
    //    <x[k],\beta> sum{\nu[i]:x[i]==x[k]}
    //     -exp(<x[k],\beta>) sum{t[i] : x[i]==x[k]}
    //  :k=1,...,K
    // }
    // Sufficient statistic:
    // {(sum{\nu[i]:x[i]==x[k]},sum{t[i] : x[i]==x[k]}):k=0,...,K-1}
    template<typename T,typename B = bool>
    class mean_event :
        public event<T,T>,
        boost::addable<
            mean_event<T,B>
        >
        // TODO equality_comparable<mean_event<T,B> >
    {
    public:
        typedef event<T,B>  event_;
        typedef event<T,T>  super_;
        typedef typename super_::value_type value_type;
        typedef std::size_t                 size_type;

        // Construction
        mean_event();
        mean_event(const event_&);
        mean_event(const mean_event&);
        mean_event& operator=(const mean_event&);

        // Access
        size_type count()const;

        // Operators
        mean_event& operator+=( const mean_event& e );
        mean_event& operator()( const event_& e );

        // I/O
        template<class Archive>
        void serialize(Archive & ar, const unsigned int version);

        private:
        value_type impl(size_type n_a,value_type a,size_type n_b,value_type b);
        size_type  count_;
        static super_ convert(const event_&);
    };

    template<typename T,typename B>
    std::ostream& operator<<(std::ostream& out,const mean_event<T,B>& e);

    template<typename E>
    struct meta_mean_event{
        typedef typename E::value_type value_type;
        typedef mean_event<value_type> type;
    };

    // Implementation //

    // Constructor
    template<typename T,typename B>
    mean_event<T,B>::mean_event()
    :super_(static_cast<T>(false),static_cast<T>(0)),
    count_(0){
        // Using super_() instead, would initialize time to inf.
    }

    template<typename T,typename B>
    mean_event<T,B>::mean_event(const event_& e)
    :super_(convert(e)),count_(1){
        const char* msg = "mean_event::mean_event(e), isinf(e.time())";
        if(boost::math::isinf(this->time())){
            throw std::runtime_error(
                msg
            );
        }
    }

    template<typename T,typename B>
    mean_event<T,B>::mean_event(const mean_event& that)
    :super_(that),count_(that.count_){}

    template<typename T,typename B>
    mean_event<T,B>&
    mean_event<T,B>::operator=(const mean_event& that){
        if(&that!=this){
            super_::operator=( that );
            count_ = that.count_;
        }
        return *this;
    }

    // Access
    template<typename T,typename B>
    typename mean_event<T,B>::size_type
    mean_event<T,B>::count()const{ return count_; }

    // Update
    template<typename T,typename B>
    mean_event<T,B>&
    mean_event<T,B>::operator()(const event_& e){
        mean_event other(e);
        return ( (*this) += other );
    }

    template<typename T,typename B>
    mean_event<T,B>&
    mean_event<T,B>::operator+=(const mean_event& other){
        size_type n_a = this->count();
        size_type n_b = other.count();
        (this->failure_)
            = (this->impl(n_a,this->failure(),n_b,other.failure()));
        (this->time_)
            = (this->impl(n_a,this->time(),n_b,other.time()));
        (this->count_) += n_b;
        return *this;
    }

    //Private
    template<typename T,typename B>
    template<class Archive>
    void mean_event<T,B>::serialize(Archive & ar, const unsigned int version)
    {
        ar & boost::serialization::base_object<super_>(*this);
        ar & count_;
    }

    template<typename T,typename B>
    typename mean_event<T,B>::value_type
    mean_event<T,B>::impl(
            size_type n_a,
            value_type a,
            size_type n_b,
            value_type b
    ){
        return ( (n_a * a) + (n_b * b) ) / (n_a + n_b);
    }

    template<typename T,typename B>
    std::ostream& operator<<(std::ostream& out,const mean_event<T,B>& that){
        typedef mean_event<T,B> that_;
        typedef typename that_::super_ super_;
        static const char* str = "(%1%,%2%)";
        format f(str);
        f % that.count() % static_cast<const super_&>(that) ;
        out << f.str();
        return out;
    }

    template<typename T,typename B>
    typename mean_event<T,B>::super_
    mean_event<T,B>::convert(const event_& e){
        static value_type zero = static_cast<value_type>(0);
        static value_type one = static_cast<value_type>(1);
        value_type t = e.time();
        if(e.failure()){
            return super_( one, t );
        }else{
            return super_( zero, t );
        }
    }

}// response
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif