//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
/*!
 * \file
**/
#ifndef NT2_INTERPOL_FUNCTIONS_IDXY_BILINEAR_HPP_INCLUDED
#define NT2_INTERPOL_FUNCTIONS_IDXY_BILINEAR_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <nt2/sdk/meta/value_as.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/core/container/dsl/value_type.hpp>
#include <boost/mpl/bool.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>

/*!
 * \ingroup interpol
 * \defgroupinterpol_idxy_bilinear idxy_bilinear
 *
 * \par Description
 * one dimensional idxy_bilinear interpolation
 * given an array a and a vector idx of "real" indices
 * provides the interpolated values along the a rows
 * by bilinear formula.
 * calls can be idxy_bilinear(a0, idx, idy)
 *              idxy_bilinear(a0, idx, idy, true) allowing extrapolation
 *              idxy_bilinear(a0, idx, idy, val1) putting val1 outside the bounds
 *              idxy_bilinear(a0, idx, idy, val1, val2)}, putting val1 under the index bounds val2 over
 *              idxy_bilinear(a0, idx, idy, val1x val2x, val1y, val2y)
 *              idxy_bilinear(a0, idx, idy, _, dim1, dim2) are the dimensions of interpolation 2 (rows), 1 (columns) of a0 by default
 *              idxy_bilinear(a0, idx, idy, val1, dim1, dim2)
 *              idxy_bilinear(a0, idx, idy, val1, val2, dim1, dim2)
 *              idxy_bilinear(a0, idx, idy, val1x val2x, val1y, val2y, dim1, dim2) can also be used
 * \par
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/idxy_bilinear.hpp>
 * \endcode
 *
 *
**/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag idxy_bilinear_ of functor idxy_bilinear
     *        in namespace nt2::tag for toolbox statistics
    **/
    struct idxy_bilinear_ : ext::unspecified_<idxy_bilinear_> { typedef ext::unspecified_<idxy_bilinear_> parent; };
  }
  NT2_FUNCTION_IMPLEMENTATION(tag::idxy_bilinear_, idxy_bilinear, 3)
  NT2_FUNCTION_IMPLEMENTATION(tag::idxy_bilinear_, idxy_bilinear, 4)
  NT2_FUNCTION_IMPLEMENTATION(tag::idxy_bilinear_, idxy_bilinear, 5)
  NT2_FUNCTION_IMPLEMENTATION(tag::idxy_bilinear_, idxy_bilinear, 6)
  NT2_FUNCTION_IMPLEMENTATION(tag::idxy_bilinear_, idxy_bilinear, 7)
  NT2_FUNCTION_IMPLEMENTATION(tag::idxy_bilinear_, idxy_bilinear, 9)
  NT2_FUNCTION_IMPLEMENTATION(tag::idxy_bilinear_, idxy_bilinear, 10)
}

namespace nt2 { namespace ext
{
  template<class Domain, class Expr,  int N>
  struct size_of<tag::idxy_bilinear_, Domain, N, Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&,N-2>::type   choice1_t;
    typedef typename boost::proto::result_of::child_c<Expr&,N-1>::type   choice2_t;

    typedef typename  boost::proto::result_of
                      ::child_c<Expr&, 1>::value_type::extent_type  result_type;

    result_type operator()(Expr& e) const
    {
      const choice1_t & v1   =  boost::proto::child_c<N-2>(e);
      const choice2_t & v2   =  boost::proto::child_c<N-1>(e);
      typedef typename nt2::meta::is_integral<choice1_t>::type c_t;
      std::size_t dim1 = 2, dim2 = 1;
      getdims(boost::proto::child_c<1>(e), v1, v2, dim1, dim2, c_t());
      result_type sizee = boost::proto::child_c<0>(e).extent();
      sizee[dim1-1] = numel(boost::proto::child_c<1>(e));
      sizee[dim2-1] = numel(boost::proto::child_c<2>(e));
      return sizee;
    }

    template<class X, class C1, class C2> static
    void getdims( const X & xi, const C1 &, const C2 &
                , std::size_t&, std::size_t&, const boost::mpl::false_&
                )
    {}

    template<class X, class C1, class C2> static
    void getdims( const X & xi, const C1& v1, const C2& v2
                , std::size_t& dim1, std::size_t& dim2, const boost::mpl::true_&
                )
    {
      dim1 = v1;
      dim2 = v2;
    }
  };

  template <class Domain, class Expr,  int N>
  struct  value_type < tag::idxy_bilinear_, Domain,N,Expr>
        : meta::value_as<Expr,0>
  {};
} }

#endif


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

//! @author Jeff Ichnowski

#pragma once
#ifndef NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_SO2_HPP
#define NIGH_IMPL_KDTREE_MEDIAN_TRAVERSAL_SO2_HPP

#include "traversal.hpp"
#include "lp_branch.hpp"
#include <Eigen/Dense>

namespace unc::robotics::nigh::impl::kdtree_median {
    template <class Tree, class Key, int p, class Get>
    class Traversal<Tree, Key, metric::SO2<p>, Get> {
        using Metric = metric::SO2<p>;
        using Space = metric::Space<Key, Metric>;
        using Distance = typename Space::Distance;

        static constexpr int dimensions = Space::dynamic
            ? Eigen::Dynamic : Space::kDimensions;

        Eigen::Array<Distance, dimensions, 2> bounds_;
        Eigen::Matrix<Distance, dimensions, 1> dists_;
        Distance distToRegion_{0};

    public:
        Traversal(const Space& space)
            : bounds_(space.dimensions(), 2)
            , dists_(space.dimensions())
        {
            bounds_.col(0) = -PI<Distance>;
            bounds_.col(1) = PI<Distance>;
            dists_.setZero();
        }

        Distance distToRegion() const {
            return distToRegion_;
        }

        template <typename Fn>
        void recur(const Key& key, unsigned a, int side, Distance split, const Fn& fn) {
            Distance b0 = bounds_(a, 0);
        }

        template <typename Nearest, typename Iter>
        void recur(
            Nearest& nearest,
            Distance split, Node *child, int childNo,
            unsigned axis,
            const Key& key,
            Iter first, Iter last)
        {
            Distance b0 = bounds_(axis, 0);
            Distance b1 = bounds_(axis, 1);

            // setting the antipodal bounds only needs to happen at
            // the top split in the tree.  We can potentially simplify
            // this code.
            if (b0 == -PI<Distance> && b1 == PI<Distance>)
                bounds_(axis, !childNo) = so2::antipode(split);

            bounds_(axis, childNo) = split;

            Distance d0 = so2::ccwDist(Space::coeff(key, axis), bounds_(axis, 0));
            Distance d1 = so2::ccwDist(bounds_(axis, 1), Space::coeff(key, axis));
            dists_[axis] = (d0 + d1 < 2*PI<Distance>) ? std::min(d0, d1) : 0;
            distToRegion_ = dists_.template lpNorm<p>();

            nearest(child, first, last);

            bounds_(axis, 0) = b0;
            bounds_(axis, 1) = b1;
        }

        template <typename Nearest, typename Iter>
        void follow(
            Nearest& nearest,
            const Space&,
            const Node *node, unsigned axis,
            const Key& key,
            Iter first, Iter last)
        {
            const auto* branch = static_cast<const LPBranch<Distance>*>(node);

            std::array<Iter, 3> iters{{
                first, first + std::distance(first, last)/2, last}};

            Distance dist = so2::ccwDist(branch->split(), Space::coeff(key, axis));
            int childNo = dist > PI<Distance>;

            Distance oldDist = dists_[axis];
            Distance oldDistToRegion = distToRegion_;

            recur(nearest, branch->split(), branch->child(childNo), childNo,
                  axis, key, iters[childNo], iters[childNo+1]);
            recur(nearest, branch->split(), branch->child(childNo), !childNo,
                  axis, key, iters[1-childNo], iters[2-childNo]);

            dists_[axis] = oldDist;
            distToRegion_ = oldDistToRegion;
        }
    };
}

#endif


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "duplicate_allele_fraction.hpp"

#include <algorithm>
#include <iterator>
#include <cassert>

#include <boost/variant.hpp>

#include "io/variant/vcf_record.hpp"
#include "allele_depth.hpp"
#include "duplicate_allele_depth.hpp"

namespace octopus { namespace csr {

const std::string DuplicateAlleleFraction::name_ = "DAF";

std::unique_ptr<Measure> DuplicateAlleleFraction::do_clone() const
{
    return std::make_unique<DuplicateAlleleFraction>(*this);
}

Measure::ResultType DuplicateAlleleFraction::get_default_result() const
{
    return std::vector<boost::optional<double>> {};
}

Measure::ResultType DuplicateAlleleFraction::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto allele_depths = boost::get<std::vector<boost::optional<int>>>(AlleleDepth{}.evaluate(call, facets));
    const auto duplicate_allele_depths = boost::get<std::vector<boost::optional<int>>>(DuplicateAlleleDepth{}.evaluate(call, facets));
    assert(allele_depths.size() == duplicate_allele_depths.size());
    std::vector<boost::optional<double>> result(allele_depths.size());
    std::transform(std::cbegin(allele_depths), std::cend(allele_depths), std::cbegin(duplicate_allele_depths), std::begin(result),
                   [] (auto depth, auto duplicate_depth) -> boost::optional<double> {
        if (depth && duplicate_depth) {
            return *depth > 0 ? static_cast<double>(std::min(*duplicate_depth, *depth)) / *depth : 0;
        } else {
            return boost::none;
        }
    });
    return result;
}

Measure::ResultCardinality DuplicateAlleleFraction::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& DuplicateAlleleFraction::do_name() const
{
    return name_;
}

std::string DuplicateAlleleFraction::do_describe() const
{
    return "Fraction of realigned reads supporting ALT alleles identified as duplicates";
}

std::vector<std::string> DuplicateAlleleFraction::do_requirements() const
{

    return {"Samples", "OverlappingReads", "ReadAssignments"};
}

} // namespace csr
} // namespace octopus


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_FREXP_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_FREXP_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/frexp.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/logical_notand.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::frexp_, tag::cpu_, (A0)(A1)(A2)(X)
                                  ,( boost::mpl::and_ <
                                     boost::is_same<A0,A1>,
                                     boost::is_same<typename dispatch::meta::as_integer<A0>::type, A2>
                                     >
                                  )
                            , ((simd_< floating_<A0>, X>))
                              ((simd_< floating_<A1>, X>))
                              ((simd_< integer_<A2>, X>))
                            )
  {
    typedef int result_type;
    inline int operator()(A0 const& a0,A1 & r0,A2 & r1) const
    {
      typedef typename meta::as_logical<A0>::type bA0;
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      typedef typename meta::as_logical<int_type>::type                 bint_type;
      typedef typename meta::scalar_of<int_type>::type                  sint_type;
      typedef typename meta::scalar_of<A0>::type                           s_type;
      const sint_type me = Maxexponent<s_type>()-1;
      const sint_type nmb= Nbmantissabits<s_type>();
      const sint_type n1 = ((2*me+3)<<nmb);
      const sint_type n2 = me<<nmb;
      const int_type vme = splat<int_type>(me);
      A0 ci_exp=simd::bitwise_cast<A0>(splat< int_type>(n1));
      r1 = simd::bitwise_cast<int_type>(b_and(ci_exp, a0));// extract exponent
      A0 x = b_notand(ci_exp, a0);                        // clear exponent in a0
      r1 = sub(shri(r1,nmb), vme);                        // compute exponent
      r0 = b_or(x,splat<int_type>(n2));                   // insert exponent+1 in x
      bA0 test0 = is_nez(a0);
      bint_type test1 = gt(r1,vme);
      r1 = if_else_zero(logical_notand(test1, test0), r1);
      r0 = if_else_zero(test0, seladd(test1,r0,a0));
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::frexp_, tag::cpu_, (A0)(A2)(X)
                                 , ( boost::is_same<typename dispatch::meta::as_integer<A0>::type, A2>)
                                 , ((simd_< floating_<A0>, X>))
                                   ((simd_< integer_<A2>, X>))
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A2 & a2) const
    {
      A0 a1;
      boost::simd::frexp(a0, a1, a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::frexp_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type  exponent;
    typedef boost::fusion::vector<A0,exponent>           result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::simd::frexp( a0
          , boost::fusion::at_c<0>(res)
          , boost::fusion::at_c<1>(res)
          );
      return res;
    }
  };
} } }

#endif


//!
//! Contains common functions used by both gravity and magnetic forward models.
//!
//! \file fwdmodel/gravmag.cpp
//! \author Darren Shen
//! \author Alistair Reid
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#include "fwdmodel/gravmag.hpp"
#include <cmath>
#include <boost/multi_array.hpp>
#include "world/voxelise.hpp"
#include <glog/logging.h>

namespace obsidian
{
  namespace fwd
  {
    GravmagInterpolatorParams makeInterpParams(const VoxelSpec& voxelisation, const Eigen::MatrixXd& locations, const WorldSpec& worldSpec)
    {
      // The interpolation weights
      uint nQuery = locations.rows();
      uint imWidth = voxelisation.xResolution + 2;  // outer grid!
      uint imHeight = voxelisation.yResolution + 2; // outer grid!
      double imMaxX = imWidth - 1; // image coordinates are 0 to xResolution-1
      double imMaxY = imHeight - 1;

      double sensorZ = locations(0, 2);
      Eigen::MatrixXi sensorIndices(nQuery, 4);
      Eigen::MatrixXd sensorWeights(nQuery, 4);
      double halfGridX = (worldSpec.xBounds.second - worldSpec.xBounds.first) / (2.0 * voxelisation.xResolution);
      double halfGridY = (worldSpec.yBounds.second - worldSpec.yBounds.first) / (2.0 * voxelisation.yResolution);
      double L = worldSpec.xBounds.first - halfGridX; // switching to outer grid!
      double R = worldSpec.xBounds.second + halfGridX;
      double T = worldSpec.yBounds.first - halfGridY;
      double B = worldSpec.yBounds.second + halfGridY;

      uint nGrid = imWidth * imHeight; // number of max possible inducing query points
      std::vector<uint> gridIndx(nGrid);  // store a map from grid index to inuse key
      for (uint j = 0; j < nGrid; j++)
        gridIndx[j] = 0; // initialise to zero (unused)

      std::vector<std::pair<uint, uint>> cell2grid; // the [INUSEth] element is the <i,j> coordinate on the grid

      for (uint i = 0; i < nQuery; i++)
      {
        // Note: this needs a refactor as it is used in more than one place
        // Get [double] position in the image - ie if there are
        double xx = imMaxX * (locations(i, 0) - L) / (R - L); // only reaches imMaxX at very final edge
        double yy = imMaxY * (locations(i, 1) - T) / (B - T);
        CHECK(locations(i, 2) == sensorZ); // if this is a problem, then you
        // must have different sensor heights -> can use the commented out stub in gravity to fix

        // Clip so we get continuous edge conditions after the image
        xx = (xx > 0) * xx;
        xx = imMaxX - (xx < imMaxX) * (imMaxX - xx);
        yy = (yy > 0) * yy;
        yy = imMaxY - (yy < imMaxY) * (imMaxY - yy);

        // Get the components for the linear interp
        uint x1 = std::floor(xx);
        uint y1 = std::floor(yy);
        uint x2 = x1 + (xx < imMaxX);
        uint y2 = y1 + (yy < imMaxY);
        double alpha = xx - (double) x1;
        double beta = yy - (double) y1;

        // The order here is x1y1 x2y1 x1y2 x2y2
        sensorWeights(i, 0) = (1 - alpha) * (1 - beta);
        sensorWeights(i, 1) = alpha * (1 - beta);
        sensorWeights(i, 2) = (1 - alpha) * beta;
        sensorWeights(i, 3) = alpha * beta;

        // Dynamically index-map
        // The order here is the same: x1y1 x2y1 x1y2 x2y2
        uint xind[4] = { x1, x2, x1, x2 };
        uint yind[4] = { y1, y1, y2, y2 };
        for (int k = 0; k < 4; k++)
        {
          uint gIndex = xind[k] + yind[k] * imWidth;
          if (gridIndx[gIndex] == 0) // not initialised
          {
            cell2grid.push_back(std::make_pair(xind[k], yind[k]));
            gridIndx[gIndex] = cell2grid.size();
          }
          sensorIndices(i, k) = gridIndx[gIndex] - 1; // zero is starting condition
        }
      }

      // Reconstitute the gridLocations
      uint nGridQry = cell2grid.size();
      Eigen::MatrixXd gridLocations(nGridQry, 3);
      for (uint k = 0; k < nGridQry; k++)
      {
        gridLocations(k, 0) = ((double) cell2grid[k].first) * (R - L) / ((double) imMaxX) + L;
        gridLocations(k, 1) = ((double) cell2grid[k].second) * (B - T) / ((double) imMaxY) + T;
        gridLocations(k, 2) = sensorZ;
      }

      return
      { sensorIndices, sensorWeights, gridLocations}; // interpParams
    }

    Eigen::VectorXd computeField(const Eigen::MatrixXd &sens, const Eigen::MatrixXi sensorIndices, const Eigen::MatrixXd sensorWeights,
                                 const Eigen::VectorXd &properties)
    {
      Eigen::VectorXd rawField = sens * properties;
      uint nQuery = sensorWeights.rows();
      Eigen::VectorXd outField(nQuery);
      for (uint i = 0; i < nQuery; i++)
      {
        // The 4 components of the linear interp
        outField(i) = 0.0;
        for (uint j = 0; j < 4; j++)
          outField(i) += rawField(sensorIndices(i, j)) * sensorWeights(i, j);
      }
      return outField;
    }

    namespace detail
    {
      //! Data structure to represent a 3D array.
      //!
      typedef boost::multi_array<double, 3> Array3d;

      //! Compute the sensitivity integral at a particular point in the mesh
      //!
      //! \param i, j, k The indices of the mesh point
      //! \param eZ The 3D array containing the sensitivities at each mesh grid point
      //!
      double computeSensIntegral(int i, int j, int k, const Array3d &eZ)
      {
        return (eZ[i + 1][j + 1][k + 1] - eZ[i + 1][j + 1][k] - eZ[i + 1][j][k + 1] + eZ[i + 1][j][k])
            - (eZ[i][j + 1][k + 1] - eZ[i][j + 1][k] - eZ[i][j][k + 1] + eZ[i][j][k]);
      }

      Eigen::MatrixXd computeSensitivity(const Eigen::VectorXd &xEdges, const Eigen::VectorXd &yEdges, const Eigen::VectorXd &zEdges,
                                         const Eigen::VectorXd &xField, const Eigen::VectorXd &yField, const Eigen::VectorXd &zField,
                                         const Eigen::MatrixXd &locations,
                                         double (*sensFunc)(double, double, double, double, double, double))
      {
        // Hold the calculation values at each point (or corner of prism)
        detail::Array3d eZ(boost::extents[xEdges.rows()][yEdges.rows()][zEdges.rows()]);

        // The sensitivity matrix contains the sensitivity for each location and prism
        int nPrisms = (xEdges.rows() - 1) * (yEdges.rows() - 1) * (zEdges.rows() - 1);
        Eigen::MatrixXd sens(locations.rows(), nPrisms);

        // For each sensor location
        for (int n = 0; n < locations.rows(); n++)
        {
          // Shift the coordinates so that the sensor location is at the origin
          Eigen::VectorXd x = xEdges.array() - locations(n, 0);
          Eigen::VectorXd y = yEdges.array() - locations(n, 1);
          Eigen::VectorXd z = -(zEdges.array() - locations(n, 2)); // flip z-axis

          // Lazy edge padding for both grav and mag
          double aLongWay = 1e5; // Metres, chosen compromise
          x(0) -= aLongWay;
          y(0) -= aLongWay;
          x(x.rows() - 1) += aLongWay;
          y(y.rows() - 1) += aLongWay;

          // Precompute eZ for each point
          for (int i = 0; i < x.rows(); ++i)
            for (int j = 0; j < y.rows(); ++j)
              for (int k = 0; k < z.rows(); ++k)
                // Cache the value for this point
                eZ[i][j][k] = sensFunc(x(i), y(j), z(k), xField(i), yField(j), zField(k));

          // Compute the sensitivities
          int idx = 0;
          for (int i = 0; i < xEdges.rows() - 1; ++i)
            for (int j = 0; j < yEdges.rows() - 1; ++j)
              for (int k = 0; k < zEdges.rows() - 1; ++k)
                sens(n, idx++) = -detail::computeSensIntegral(i, j, k, eZ);
        }

        return sens;
      }
    } // namespace detail
  } // namespace fwd
} // namespace obsidian


#include "prepare_test_data.h"

#include <benchmark/benchmark.h>
#include <Eigen/Dense>

static void mat4_add(benchmark::State& state) {
    const auto testData = prepare_mat4_test_data_eigen<Eigen::Matrix4f>(state.range(0));

    Eigen::Matrix4f res = Eigen::Matrix4f::Zero();

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = testData[0] + testData[1];
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void mat4_mult(benchmark::State& state) {
    const auto testData = prepare_mat4_test_data_eigen<Eigen::Matrix4f>(state.range(0));

    Eigen::Matrix4f res = Eigen::Matrix4f::Zero();

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = testData[0] * testData[1];
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void mat4_mult_loop(benchmark::State& state) {
    const auto testData = prepare_mat4_test_data_eigen<Eigen::Matrix4f>(state.range(0));

    Eigen::Matrix4f res = Eigen::Matrix4f::Zero();

    for (auto _ : state) {
        benchmark::ClobberMemory();
        for (const auto& vec : testData) {
            res *= vec;
        }
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

static void mat4_mult_loop_accumulate(benchmark::State& state) {
    const auto testData = prepare_mat4_test_data_eigen<Eigen::Matrix4f>(state.range(0));

    Eigen::Matrix4f res = Eigen::Matrix4f::Zero();

    for (auto _ : state) {
        benchmark::ClobberMemory();
        res = std::accumulate(
            testData.begin(), testData.end(), Eigen::Matrix4f(), [](const Eigen::Matrix4f& lhs, const Eigen::Matrix4f& rhs) { return lhs * rhs; });
        benchmark::ClobberMemory();
    }
    benchmark::DoNotOptimize(res);
}

BENCHMARK(mat4_add)->Arg(2);
BENCHMARK(mat4_mult)->Arg(2);
BENCHMARK(mat4_mult_loop)->Arg(2)->Arg(8)->Arg(64)->Arg(1 << 10);
BENCHMARK(mat4_mult_loop_accumulate)->Arg(2)->Arg(8)->Arg(64)->Arg(1 << 10);

// Run the benchmark
BENCHMARK_MAIN();


/* -*- c++ -*- */
/*
 * Copyright 2014 Communications Engineering Lab, KIT.
 *
 * This is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * This software is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include <boost/circular_buffer.hpp>
#include "estimator_rcs_impl.h"
#include <numeric>

namespace gr {
  namespace radar {

    estimator_rcs::sptr
    estimator_rcs::make(int num_mean, float center_freq, float antenna_gain_tx, float antenna_gain_rx, float usrp_gain_rx, float power_tx, float corr_factor, float exponent)
    {
      return gnuradio::get_initial_sptr
        (new estimator_rcs_impl(num_mean, center_freq, antenna_gain_tx, antenna_gain_rx, usrp_gain_rx, power_tx, corr_factor, exponent));
    }

    /*
     * The private constructor
     */
    estimator_rcs_impl::estimator_rcs_impl(int num_mean, float center_freq, float antenna_gain_tx, float antenna_gain_rx, float usrp_gain_rx, float power_tx, float corr_factor, float exponent)
      : gr::block("estimator_rcs",
              gr::io_signature::make(0,0,0),
              gr::io_signature::make(0,0,0))
    {
        d_num_mean = num_mean;
        d_center_freq = center_freq;
        d_antenna_gain_tx = antenna_gain_tx;
        d_antenna_gain_rx = antenna_gain_rx;
        d_usrp_gain_rx = usrp_gain_rx;
        d_power_tx = power_tx; // needs to be calibrated for every usage
        d_corr_factor = corr_factor;
        d_exponent = exponent;

        d_rcs_vals.resize(d_num_mean);

        // Register input message port
        d_port_id_in = pmt::mp("Msg in");
        message_port_register_in(d_port_id_in);
        set_msg_handler(d_port_id_in, boost::bind(&estimator_rcs_impl::handle_msg, this, _1));

        // Register output message port
        d_port_id_out = pmt::mp("Msg out");
        message_port_register_out(d_port_id_out);

        d_loop_counter = 0;

        // constant factors in radar equation
        d_antenna_gain_abs_rx = pow(10, d_antenna_gain_rx/10);
        d_antenna_gain_abs_tx = pow(10, d_antenna_gain_tx/10);
        d_lambda = c_light/d_center_freq;
        d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));

    }

    /*
     * Our virtual destructor.
     */
    estimator_rcs_impl::~estimator_rcs_impl()
    {
    }

    void
    estimator_rcs_impl::set_num_mean(int val){
      d_num_mean = val;
      d_rcs_vals.clear();
      d_rcs_vals.resize(d_num_mean);
      d_loop_counter = 0;
    }

    void
    estimator_rcs_impl::set_center_freq(float val){
      d_center_freq = val;
      d_lambda = c_light/d_center_freq;
      d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));
    }

    void
    estimator_rcs_impl::set_antenna_gain_tx(float val){
      d_antenna_gain_tx = val;
      d_antenna_gain_abs_tx = pow(10, d_antenna_gain_tx/10);
      d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));
    }

    void
    estimator_rcs_impl::set_antenna_gain_rx(float val){
      d_antenna_gain_rx = val;
      d_antenna_gain_abs_rx = pow(10, d_antenna_gain_rx/10);
      d_fak = pow(4.0*M_PI, 3) / (d_antenna_gain_abs_rx * d_antenna_gain_abs_tx * pow(d_lambda, 2));
    }

    void
    estimator_rcs_impl::set_usrp_gain_rx(float val){
      d_usrp_gain_rx = val;
    }

    void
    estimator_rcs_impl::set_power_tx(float val){
      d_power_tx = val;
    }

    void
    estimator_rcs_impl::set_corr_factor(float val){
      d_corr_factor = val;
    }

    float
    estimator_rcs_impl::calculate_vector_mean(boost::circular_buffer<float>* rcs_vals) {
      float sum_of_elems = 0;
      for(int k=0; k<rcs_vals->size(); k++){
        sum_of_elems += (*rcs_vals)[k];
      }
      return sum_of_elems/rcs_vals->size();
    }

    float
    estimator_rcs_impl::calculate_rcs() {
      // catch errors
			if(d_range.size() == 0) throw std::runtime_error("range vector has size zero");
			if(d_power.size() == 0) throw std::runtime_error("power vector has size zero");

      // regard usrp gain and signal path
			float power_rx = pow(d_power[0], d_exponent) / d_power_tx / pow(10, d_usrp_gain_rx/10);

			float fak = d_fak * pow(d_range[0], 4);

      // debug output
      // std::cout << "PowerTx: " << d_power_tx << std::endl;
      // std::cout << "PowerRx: " << power_rx << std::endl;
      // std::cout << "Lambda: " << d_lambda << std::endl;
      // std::cout << "GainRx: " << d_antenna_gain_rx << std::endl;
      // std::cout << "GainTx: " << d_antenna_gain_tx << std::endl;
      // std::cout << "fak: " << fak << std::endl;

			return power_rx/d_power_tx * fak * d_corr_factor;
    }

    void
    estimator_rcs_impl::handle_msg(pmt::pmt_t msg)
    {
        // Read msg from peak detector
        d_msg_hold.clear();
        pmt::pmt_t msg_part;
        bool found_range = false;
        bool found_power = false;
        for(int k=0; k<pmt::length(msg); k++){
          msg_part = pmt::nth(k,msg);
          if(pmt::symbol_to_string(pmt::nth(0,msg_part))=="range"){
            d_prange = pmt::nth(1,msg_part);
            d_msg_hold.push_back(msg_part);
            found_range = true;
          }
          else if(pmt::symbol_to_string(pmt::nth(0,msg_part))=="power"){
            d_ppower = pmt::nth(1,msg_part);
            found_power = true;
          }
          else{
            d_msg_hold.push_back(msg_part);
          }
        }

        if(not(found_range&&found_power)) throw std::runtime_error("range or power identifier (symbol) not found");

        d_range = pmt::f32vector_elements(d_prange);
        d_power = pmt::f32vector_elements(d_ppower);
        d_rcs.clear();

        if(d_range.size()!=d_power.size()) throw std::runtime_error("range and power vectors do not have same size");

        // Calculate RCS
        float rcs_mean = 0.0;
        if(d_range.size() == 0 && d_power.size() == 0){
          //std::cout << "ERROR: No target detected for RCS calculation" << std::endl;
        }
        else {
          d_rcs_vals.push_back(calculate_rcs());
        }
        //std::cout << "RCS_TEMP: " << calculate_rcs() << std::endl;

        if(d_loop_counter+1 >= d_num_mean) {
            rcs_mean = calculate_vector_mean(&d_rcs_vals);
        }
        else {
            d_loop_counter++;
        }

        for(int k=0; k<d_range.size(); k++){
            d_rcs.push_back(rcs_mean);
        }

        // Push pmt to output msg port
        d_rcs_key = pmt::string_to_symbol("rcs"); // identifier velocity
        d_rcs_value = pmt::init_f32vector(d_rcs.size(), d_rcs); // vector to pmt
        d_rcs_pack = pmt::list2(d_rcs_key, d_rcs_value); // make list for velocity information

        d_value = pmt::list1(d_rcs_pack);
        for(int k=0; k<d_msg_hold.size(); k++){
            d_value = pmt::list_add(d_value, d_msg_hold[k]);
        }

        message_port_pub(d_port_id_out,d_value); // publish message
    }

  } /* namespace radar */
} /* namespace gr */


// Boost.Geometry

// Copyright (c) 2018, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_LINE_INTERPOLATE_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_LINE_INTERPOLATE_HPP

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/formulas/interpolate_point_spherical.hpp>
#include <boost/geometry/srs/spheroid.hpp>
#include <boost/geometry/strategies/line_interpolate.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace line_interpolate
{

/*!
\brief Interpolate point on a spherical segment.
\ingroup strategies
\tparam CalculationType \tparam_calculation
\tparam DistanceStrategy The underlying point-point distance strategy

\qbk{
[heading See also]
\* [link geometry.reference.algorithms.line_interpolate.line_interpolate_4_with_strategy line_interpolate (with strategy)]
}

 */
template
<
    typename CalculationType = void,
    typename DistanceStrategy = distance::haversine<double, CalculationType>
>
class spherical
{
public:

    typedef typename DistanceStrategy::radius_type radius_type;

    inline spherical()
    {}

    explicit inline spherical(typename DistanceStrategy::radius_type const& r)
        : m_strategy(r)
    {}

    inline spherical(DistanceStrategy const& s)
        : m_strategy(s)
    {}

    // point-point strategy getters
    struct distance_pp_strategy
    {
        typedef DistanceStrategy type;
    };

    inline typename distance_pp_strategy::type get_distance_pp_strategy() const
    {
        return m_strategy;
    }

    template <typename Point, typename Fraction, typename Distance>
    inline void apply(Point const& p0,
                      Point const& p1,
                      Fraction const& fraction,
                      Point & p,
                      Distance const&) const
    {
        typedef typename select_calculation_type_alt
        <
            CalculationType,
            Point
        >::type calc_t;

        formula::interpolate_point_spherical<calc_t> formula;

        calc_t angle01;
        formula.compute_angle(p0, p1, angle01);
        formula.compute_axis(p0, angle01);

        calc_t a = angle01 * fraction;
        formula.compute_point(a, p);
    }
private :
    DistanceStrategy m_strategy;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<spherical_equatorial_tag>
{
    typedef strategy::line_interpolate::spherical<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::line_interpolate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_LINE_INTERPOLATE_HPP


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ql/indexes/iborindex.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <qle/pricingengines/analyticlgmswaptionengine.hpp>

#include <boost/bind.hpp>

namespace QuantExt {

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<LinearGaussMarkovModel>& model,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->parametrization()),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<CrossAssetModel>& model, const Size ccy,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->irlgm1f(ccy)),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<IrLgm1fParametrization> irlgm1f,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(irlgm1f),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping) {
    registerWith(c_);
}

void AnalyticLgmSwaptionEngine::calculate() const {

    QL_REQUIRE(arguments_.settlementType == Settlement::Physical, "cash-settled swaptions are not supported ...");

    Date reference = p_->termStructure()->referenceDate();

    Date expiry = arguments_.exercise->dates().back();

    if (expiry <= reference) {
        // swaption is expired, possibly generated swap is not
        // valued by this engine, so we set the npv to zero
        results_.value = 0.0;
        return;
    }

    VanillaSwap swap = *arguments_.swap;
    Option::Type type = arguments_.type == VanillaSwap::Payer ? Option::Call : Option::Put;
    Schedule fixedSchedule = swap.fixedSchedule();
    Schedule floatSchedule = swap.floatingSchedule();

    j1_ = std::lower_bound(fixedSchedule.dates().begin(), fixedSchedule.dates().end(), expiry) -
          fixedSchedule.dates().begin();
    k1_ = std::lower_bound(floatSchedule.dates().begin(), floatSchedule.dates().end(), expiry) -
          floatSchedule.dates().begin();

    // compute S_i, i.e. equivalent fixed rate spreads compensating for
    // a) a possibly non-zero float spread and
    // b) a spread between the ibor indices forwarding curve and the
    //     discounting curve
    // here, we do not work with a spread corrections directly, but
    // with this multiplied by the nominal and accrual basis,
    // so S_i is really an amount correction.

    S_.resize(arguments_.fixedCoupons.size() - j1_);
    for (Size i = 0; i < S_.size(); ++i) {
        S_[i] = 0.0;
    }
    S_m1 = 0.0;
    Size ratio = static_cast<Size>(
        static_cast<Real>(arguments_.floatingCoupons.size()) / static_cast<Real>(arguments_.fixedCoupons.size()) + 0.5);
    QL_REQUIRE(ratio >= 1, "floating leg's payment frequency must be equal or "
                           "higher than fixed leg's payment frequency in "
                           "analytic lgm swaption engine");

    Size k = k1_;
    boost::shared_ptr<IborIndex> flatIbor = swap.iborIndex()->clone(c_);
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        Real sum1 = 0.0, sum2 = 0.0;
        for (Size rr = 0; rr < ratio && k < arguments_.floatingCoupons.size(); ++rr, ++k) {
            Real amount = arguments_.floatingCoupons[k];
            Real lambda1 = 0.0, lambda2 = 1.0;
            if (floatSpreadMapping_ == proRata) {
                // we do not use the exact pay dates but the ratio to determine
                // the distance to the adjacent payment dates
                lambda2 = static_cast<Real>(rr + 1) / static_cast<Real>(ratio);
                lambda1 = 1.0 - lambda2;
            }
            if (amount != Null<Real>()) {
                Real flatAmount = flatIbor->fixing(arguments_.floatingFixingDates[k]) *
                                  arguments_.floatingAccrualTimes[k] * arguments_.nominal;
                Real correction = (amount - flatAmount) * c_->discount(arguments_.floatingPayDates[k]);
                sum1 += lambda1 * correction;
                sum2 += lambda2 * correction;
            } else {
                // if no amount is given, we do not need a spread correction
                // due to different forward / discounting curves since then
                // no curve is attached to the swap's ibor index and so we
                // assume a one curve setup;
                // but we can still have a float spread that has to be converted
                // into a fixed leg's payment
                Real correction = arguments_.nominal * arguments_.floatingSpreads[k] *
                                  arguments_.floatingAccrualTimes[k] * c_->discount(arguments_.floatingPayDates[k]);
                sum1 += lambda1 * correction;
                sum2 += lambda2 * correction;
            }
        }
        if (j > j1_) {
            S_[j - j1_ - 1] += sum1 / c_->discount(arguments_.fixedPayDates[j - 1]);
        } else {
            S_m1 += sum1 / c_->discount(arguments_.floatingResetDates[k1_]);
        }
        S_[j - j1_] += sum2 / c_->discount(arguments_.fixedPayDates[j]);
    }

    Real w = type == Option::Call ? -1.0 : 1.0;

    // it is a requirement that H' does not change its sign,
    // with u = -1.0 we handle the case H' < 0
    Real u = p_->Hprime(0.0) > 0.0 ? 1.0 : -1.0;

    // do the actual pricing

    zetaex_ = p_->zeta(p_->termStructure()->timeFromReference(expiry));
    H0_ = p_->H(p_->termStructure()->timeFromReference(arguments_.floatingResetDates[k1_]));
    D0_ = c_->discount(arguments_.floatingResetDates[k1_]);
    Hj_.resize(arguments_.fixedCoupons.size() - j1_);
    Dj_.resize(arguments_.fixedCoupons.size() - j1_);
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        Hj_[j - j1_] = p_->H(p_->termStructure()->timeFromReference(arguments_.fixedPayDates[j]));
        Dj_[j - j1_] = c_->discount(arguments_.fixedPayDates[j - j1_]);
    }

    Brent b;
    Real yStar;
    try {
        yStar = b.solve(boost::bind(&AnalyticLgmSwaptionEngine::yStarHelper, this, _1), 1.0E-6, 0.0, 0.01);
    } catch (const std::exception& e) {
        QL_FAIL("AnalyticLgmSwaptionEngine, failed to compute yStar, " << e.what());
    }

    CumulativeNormalDistribution N;
    Real sqrt_zetaex = std::sqrt(zetaex_);
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += w * (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               N(u * w * (yStar + (Hj_[j - j1_] - H0_) * zetaex_) / sqrt_zetaex);
    }
    sum += -w * S_m1 * D0_ * N(u * w * yStar / sqrt_zetaex);
    sum += w * (arguments_.nominal * Dj_.back() * N(u * w * (yStar + (Hj_.back() - H0_) * zetaex_) / sqrt_zetaex) -
                arguments_.nominal * D0_ * N(u * w * yStar / sqrt_zetaex));
    results_.value = sum;

    results_.additionalResults["fixedAmountCorrectionSettlement"] = S_m1;
    results_.additionalResults["fixedAmountCorrections"] = S_;

} // calculate

Real AnalyticLgmSwaptionEngine::yStarHelper(const Real y) const {
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               std::exp(-(Hj_[j - j1_] - H0_) * y - 0.5 * (Hj_[j - j1_] - H0_) * (Hj_[j - j1_] - H0_) * zetaex_);
    }
    sum += -S_m1 * D0_;
    sum += Dj_.back() * arguments_.nominal *
           std::exp(-(Hj_.back() - H0_) * y - 0.5 * (Hj_.back() - H0_) * (Hj_.back() - H0_) * zetaex_);
    sum -= D0_ * arguments_.nominal;
    return sum;
}

} // namespace QuantExt


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief fail_heterogeneous_unit.cpp

\details
make sure that trying to bind a heterogeneous system to a different dimension fails.

Output:
@verbatim
@endverbatim
**/

#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs.hpp>
//#include <boost/units/systems/conversions/convert_cgs_to_si.hpp>
//#include <boost/units/systems/conversions/convert_si_to_cgs.hpp>

namespace bu = boost::units;

template<class System>
bu::quantity<bu::unit<bu::energy_dimension, System> > f(bu::quantity<bu::unit<bu::length_dimension, System> > l) {
    return(static_cast<bu::quantity<bu::unit<bu::energy_dimension, System> > >(f(static_cast<bu::quantity<bu::si::length> >(l))));
}
bu::quantity<bu::si::energy> f(bu::quantity<bu::si::length> l) {
    return(l * l * 2.0 * bu::si::kilograms / bu::pow<2>(bu::si::seconds));
}

int main() {

    f(1.0 * bu::pow<2>(bu::si::meters) / bu::cgs::centimeters);

    return(0);
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <iostream>                  // for std::cout
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <stdint.h>
#include <stdio.h>
extern "C" {
    #include "c_t_2.h"
    #include "tester.h"
}
#include <map>
#include <sys/time.h>

#ifdef USE_UINT64_T
typedef struct two_ids_thing two_ids_thing, *two_ids_thing_p;

struct two_ids_thing {
    uint64_t source;
    uint64_t target;
    bool operator<(two_ids_thing other) const {
        return source > other.source;
    }
};
#endif

typedef boost::adjacency_list<> Graph;
typedef boost::erdos_renyi_iterator<boost::minstd_rand, Graph> ERGen;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;
typedef boost::graph_traits<Graph> GraphTraits;
typedef boost::graph_traits<Graph>::vertex_iterator vertex_iter;

uint32_t size_walk = 10;
int_type *id_array = (int_type *)malloc(sizeof(id_array[0])*size_walk);
uint32_t node_count = 0;
long long n_count = 50000;
Vertex vt;

float random_walk(Vertex vt, IndexMap index, Graph g, std::map<int_type, int_type> self_map) {
    struct timeval tval_before, tval_after, tval_result;
    gettimeofday(&tval_before, NULL);
    for (int i=0;i<size_walk;i++) {
        boost::graph_traits<Graph>::out_edge_iterator out_i, out_end;
        GraphTraits::edge_descriptor e;
        int e_degree = out_degree(vt, g);
        while (e_degree == 0) {
            int_type r = self_map[random_int(0, n_count-1)];
            std::pair<vertex_iter, vertex_iter> vp;
            vp = vertices(g);
            vp.first = vp.first + r;
            vt = *vp.first;
            e_degree = out_degree(vt, g);
        }
        int random_edge = random_int(0, e_degree-1);
        boost::tie(out_i, out_end) = out_edges(vt, g);
        out_i = out_i + random_edge;
        e = *out_i;
        vt = target(e, g);
    }
    gettimeofday(&tval_after, NULL);
    timersub(&tval_after, &tval_before, &tval_result);
    printf("Time for theirs: %ld.%06d\n", tval_result.tv_sec, tval_result.tv_usec);
    float time_elapsed = ((long int)tval_result.tv_usec + (long int)tval_result.tv_sec/1000000)/1000000.0;
    return time_elapsed;
}
float random_walk_c(val_t vt, cow_trie_p g, std::map<int_type, int_type> map) {
    struct timeval tval_before, tval_after, tval_result;
    gettimeofday(&tval_before, NULL);
    for (int i=0;i<size_walk;i++) {
        uint32_t e_degree = vt._.node.succ_ct;
        while (e_degree == 0) {
            int_type random_vertex = map[random_int(0, n_count-1)];
            cow_trie_lookup(g, random_vertex, &vt);
            e_degree = vt._.node.succ_ct;
        }
        int random_edge = random_int(0, e_degree-1);
        int_type key = edge_list_values(vt._.node.succs)[random_edge];
        cow_trie_lookup(g, key, &vt);
        cow_trie_lookup(g, vt._.edge.succ, &vt);
    }
    gettimeofday(&tval_after, NULL);
    timersub(&tval_after, &tval_before, &tval_result);
    printf("Time for ours: %ld.%06d\n", tval_result.tv_sec, tval_result.tv_usec);
    float time_elapsed = ((long int)tval_result.tv_usec + (long int)tval_result.tv_sec/1000000)/1000000.0;
    return time_elapsed;
}

size_t traverse(cow_trie_p map) {
    size_t count = 0;
    count += map->size;
    for (int i=0;i<count_one_bits(map->child_bitmap);i++) {
        count += traverse(cow_trie_children(map)[i]);
    }
    return count;
}

int main() {
    GraphTraits::out_edge_iterator out_i, out_end;
    GraphTraits::edge_descriptor e;
    boost::graph_traits<Graph>::adjacency_iterator ai;
    boost::graph_traits<Graph>::adjacency_iterator ai_end;
    boost::minstd_rand gen;

    #ifdef USE_UINT64_T
    std::map<two_ids_thing, uint32_t> m;
    #else
    std::map<uint64_t, uint32_t> m;
    #endif
    std::map<int_type, int_type> id_map;
    std::map<int_type, int_type> self_map;

    size_t count = 0;

    uint64_t *v_array = (uint64_t *)malloc(sizeof(uint64_t)*(n_count + n_count*3));
    // Create graph with 100 nodes and edges with probability 0.05
    double prob = n_count*n_count;
    prob = n_count*3/prob;
    Graph g(ERGen(gen, n_count, prob), ERGen(), n_count);

    std::cout << "done generating\n\n";

    IndexMap index = get(boost::vertex_index, g);
    cow_trie_p my_map = cow_trie_alloc(0, 0);
    my_map->ref_count = 1;
    my_map->child_bitmap = 0;
    my_map->value_bitmap = 0;
    int_type id = 0;

    std::cout << "beginning vertex iteration\n\n";
    std::pair<vertex_iter, vertex_iter> vp;
    for (vp = vertices(g); vp.first != vp.second; ++vp.first) {
        Vertex v = *vp.first;
        boost::tie(out_i, out_end) = out_edges(v, g);
        uint32_t deg = out_degree(v, g);
        int_type *succs = new int_type[deg];
        val_p my_val = cow_trie_create_val(0, deg, succs, 0, 0);
        int_type my_id = cow_trie_insert(my_map, *my_val, &my_map);
        free(my_val);
        id_map[id] = my_id;
        self_map[id] = id;
        ++id;
    }

    std::cout << "vertex iteration done, beginning edge iteration\n\n";
    boost::graph_traits<Graph>::edge_iterator ei, ei_end;
    boost::tie(ei, ei_end) = edges(g);
    std::cout << "id before is" << id << "\n\n";
    for (ei; ei != ei_end; ++ei) {
        val_p my_val = cow_trie_create_val(1, 0, 0, id_map[index[source(*ei, g)]], id_map[index[target(*ei, g)]]);
        int_type my_id = cow_trie_insert(my_map, *my_val, &my_map);
        free(my_val);
        #ifdef USE_UINT64_T
        two_ids_thing t;
        t.source = index[source(*ei, g)];
        t.target = index[target(*ei, g)];
        m[t] = my_id;
        #else
        m[index[source(*ei, g)] << 32 | index[target(*ei, g)]] = my_id;
        #endif
        id_map[id] = my_id;
        self_map[id] = id;
        ++id;
    }
    std::cout << "id is\n\n" << id << "\n\n";
    std::cout << "done edge iterating\n\n";
    std::cout << "putting in successors\n\n";
    int p = 0;
    for (vp = vertices(g); vp.first != vp.second; ++vp.first) {
        Vertex v = *vp.first;
        val_t my_val;
        cow_trie_lookup(my_map, id_map[p], &my_val);
        int_type *succs = edge_list_values(my_val._.node.succs);
        boost::tie(out_i, out_end) = out_edges(v, g);
        int eindex = 0;
        for (out_i; out_i != out_end; ++out_i) {
            e = *out_i;
            Vertex src = source(e, g), targ = target(e, g);
            #ifdef USE_UINT64_T
            two_ids_thing t;
            t.source = index[src];
            t.target = index[targ];
            #else
            uint64_t t = index[src] << 32 | index[targ];
            #endif
            int_type id = m[t];
            succs[eindex] = id;
            ++eindex;
        }
        p++;
    }
    val_t v;
    cow_trie_lookup(my_map, 0, &v);
    Vertex vtest = *vertices(g).first;
    std::cout << "beginning random traversal\n\n";
    float t2 = random_walk_c(v, my_map, id_map);
    float t1 = random_walk(vtest, index, g, self_map);
    std::cout << "end random traversal\n\n";
}


/*
 Copyright (c) 2010-2012, Paul Houx - All rights reserved.
 This code is intended for use with the Cinder C++ library: http://libcinder.org

 Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and
	the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
	the following disclaimer in the documentation and/or other materials provided with the distribution.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 POSSIBILITY OF SUCH DAMAGE.
*/

#include "Labels.h"
#include "Conversions.h"

#include "text/FontStore.h"

#include <boost/algorithm/string.hpp>
#include <boost/tokenizer.hpp>

using namespace ci;
using namespace ci::app;
using namespace ph;

Labels::Labels(void)
{
}

Labels::~Labels(void)
{
}

void Labels::setup()
{
	// intialize labels
	text::fonts().loadFont( loadAsset("fonts/Ubuntu-BoldItalic.sdff") );
	mLabels.setFont( text::fonts().getFont("Ubuntu-BoldItalic") );
	mLabels.setFontSize( 16.0f );
	mLabels.setBoundary( text::Text::LINE );
	//mLabels.setOffset( 2.5f, 2.5f );

	double alpha = toRadians( 17.76112222 * 15.0 );
	double delta = toRadians( -29.00780555 );
	Vec3f position = 8330.0 * Vec3f((float) (sin(alpha) * cos(delta)), (float) sin(delta), (float) (cos(alpha) * cos(delta)));

	mLabels.addLabel( position, "Center of the Galaxy" );
}

void Labels::draw()
{
	glPushAttrib( GL_CURRENT_BIT | GL_COLOR_BUFFER_BIT );
	gl::enableAdditiveBlending();
	gl::color( Color::white() * mAttenuation );

	mLabels.draw();

	glPopAttrib();
}

void Labels::setCameraDistance( float distance )
{
	static const float minimum = 0.25f;
	static const float maximum = 1.0f;
	static const float range = 10.0f;

	if( distance > range ) {
		mAttenuation = ci::lerp<float>( minimum, 0.0f, (distance - range) / range );
		mAttenuation = math<float>::clamp( mAttenuation, 0.0f, maximum );
	}
	else {
		mAttenuation = math<float>::clamp( 1.0f - math<float>::log10( distance ) / math<float>::log10(range), minimum, maximum );
	}
}

void Labels::load( DataSourceRef source )
{
	console() << "Loading label database from CSV, please wait..." << std::endl;

	mLabels.clear();

	// load the star database
	std::string	stars = loadString( source );

	// use boost tokenizer to parse the file
	std::vector<std::string> tokens;
	boost::split_iterator<std::string::iterator> lineItr, endItr;
	for (lineItr=boost::make_split_iterator(stars, boost::token_finder(boost::is_any_of("\n\r")));lineItr!=endItr;++lineItr) {
		// retrieve a single, trimmed line
		std::string line = boost::algorithm::trim_copy( boost::copy_range<std::string>(*lineItr) );
		if(line.empty()) continue;

		// split into tokens
		boost::algorithm::split( tokens, line, boost::is_any_of(";"), boost::token_compress_off );

		// skip if data was incomplete
		if(tokens.size() < 23)  continue;

		//
		try {
			// name
			std::string name = boost::trim_copy( tokens[6] );
			//if( name.empty() ) name = boost::trim_copy( tokens[5] );
			//if( name.empty() ) name = boost::trim_copy( tokens[4] );
			if( name.empty() ) continue;

			// position
			double ra = Conversions::toDouble(tokens[7]);
			double dec = Conversions::toDouble(tokens[8]);
			float distance = Conversions::toFloat(tokens[9]);

			double alpha = toRadians( ra * 15.0 );
			double delta = toRadians( dec );

			Vec3f position = distance * Vec3f((float) (sin(alpha) * cos(delta)), (float) sin(delta), (float) (cos(alpha) * cos(delta)));

			mLabels.addLabel( position, name );
		}
		catch(...) {
			// some of the data was invalid, ignore
			continue;
		}
	}
}

void Labels::read(DataSourceRef source)
{
	IStreamRef in = source->createStream();

	mLabels.clear();

	uint8_t versionNumber;
	in->read( &versionNumber );

	uint32_t numLabels;
	in->readLittle( &numLabels );

	for( size_t idx = 0; idx < numLabels; ++idx ) {
		Vec3f position;
		in->readLittle( &position.x ); in->readLittle( &position.y ); in->readLittle( &position.z );
		std::string name;
		in->read( &name );

		mLabels.addLabel( position, name );
	}
}

void Labels::write(DataTargetRef target)
{
	OStreamRef out = target->getStream();

	const uint8_t versionNumber = 1;
	out->write( versionNumber );

	out->writeLittle( static_cast<uint32_t>( mLabels.size() ) );

	for( text::TextLabelListConstIter it = mLabels.begin(); it != mLabels.end(); ++it ) {
		Vec3f position = it->first;
		out->writeLittle( position.x ); out->writeLittle( position.y ); out->writeLittle( position.z );
		std::string name = toUtf8( it->second );
		out->write( name );
	}
}


// Copyright (c) 2015-2017 The Worldcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <arith_uint256.h>
#include <worldcoin.h>
#include <pow.h>
#include <util.h>

// As Worldcoin calculate block reward using trigonometric functions to follow the Earth rotation ...
#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

int static generateMTRandom(unsigned int s, int range)
{
    boost::mt19937 gen(s);
    boost::uniform_int<> dist(1, range);
    return dist(gen);
}

// Worldcoin: Normally minimum difficulty blocks can only occur in between
// retarget blocks. However, once we introduce Digishield every block is
// a retarget, so we need to handle minimum difficulty on all blocks.
bool AllowDigishieldMinDifficultyForBlock(const CBlockIndex* pindexLast, const CBlockHeader *pblock, const Consensus::Params& params)
{
   // never allow minimal difficulty, WDC retarget works fine
    return false;
}

//unsigned int CalculateWorldcoinNextWorkRequired(const CBlockIndex* pindexLast, int64_t nFirstBlockTime, const Consensus::Params& params)
//{
//    // do not change retarget algo
//    return CalculateNextWorkRequired(pindexLast, nFirstBlockTime, params);
//}

bool CheckAuxPowProofOfWork(const CBlockHeader& block, const Consensus::Params& params)
{
    // check legacy blocks and non-AUXPOW
    if ( block.IsLegacy() || !block.IsAuxpow() ) {
        if (block.auxpow)
            return error("%s : legacy block with auxpow properties",
                         __func__);
        return CheckProofOfWork(block.GetPoWHash(), block.nBits, params);
    }

    /* We have auxpow.  Check it.  */
    if (!block.auxpow)
            return error("%s : auxpow block without auxpow properties",
                         __func__);

    /* Except for legacy blocks with full version 1, ensure that
       the chain ID is correct.  Legacy blocks are not allowed since
       the merge-mining start, which is checked in AcceptBlockHeader
       where the height is known.  */
    if (params.fStrictChainId && (block.GetChainId() != params.nAuxpowChainId) )
        return error("%s : block does not have our chain ID"
                     " (got %d, expected %d, full nVersion %d)",
                     __func__, block.GetChainId(),
                     params.nAuxpowChainId, block.nVersion);

    if (!block.auxpow->check(block.GetHash(), block.GetChainId(), params))
        return error("%s : AUX POW is not valid", __func__);
    if (!CheckProofOfWork(block.auxpow->getParentBlockPoWHash(), block.nBits, params))
        return error("%s : AUX proof of work failed", __func__);

    return true;
}

unsigned int CalculateWorldcoinNextWorkRequired(const CBlockIndex* pindexLast, int64_t nFirstBlockTime, const Consensus::Params& params)
{
    if (params.fPowNoRetargeting)
        return pindexLast->nBits;

    int nHeight = pindexLast->nHeight + 1;
    int64_t retargetTimespan = params.nPowTargetTimespan;
    int64_t nActualTimespan = pindexLast->GetBlockTime() - nFirstBlockTime;
    int64_t nModulatedTimespan = nActualTimespan;
    int64_t nMaxTimespan;
    int64_t nMinTimespan;

    bool fNewDifficultyProtocol = (nHeight >= params.nDiffChangeTarget);

    if (fNewDifficultyProtocol) {
        retargetTimespan = params.nTargetTimespanRe;
        nMinTimespan = (retargetTimespan - (retargetTimespan/10));
        nMaxTimespan = (retargetTimespan + (retargetTimespan/10));
    } else {
        retargetTimespan = params.nPowTargetTimespan;
        nMinTimespan = retargetTimespan/4;
        nMaxTimespan = retargetTimespan*4;
    }

    // Limit adjustment step
    if (nModulatedTimespan < nMinTimespan)
        nModulatedTimespan = nMinTimespan;
    else if (nModulatedTimespan > nMaxTimespan)
        nModulatedTimespan = nMaxTimespan;

    // Retarget
    arith_uint256 bnNew;
    arith_uint256 bnOld;
    bnNew.SetCompact(pindexLast->nBits);
    bnOld = bnNew;
    // litecoin: intermediate uint256 can overflow by 1 bit
    const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);
    bool fShift = bnNew.bits() > bnPowLimit.bits() - 1;
    if (fShift)
        bnNew >>= 1;
    bnNew *= nModulatedTimespan;
    bnNew /= retargetTimespan;
    if (fShift)
        bnNew <<= 1;

    if (bnNew > bnPowLimit)
        bnNew = bnPowLimit;

    return bnNew.GetCompact();
}

CAmount GetWorldcoinBlockSubsidy(int nHeight, const Consensus::Params& consensusParams)
{
    const CAmount nMinSubsidy = 1 * COIN;
    // base payout
    CAmount nSubsidy = 50 * COIN;
    // bypass, don't use this function
    return nSubsidy;
}


// Copyright (c) 2005 - 2015 Marc de Kamps
// 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 copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#ifndef _CODE_LIBS_TWODLIB_MASTERGRIDJUMP_INCLUDE_GUARD
#define _CODE_LIBS_TWODLIB_MASTERGRIDJUMP_INCLUDE_GUARD

#include <string>
#include <boost/numeric/odeint.hpp>
#include "CSRMatrix.hpp"
#include "TransitionMatrix.hpp"
#include "Ode2DSystemGroup.hpp"
#include "MasterParameter.hpp"

namespace TwoDLib {

	//! OpenMP version of a forward Euler integration of the Master equation

	class MasterGridJump {
	public:

		MasterGridJump
		(
			Ode2DSystemGroup&,
			double
		);

		void MVGrid(
			vector<double>&       dydt,
			const vector<double>& vec_mass,
			double                rate,
			unsigned int          efficiacy_index) const;

		void CalculateStaticEfficiacies(vector<double>& efficacy_map);
		void CalculateStaticEfficiaciesForConductance(vector<double>& efficacy_map, vector<double>& rest_v);

		void Apply(double t_step, const vector<double>& rates);

		void operator()(const vector<double>&, vector<double>&, const double t = 0);

	private:

		MasterGridJump& operator=(const MasterGridJump&);

		Ode2DSystemGroup& _sys;

		double _cell_width;

		vector<double>			_dydt;
		vector<vector<double>>		_stays;
		vector<vector<double>>		_goes;
		vector<vector<int>>				_offset1;
		vector<vector<int>>				_offset2;

		const vector<double>* _p_vec_rates;
	};
}

#endif // include guard


#include <iostream>
#include <algorithm>    // std::unique_copy, std::sort, std::distance
#include <random>
#include <GraphMol/MolOps.h>
#include <GraphMol/GraphMol.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/SmilesParse/SmilesWrite.h>
#include <GraphMol/Subgraphs/Subgraphs.h>
#include <GraphMol/Subgraphs/SubgraphUtils.h>
#include "sha1.hpp"
#include <boost/uuid/sha1.hpp>

using namespace RDKit;
struct compare
{
    int key;
    compare(int const &i): key(i) { }

    bool operator()(int const &i)
    {
        return (i == key);
    }
};

struct distrib{
	std::vector<uint64_t> da;
	std::vector<uint64_t> db;
	long int max_hash;
	long long int prime;
};

// generator of distribution for Hashing
distrib generatordistrib(int seed, int distribsize){

	distrib dab;
	long int max_hash = std::llround(std::pow(2,32)) - 1; // fix
    long long int prime = std::llround(std::pow(2,61)) -1; // fix
 	std::vector<uint64_t> v1;
 	std::vector<uint64_t> v2;

	// generator 1:
    std::mt19937::result_type myseed = seed;
    std::mt19937 e1(myseed); //_64 to have 64 bits double not needed
    std::uniform_int_distribution<long long int> dist1(1, max_hash); // fix dist1
    while (v1.size()<distribsize) {
    	long long int a = dist1(e1);
        bool result = !std::any_of(v1.begin(),v1.end(),compare(a));
        if (result) {
        	v1.push_back(a);

    	}
    }

    // generator 2:
    std::mt19937 e2(myseed); //_64 to have 64 bits double not needed
    std::uniform_int_distribution<long long int> dist2(0, max_hash); // fix dist2
    while (v2.size()<distribsize) {
    	long long int b = dist2(e2);
        bool result = !std::any_of(v2.begin(),v2.end(),compare(b));
        if (result) {
        	v2.push_back(b);
    	}
    }

    dab.da=v1;
    dab.db=v2;
    dab.prime=prime;
    dab.max_hash=max_hash;
    return dab;

}

std::vector<std::string> shingling_from_mol(RDKit::ROMol *in_mol, int radius, bool rings, bool kekulize) {
    std::vector<std::string> shingling;
    if (rings){
    	PATH_TYPE bonds;
		RDKit::VECT_INT_VECT molrings;
  		RDKit::MolOps::symmetrizeSSSR(*in_mol, molrings);
  		for (const auto molring : molrings) {
  			for (const auto index1: molring) {
  				//std::cout << "index1 : "<< index1 << std::endl;
      			for (const auto index2: molring) {
      				//std::cout << "index2 : "<< index2 << std::endl;
      				if(index2!=index1) {
      					const Bond *bnd = in_mol->getBondBetweenAtoms(index1, index2);
      					if (bnd) {
      						bool result = !std::any_of(bonds.begin(),bonds.end(),compare(bnd->getIdx()));
      						if (result) {
      							bonds.push_back(bnd->getIdx());
      						}
                       	}
                    }
                }
    		}
  		}

  		// by default canonical MolToSmiles
  		RDKit::ROMol *submol = Subgraphs::pathToSubmol(*in_mol, bonds);
  		std::string subsmi = RDKit::MolToSmiles(*submol);
        shingling.push_back(subsmi);
	}

	PATH_TYPE p;
	std::string smiles;
    for (int index = 0; index < in_mol->getNumAtoms(); index++){
      for (int i = 1; i < radius + 1;  i++){
      	p = RDKit::findAtomEnvironmentOfRadiusN(*in_mol, i, index);
        INT_MAP_INT amap; // map
        ROMol *submol = Subgraphs::pathToSubmol(*in_mol, p, false, amap);
        // check if the atomindex match the current index value!
        if (amap.count(index) > 0){
	        smiles = RDKit::MolToSmiles(*submol, true, false, amap[index], true);
	        if (!smiles.empty()){
	           shingling.push_back(smiles);
	        }
        }
      }
	}
	// remove duplicates then!
	sort (shingling.begin(), shingling.end());
    shingling.erase(std::unique(shingling.begin(), shingling.end()), shingling.end());
    return shingling;
}

std::vector<uint32_t> from_molecular_shingling(std::vector<std::string> token, distrib dab, int FPsize){
	// set the hash_values
	std::vector<uint32_t> hash_values;

	for (int i=0;i<FPsize;i++){
		hash_values.push_back(dab.max_hash);
	}
	// generate hashes
	for(const auto res : token){
		boost::uuids::detail::sha1 sha1;
	    unsigned int hash[5];
	    sha1.process_bytes(res.c_str(), sizeof(res.c_str())-1);
	    sha1.get_digest(hash);
	    uint32_t t_h = hash[0];

		//SHA1 checksum;
    	//checksum.update(res);
    	//uint32_t t_h = checksum.Hash(); // not same as in python
        std::cout << "t_h : "<< t_h << std::endl;

    	for (int j=0;j<FPsize;j++){
			hash_values[j] = fmin( hash_values[j] , remainder( remainder( dab.da[j] * t_h + dab.db[j], dab.prime) , dab.max_hash) );
		}
	}
	return hash_values;
}

int main()
{
	int FPsize = 2048;
    distrib dab = generatordistrib(42, FPsize);
    //std::cout << dab.da.size()<< ", " << dab.db.size()<< std::endl ;
	std::cout << "prime : "<< dab.prime << std::endl;
	std::cout << "maxhash : "<< dab.max_hash << std::endl;

	RDKit::ROMol *mol1 = RDKit::SmilesToMol("c1ccccc1");

	std::vector<std::string> res = shingling_from_mol(mol1, 3, true, true);

    std::vector<uint32_t> hash_values = from_molecular_shingling(res, dab, FPsize);
	for (int i=0;i<FPsize;i++){
		std::cout <<  hash_values[i]  << ",";
	}

	std::cout << std::endl;

}


/*****************************************************************************/
/*  Copyright (c) 2015, Karl Pauwels                                         */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include <multi_rigid_detector.h>
#include <stdexcept>
#undef Success
#include <Eigen/Geometry>

namespace interface {

MultiRigidDetector::MultiRigidDetector(int image_width, int image_height,
                                       cv::Mat camera_matrix,
                                       std::vector<std::string> obj_filenames,
                                       int device_id, Parameters parameters)
    : image_width_(image_width), image_height_(image_height),
      parameters_(parameters) {
  double nodal_point_x = camera_matrix.at<double>(0, 2);
  double nodal_point_y = camera_matrix.at<double>(1, 2);
  double focal_length_x = camera_matrix.at<double>(0, 0);
  double focal_length_y = camera_matrix.at<double>(1, 1);

  d_multiple_rigid_pose_sparse_ =
      std::unique_ptr<pose::D_MultipleRigidPoseSparse>(
          new pose::D_MultipleRigidPoseSparse(
              image_width_, image_height_, nodal_point_x, nodal_point_y,
              focal_length_x, focal_length_y, device_id, parameters_.vec_size_,
              parameters_.num_iter_ransac_));

  for (auto &it : obj_filenames)
    d_multiple_rigid_pose_sparse_->addModel(it.c_str());
}

void MultiRigidDetector::estimatePose(const cv::Mat &image, int object_index,
                                      pose::TranslationRotation3D &pose) {
  if (image.type() != CV_8UC1)
    throw std::runtime_error(std::string(
        "MultiRigidDetector::updatePoses:: image must be CV_8UC1\n"));
  if ((image.rows != image_height_) || (image.cols != image_width_))
    throw std::runtime_error(std::string(
        "MultiRigidDetector::updatePoses:: incorrect image size\n"));
  if (object_index >= getNumberOfObjects())
    throw std::runtime_error(std::string(
        "MultiRigidDetector::updatePoses:: object index out of range\n"));

  pose = d_multiple_rigid_pose_sparse_->estimatePoseSpecificObject(
      image, object_index);
}

void MultiRigidDetector::estimatePose(const cv::Mat &image, int object_index,
                                      geometry_msgs::Pose &pose) {
  pose::TranslationRotation3D pose_tr;
  estimatePose(image, object_index, pose_tr);

  double t[3];
  pose_tr.getT(t);
  double x, y, z, w;
  pose_tr.getQuaternion(x, y, z, w);

  pose.position.x = t[0];
  pose.position.y = t[1];
  pose.position.z = t[2];
  pose.orientation.x = x;
  pose.orientation.y = y;
  pose.orientation.z = z;
  pose.orientation.w = w;
}

void MultiRigidDetector::setCameraMatrix(const cv::Mat &camera_matrix) {
  double nodal_point_x = camera_matrix.at<double>(0, 2);
  double nodal_point_y = camera_matrix.at<double>(1, 2);
  double focal_length_x = camera_matrix.at<double>(0, 0);
  double focal_length_y = camera_matrix.at<double>(1, 1);

  d_multiple_rigid_pose_sparse_->updateCalibration(
      image_width_, image_height_, nodal_point_x, nodal_point_y, focal_length_x,
      focal_length_y);
}

void MultiRigidDetector::setObjects(std::vector<std::string> obj_filenames) {
  d_multiple_rigid_pose_sparse_->removeAllModels();
  for (auto &it : obj_filenames)
    d_multiple_rigid_pose_sparse_->addModel(it.c_str());
}

int MultiRigidDetector::getNumberOfObjects() {
  return (d_multiple_rigid_pose_sparse_->getNumberOfObjects());
}
}


/*
 * Copyright (c) 2013 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

#include <gtest/gtest.h>

#include <qi/clock.hpp>
#include <qi/os.hpp>
#include <boost/thread.hpp>
#include <thread>
#include <chrono>

namespace chrono = boost::chrono;

TEST(QiClock, initialization)
{
  qi::Duration d0 = qi::Duration::zero();
  EXPECT_EQ(0, d0.count());

  qi::Duration d1(0);
  EXPECT_EQ(0, d1.count());

  qi::Clock::time_point t0;
  EXPECT_EQ(0, t0.time_since_epoch().count());

  qi::Clock::time_point t1(qi::Duration::zero());
  EXPECT_EQ(0, t1.time_since_epoch().count());

  qi::Clock::time_point t2 = qi::Clock::time_point();
  EXPECT_EQ(0, t2.time_since_epoch().count());
}

//TEST(QiClock, clock_sleep)
//{
//  typedef chrono::duration<int64_t, boost::pico> picoseconds;
//  qi::sleepFor(chrono::milliseconds(1));
//  qi::sleepFor(chrono::milliseconds(1) + picoseconds(1));
//  qi::sleepFor(picoseconds(1));

//  qi::sleepUntil(qi::SteadyClock::now() + chrono::seconds(1));
//  qi::sleepUntil(qi::SteadyClock::now());
//  qi::sleepUntil(qi::SteadyClock::now() - chrono::seconds(1));

//  qi::sleepUntil(qi::SystemClock::now() + chrono::seconds(1));
//  qi::sleepUntil(qi::SystemClock::now());
//  qi::sleepUntil(qi::SystemClock::now() - chrono::seconds(1));
//}

TEST(QiClock, clock_sleep_our)
{
  qi::sleepUntil(qi::SteadyClock::now() + qi::Seconds(1));
  qi::sleepUntil(qi::SteadyClock::now());
  qi::sleepUntil(qi::SteadyClock::now() - qi::Seconds(1));

  qi::sleepUntil(qi::Clock::now() + qi::Seconds(1));
  qi::sleepUntil(qi::Clock::now());
  qi::sleepUntil(qi::Clock::now() - qi::Seconds(1));

  qi::sleepUntil(qi::SystemClock::now() + qi::Seconds(1));
  qi::sleepUntil(qi::SystemClock::now());
  qi::sleepUntil(qi::SystemClock::now() - qi::Seconds(1));
}

using uint32ms = chrono::duration<uint32_t, boost::milli>;

TEST(QiClock, tofromUint32ms)
{
  // a test to show how we can convert from 32-bits guess-what-my-epoch-is
  // time stamps to qi::SteadyClock::time_point
  qi::Clock::duration period =
      qi::Clock::duration(uint32ms::max()) + qi::Clock::duration(uint32ms(1));
  qi::Clock::duration eps = chrono::milliseconds(4);

  // check sum-ms "noise" is removed
  qi::Clock::duration noise = chrono::nanoseconds(654321);
  qi::Clock::time_point t(period/4);
  uint32_t input_ms = chrono::duration_cast<uint32ms>(t.time_since_epoch()).count();
  EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t+noise, qi::Clock::Expect_SoonerOrLater));
  EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t-noise, qi::Clock::Expect_SoonerOrLater));

  // check we get the expected values
  for (int i = 0; i<8; ++i) {
    t = qi::Clock::time_point((i*period)/4);
    input_ms = chrono::duration_cast<uint32ms>(t.time_since_epoch()).count();

    EXPECT_EQ(input_ms, qi::Clock::toUint32ms(t));

    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t, qi::Clock::Expect_SoonerOrLater));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t + eps, qi::Clock::Expect_SoonerOrLater));
    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t + period/2, qi::Clock::Expect_SoonerOrLater));
    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t + period/2 + eps, qi::Clock::Expect_SoonerOrLater));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t - eps, qi::Clock::Expect_SoonerOrLater));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t - period/2 + eps, qi::Clock::Expect_SoonerOrLater));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t - period/2, qi::Clock::Expect_SoonerOrLater));
    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t - period/2 - eps, qi::Clock::Expect_SoonerOrLater));

    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t - eps, qi::Clock::Expect_Sooner));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t, qi::Clock::Expect_Sooner));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t + eps, qi::Clock::Expect_Sooner));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t + period - eps, qi::Clock::Expect_Sooner));
    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t + period, qi::Clock::Expect_Sooner));
    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t + period + eps, qi::Clock::Expect_Sooner));

    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t - period - eps, qi::Clock::Expect_Later));
    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t - period, qi::Clock::Expect_Later));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t - period + eps, qi::Clock::Expect_Later));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t - eps, qi::Clock::Expect_Later));
    EXPECT_TRUE(t == qi::Clock::fromUint32ms(input_ms, t, qi::Clock::Expect_Later));
    EXPECT_FALSE(t == qi::Clock::fromUint32ms(input_ms, t + eps, qi::Clock::Expect_Later));
  }
}

TEST(QiClock, toIso8601String)
{
  qi::SystemClockTimePoint t0(qi::Duration(0));
  EXPECT_EQ("1970-01-01T000000.000Z", qi::toISO8601String(t0));
  // we do not round, we ceil:
  EXPECT_EQ("1970-01-01T000000.000Z", qi::toISO8601String(t0 + qi::MicroSeconds(999)));
  EXPECT_EQ("1970-01-01T000001.042Z", qi::toISO8601String(t0 + qi::MilliSeconds(1042)));
  EXPECT_EQ("1970-02-01T010203.000Z", qi::toISO8601String(t0 + qi::Hours(24*31+1)+qi::Minutes(2)+qi::Seconds(3)));
}

TEST(QiClock, to_string)
{
  // durations defined in libqi
  EXPECT_EQ("2 nanoseconds", qi::to_string(qi::Duration(2)));
  EXPECT_EQ("2 nanoseconds", qi::to_string(qi::NanoSeconds(2)));
  EXPECT_EQ("2 microseconds", qi::to_string(qi::MicroSeconds(2)));
  EXPECT_EQ("2 milliseconds", qi::to_string(qi::MilliSeconds(2)));
  EXPECT_EQ("2 seconds", qi::to_string(qi::Seconds(2)));
  EXPECT_EQ("2 minutes", qi::to_string(qi::Minutes(2)));
  EXPECT_EQ("2 hours", qi::to_string(qi::Hours(2)));

  // time_points defined in libqi
  EXPECT_EQ("2 nanoseconds since program start",
            qi::to_string(qi::SteadyClock::time_point(qi::Duration(2))));
  EXPECT_EQ("2 nanoseconds since boot",
            qi::to_string(qi::Clock::time_point(qi::Duration(2))));
  EXPECT_EQ("2 nanoseconds since Jan 1, 1970",
            qi::to_string(qi::SystemClock::time_point(qi::Duration(2))));

  // custom duration
  EXPECT_EQ("0.5 seconds", qi::to_string(boost::chrono::duration<float, boost::ratio<1,1>>(0.5f)));

  // custom time_point for a clock defined in libqi
  EXPECT_EQ("2 hours since Jan 1, 1970",
            qi::to_string(boost::chrono::time_point_cast<qi::Hours>(qi::SystemClock::time_point(qi::Hours(2)))));
}

TEST(QiClock, durationSince)
{
  qi::SteadyClock::time_point tp = qi::SteadyClock::now();

  const qi::Duration sleepDuration = qi::MilliSeconds(500);
  const qi::MilliSeconds sleepDurationMs = boost::chrono::duration_cast<qi::MilliSeconds>(sleepDuration);
  const qi::MicroSeconds sleepDurationUs = boost::chrono::duration_cast<qi::MicroSeconds>(sleepDuration);

  boost::this_thread::sleep_for(sleepDuration);
  const qi::MilliSeconds durMs = qi::durationSince<qi::MilliSeconds>(tp);
  const qi::MicroSeconds durUs = qi::durationSince<qi::MicroSeconds>(tp);
  const qi::Duration tol = qi::MilliSeconds(1); // only needed on Windows
  EXPECT_GE(durMs + tol, sleepDurationMs);
  EXPECT_GE(durUs + tol, sleepDurationUs);
}


// Polymorphic arrays example

#include <blitz/array.h>

using namespace blitz;

class Material {
public:
    virtual double density() const = 0;
};

class Air {
public:
    virtual double density() const { return 0.291; }
};

class Water {
public:
    virtual double density() const { return 0.335; }
};

int main()
{
    Array<Material*,1> A(4);
}


#pragma once

// C++ standard library
#include <functional>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/armadillo.hpp"
// IWYU pragma: no_forward_declare mant::Hash
// IWYU pragma: no_forward_declare mant::IsEqual

namespace mant {
  class OptimisationProblem {
   public:
    const arma::uword numberOfDimensions_;

    explicit OptimisationProblem(
        const arma::uword numberOfDimensions);

    void setObjectiveFunctions(
        const std::vector<std::pair<
            std::function<double(
                const arma::vec& parameter_)>,
            std::string>>& objectiveFunctions);

    std::vector<std::pair<std::function<double(const arma::vec& parameter_)>, std::string>> getObjectiveFunctions() const;

    double getObjectiveValue(
        arma::vec parameter);
    double getObjectiveValueOfNormalisedParameter(
        const arma::vec& normalisedParameter);

    void setLowerBounds(
        const arma::vec& lowerBounds);
    arma::vec getLowerBounds() const;

    void setUpperBounds(
        const arma::vec& upperBounds);
    arma::vec getUpperBounds() const;

    void setParameterPermutation(
        const arma::uvec& parameterPermutation);
    arma::uvec getParameterPermutation() const;

    void setParameterScaling(
        const arma::vec& parameterScaling);
    arma::vec getParameterScaling() const;

    void setParameterTranslation(
        const arma::vec& parameterTranslation);
    arma::vec getParameterTranslation() const;

    void setParameterRotation(
        const arma::mat& parameterRotation);
    arma::mat getParameterRotation() const;

    void setMinimalParameterDistance(
        const arma::vec& minimalParameterDistance);
    arma::vec getMinimalParameterDistance() const;

    void setObjectiveValueScaling(
        const double objectiveValueScaling);
    double getObjectiveValueScaling() const;

    void setObjectiveValueTranslation(
        const double objectiveValueTranslation);
    double getObjectiveValueTranslation() const;

    std::unordered_map<arma::vec, double, Hash, IsEqual> getCachedSamples() const;

    arma::uword getUsedNumberOfEvaluations() const;
    arma::uword getUsedNumberOfDistinctEvaluations() const;

    void reset();

   protected:
    std::vector<std::pair<
        std::function<double(
            const arma::vec& parameter_)>,
        std::string>>
        objectiveFunctions_;

    arma::vec lowerBounds_;
    arma::vec upperBounds_;

    arma::uvec parameterPermutation_;
    arma::vec parameterScaling_;
    arma::vec parameterTranslation_;
    arma::mat parameterRotation_;
    arma::vec minimalParameterDistance_;

    double objectiveValueScaling_;
    double objectiveValueTranslation_;

    std::unordered_map<arma::vec, double, Hash, IsEqual> cachedSamples_;

    arma::uword usedNumberOfEvaluations_;
    arma::uword usedNumberOfDistinctEvaluations_;
  };
}