/*
  Copyright 2008 Larry Gritz and the other authors and contributors.
  All Rights Reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are
  met:
  * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
  * Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
  * Neither the name of the software's owners 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.

  (This is the Modified BSD License)
*/

/// \file
/// ImageBufAlgo functions for filtered transformations

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

#include <OpenEXR/half.h>
#include <OpenEXR/ImathMatrix.h>
#include <OpenEXR/ImathBox.h>

#include <cmath>

#include "OpenImageIO/imagebuf.h"
#include "OpenImageIO/imagebufalgo.h"
#include "OpenImageIO/imagebufalgo_util.h"
#include "OpenImageIO/dassert.h"
#include "OpenImageIO/filter.h"
#include "OpenImageIO/thread.h"

OIIO_NAMESPACE_ENTER {

namespace {

// Poor man's Dual2<float> makes it easy to compute with differentials. For
// a rich man's implementation and full documentation, see
// OpenShadingLanguage (dual2.h).
class Dual2 {
public:
    float val() const { return m_val; }
    float dx() const { return m_dx; }
    float dy() const { return m_dy; }
    Dual2 (float val) : m_val(val), m_dx(0.0f), m_dy(0.0f) {}
    Dual2 (float val, float dx, float dy) : m_val(val), m_dx(dx), m_dy(dy) {}
    Dual2& operator= (float f) { m_val = f; m_dx = 0.0f; m_dy = 0.0f; return *this; }
    friend Dual2 operator+ (const Dual2 &a, const Dual2 &b) {
        return Dual2 (a.m_val+b.m_val, a.m_dx+b.m_dx, a.m_dy+b.m_dy);
    }
    friend Dual2 operator+ (const Dual2 &a, float b) {
        return Dual2 (a.m_val+b, a.m_dx, a.m_dy);
    }
    friend Dual2 operator* (const Dual2 &a, float b) {
        return Dual2 (a.m_val*b, a.m_dx*b, a.m_dy*b);
    }
    friend Dual2 operator* (const Dual2 &a, const Dual2 &b) {
        // Use the chain rule
        return Dual2 (a.m_val*b.m_val,
                      a.m_val*b.m_dx + a.m_dx*b.m_val,
                      a.m_val*b.m_dy + a.m_dy*b.m_val);
    }
    friend Dual2 operator/ (const Dual2 &a, const Dual2 &b) {
        float bvalinv = 1.0f / b.m_val;
        float aval_bval = a.m_val * bvalinv;
        return Dual2 (aval_bval,
                      bvalinv * (a.m_dx - aval_bval * b.m_dx),
                      bvalinv * (a.m_dy - aval_bval * b.m_dy));
    }
private:
    float m_val, m_dx, m_dy;
};

/// Transform a 2D point (x,y) with derivatives by a 3x3 affine matrix to
/// obtain a transformed point with derivatives.
inline void
robust_multVecMatrix (const Imath::M33f &M,
                      const Dual2 &x, const Dual2 &y,
                      Dual2 &outx, Dual2 &outy)
{
    Dual2 a = x * M[0][0] + y * M[1][0] + M[2][0];
    Dual2 b = x * M[0][1] + y * M[1][1] + M[2][1];
    Dual2 w = x * M[0][2] + y * M[1][2] + M[2][2];

    if (w.val() != 0.0f) {
       outx = a / w;
       outy = b / w;
    } else {
       outx = 0.0f;
       outy = 0.0f;
    }
}

// Transform an ROI by an affine matrix.
ROI
transform (const Imath::M33f &M, ROI roi)
{
    Imath::V2f ul (roi.xbegin+0.5f, roi.ybegin+0.5f);
    Imath::V2f ur (roi.xend-0.5f, roi.ybegin+0.5f);
    Imath::V2f ll (roi.xbegin+0.5f, roi.yend-0.5f);
    Imath::V2f lr (roi.xend-0.5f, roi.yend-0.5f);
    M.multVecMatrix (ul, ul);
    M.multVecMatrix (ur, ur);
    M.multVecMatrix (ll, ll);
    M.multVecMatrix (lr, lr);
    Imath::Box2f box (ul);
    box.extendBy (ll);
    box.extendBy (ur);
    box.extendBy (lr);
    int xmin = int (floorf(box.min.x));
    int ymin = int (floorf(box.min.y));
    int xmax = int (floorf(box.max.x)) + 1;
    int ymax = int (floorf(box.max.y)) + 1;
    return ROI (xmin, xmax, ymin, ymax, roi.zbegin, roi.zend, roi.chbegin, roi.chend);
}

// Given s,t image space coordinates and their derivatives, compute a
// filtered sample using the derivatives to guide the size of the filter
// footprint.
template<typename SRCTYPE>
inline void
filtered_sample (const ImageBuf &src, float s, float t,
                 float dsdx, float dtdx, float dsdy, float dtdy,
                 const Filter2D *filter, ImageBuf::WrapMode wrap,
                 float *result)
{
    DASSERT (filter);
    // Just use isotropic filtering
    float ds = std::max (1.0f, std::max (fabsf(dsdx), fabsf(dsdy)));
    float dt = std::max (1.0f, std::max (fabsf(dtdx), fabsf(dtdy)));
    float ds_inv = 1.0f / ds;
    float dt_inv = 1.0f / dt;
    float filterrad_s = 0.5f * ds * filter->width();
    float filterrad_t = 0.5f * dt * filter->width();
    ImageBuf::ConstIterator<SRCTYPE> samp (src,
                      (int)floorf(s-filterrad_s), (int)ceilf(s+filterrad_s),
                      (int)floorf(t-filterrad_t), (int)ceilf(t+filterrad_t),
                      0, 1, wrap);
    int nc = src.nchannels();
    float *sum = ALLOCA (float, nc);
    memset (sum, 0, nc*sizeof(float));
    float total_w = 0.0f;
    for ( ; ! samp.done(); ++samp) {
        float w = (*filter) (ds_inv*(samp.x()+0.5f-s), dt_inv*(samp.y()+0.5f-t));
        for (int c = 0; c < nc; ++c)
            sum[c] += w * samp[c];
        total_w += w;
    }
    if (total_w != 0.0f)
        for (int c = 0; c < nc; ++c)
            result[c] = sum[c] / total_w;
    else
        for (int c = 0; c < nc; ++c)
            result[c] = 0.0f;
}

} // end anon namespace

template<typename DSTTYPE, typename SRCTYPE>
static bool
resize_ (ImageBuf &dst, const ImageBuf &src,
         Filter2D *filter, ROI roi, int nthreads)
{
    if (nthreads != 1 && roi.npixels() >= 1000) {
        // Lots of pixels and request for multi threads? Parallelize.
        ImageBufAlgo::parallel_image (
            boost::bind(resize_<DSTTYPE,SRCTYPE>, boost::ref(dst),
                        boost::cref(src), filter,
                        _1 /*roi*/, 1 /*nthreads*/),
            roi, nthreads);
        return true;
    }

    // Serial case

    const ImageSpec &srcspec (src.spec());
    const ImageSpec &dstspec (dst.spec());
    int nchannels = dstspec.nchannels;

    // Local copies of the source image window, converted to float
    float srcfx = srcspec.full_x;
    float srcfy = srcspec.full_y;
    float srcfw = srcspec.full_width;
    float srcfh = srcspec.full_height;

    // Ratios of dst/src size.  Values larger than 1 indicate that we
    // are maximizing (enlarging the image), and thus want to smoothly
    // interpolate.  Values less than 1 indicate that we are minimizing
    // (shrinking the image), and thus want to properly filter out the
    // high frequencies.
    float xratio = float(dstspec.full_width) / srcfw; // 2 upsize, 0.5 downsize
    float yratio = float(dstspec.full_height) / srcfh;

    float dstfx = dstspec.full_x;
    float dstfy = dstspec.full_y;
    float dstfw = dstspec.full_width;
    float dstfh = dstspec.full_height;
    float dstpixelwidth = 1.0f / dstfw;
    float dstpixelheight = 1.0f / dstfh;
    float *pel = ALLOCA (float, nchannels);
    float filterrad = filter->width() / 2.0f;

    // radi,radj is the filter radius, as an integer, in source pixels.  We
    // will filter the source over [x-radi, x+radi] X [y-radj,y+radj].
    int radi = (int) ceilf (filterrad/xratio);
    int radj = (int) ceilf (filterrad/yratio);
    int xtaps = 2*radi + 1;
    int ytaps = 2*radj + 1;
    bool separable = filter->separable();
    float *xfiltval = NULL, *yfiltval = NULL;
    if (separable) {
        // Allocate temp space to cache the filter weights
        xfiltval = ALLOCA (float, xtaps);
        yfiltval = ALLOCA (float, ytaps);
    }
#if 0
    std::cerr << "Resizing " << srcspec.full_width << "x" << srcspec.full_height
              << " to " << dstspec.full_width << "x" << dstspec.full_height << "\n";
    std::cerr << "ratios = " << xratio << ", " << yratio << "\n";
    std::cerr << "examining src filter support radius of " << radi << " x " << radj << " pixels\n";
    std::cerr << "dst range " << roi << "\n";
    std::cerr << "separable filter\n";
#endif

    // We're going to loop over all output pixels we're interested in.
    //
    // (s,t) = NDC space coordinates of the output sample we are computing.
    //     This is the "sample point".
    // (src_xf, src_xf) = source pixel space float coordinates of the
    //     sample we're computing. We want to compute the weighted sum
    //     of all the source image pixels that fall under the filter when
    //     centered at that location.
    // (src_x, src_y) = image space integer coordinates of the floor,
    //     i.e., the closest pixel in the source image.
    // src_xf_frac and src_yf_frac are the position within that pixel
    //     of our sample.
    ImageBuf::Iterator<DSTTYPE> out (dst, roi);
    for (int y = roi.ybegin;  y < roi.yend;  ++y) {
        float t = (y-dstfy+0.5f)*dstpixelheight;
        float src_yf = srcfy + t * srcfh;
        int src_y;
        float src_yf_frac = floorfrac (src_yf, &src_y);

        // If using separable filters, our vertical set of filter tap
        // weights will be the same for the whole scanline we're on.  Just
        // compute and normalize them once.
        float totalweight_y = 0.0f;
        if (separable) {
            for (int j = 0;  j < ytaps;  ++j) {
                float w = filter->yfilt (yratio * (j-radj-(src_yf_frac-0.5f)));
                yfiltval[j] = w;
                totalweight_y += w;
            }
            for (int i = 0;  i < ytaps;  ++i)
                yfiltval[i] /= totalweight_y;
        }

        for (int x = roi.xbegin;  x < roi.xend;  ++x) {
            float s = (x-dstfx+0.5f)*dstpixelwidth;
            float src_xf = srcfx + s * srcfw;
            int src_x;
            float src_xf_frac = floorfrac (src_xf, &src_x);
            for (int c = 0;  c < nchannels;  ++c)
                pel[c] = 0.0f;
            if (separable) {
                // Cache and normalize the horizontal filter tap weights
                // just once for this (x,y) position, reuse for all vertical
                // taps.
                float totalweight_x = 0.0f;
                for (int i = 0;  i < xtaps;  ++i) {
                    float w = filter->xfilt (xratio * (i-radi-(src_xf_frac-0.5f)));
                    xfiltval[i] = w;
                    totalweight_x += w;
                }

                if (totalweight_x != 0.0f) {
                    for (int i = 0;  i < xtaps;  ++i)  // normalize x filter
                        xfiltval[i] /= totalweight_x;  // weights
                    ImageBuf::ConstIterator<SRCTYPE> srcpel (src, src_x-radi, src_x+radi+1,
                                                             src_y-radj, src_y+radj+1,
                                                             0, 1, ImageBuf::WrapClamp);
                    for (int j = -radj;  j <= radj;  ++j) {
                        float wy = yfiltval[j+radj];
                        if (wy == 0.0f) {
                            // 0 weight for this y tap -- move to next line
                            srcpel.pos (srcpel.x(), srcpel.y()+1, srcpel.z());
                            continue;
                        }
                        for (int i = 0;  i < xtaps; ++i, ++srcpel) {
                            float w = wy * xfiltval[i];
                            for (int c = 0;  c < nchannels;  ++c)
                                pel[c] += w * srcpel[c];
                        }
                    }
                }
                // Copy the pixel value (already normalized) to the output.
                DASSERT (out.x() == x && out.y() == y);
                if (totalweight_y == 0.0f) {
                    // zero it out
                    for (int c = 0;  c < nchannels;  ++c)
                        out[c] = 0.0f;
                } else {
                    for (int c = 0;  c < nchannels;  ++c)
                        out[c] = pel[c];
                }
            } else {
                // Non-separable
                float totalweight = 0.0f;
                ImageBuf::ConstIterator<SRCTYPE> srcpel (src, src_x-radi, src_x+radi+1,
                                                       src_y-radi, src_y+radi+1,
                                                       0, 1, ImageBuf::WrapClamp);
                for (int j = -radj;  j <= radj;  ++j) {
                    for (int i = -radi;  i <= radi;  ++i, ++srcpel) {
                        float w = (*filter)(xratio * (i-(src_xf_frac-0.5f)),
                                            yratio * (j-(src_yf_frac-0.5f)));
                        totalweight += w;
                        if (w == 0.0f)
                            continue;
                        DASSERT (! srcpel.done());
                        for (int c = 0;  c < nchannels;  ++c)
                            pel[c] += w * srcpel[c];
                    }
                }
                DASSERT (srcpel.done());
                // Rescale pel to normalize the filter and write it to the
                // output image.
                DASSERT (out.x() == x && out.y() == y);
                if (totalweight == 0.0f) {
                    // zero it out
                    for (int c = 0;  c < nchannels;  ++c)
                        out[c] = 0.0f;
                } else {
                    for (int c = 0;  c < nchannels;  ++c)
                        out[c] = pel[c] / totalweight;
                }
            }

            ++out;
        }
    }

    return true;
}

bool
ImageBufAlgo::resize (ImageBuf &dst, const ImageBuf &src,
                      Filter2D *filter, ROI roi, int nthreads)
{
    if (! IBAprep (roi, &dst, &src,
            IBAprep_REQUIRE_SAME_NCHANNELS | IBAprep_NO_SUPPORT_VOLUME |
            IBAprep_NO_COPY_ROI_FULL))
        return false;

    // Set up a shared pointer with custom deleter to make sure any
    // filter we allocate here is properly destroyed.
    boost::shared_ptr<Filter2D> filterptr ((Filter2D*)NULL, Filter2D::destroy);
    bool allocfilter = (filter == NULL);
    if (allocfilter) {
        // If no filter was provided, punt and just linearly interpolate.
        const ImageSpec &srcspec (src.spec());
        const ImageSpec &dstspec (dst.spec());
        float wratio = float(dstspec.full_width) / float(srcspec.full_width);
        float hratio = float(dstspec.full_height) / float(srcspec.full_height);
        float w = 2.0f * std::max (1.0f, wratio);
        float h = 2.0f * std::max (1.0f, hratio);
        filter = Filter2D::create ("triangle", w, h);
        filterptr.reset (filter);
    }

    bool ok;
    OIIO_DISPATCH_TYPES2 (ok, "resize", resize_,
                          dst.spec().format, src.spec().format,
                          dst, src, filter, roi, nthreads);
    return ok;
}

bool
ImageBufAlgo::resize (ImageBuf &dst, const ImageBuf &src,
                      string_view filtername_, float fwidth,
                      ROI roi, int nthreads)
{
    if (! IBAprep (roi, &dst, &src,
            IBAprep_REQUIRE_SAME_NCHANNELS | IBAprep_NO_SUPPORT_VOLUME |
            IBAprep_NO_COPY_ROI_FULL))
        return false;
    const ImageSpec &srcspec (src.spec());
    const ImageSpec &dstspec (dst.spec());

    // Resize ratios
    float wratio = float(dstspec.full_width) / float(srcspec.full_width);
    float hratio = float(dstspec.full_height) / float(srcspec.full_height);

    // Set up a shared pointer with custom deleter to make sure any
    // filter we allocate here is properly destroyed.
    boost::shared_ptr<Filter2D> filter ((Filter2D*)NULL, Filter2D::destroy);
    std::string filtername = filtername_;
    if (filtername.empty()) {
        // No filter name supplied -- pick a good default
        if (wratio > 1.0f || hratio > 1.0f)
            filtername = "blackman-harris";
        else
            filtername = "lanczos3";
    }
    for (int i = 0, e = Filter2D::num_filters();  i < e;  ++i) {
        FilterDesc fd;
        Filter2D::get_filterdesc (i, &fd);
        if (fd.name == filtername) {
            float w = fwidth > 0.0f ? fwidth : fd.width * std::max (1.0f, wratio);
            float h = fwidth > 0.0f ? fwidth : fd.width * std::max (1.0f, hratio);
            filter.reset (Filter2D::create (filtername, w, h));
            break;
        }
    }
    if (! filter) {
        dst.error ("Filter \"%s\" not recognized", filtername);
        return false;
    }

    bool ok;
    OIIO_DISPATCH_TYPES2 (ok, "resize", resize_,
                          dstspec.format, srcspec.format,
                          dst, src, filter.get(), roi, nthreads);
    return ok;
}

template<typename DSTTYPE, typename SRCTYPE>
static bool
resample_ (ImageBuf &dst, const ImageBuf &src, bool interpolate,
           ROI roi, int nthreads)
{
    if (nthreads != 1 && roi.npixels() >= 1000) {
        // Lots of pixels and request for multi threads? Parallelize.
        ImageBufAlgo::parallel_image (
            boost::bind(resample_<DSTTYPE,SRCTYPE>, boost::ref(dst),
                        boost::cref(src), interpolate,
                        _1 /*roi*/, 1 /*nthreads*/),
            roi, nthreads);
        return true;
    }

    // Serial case

    const ImageSpec &srcspec (src.spec());
    const ImageSpec &dstspec (dst.spec());
    int nchannels = src.nchannels();

    // Local copies of the source image window, converted to float
    float srcfx = srcspec.full_x;
    float srcfy = srcspec.full_y;
    float srcfw = srcspec.full_width;
    float srcfh = srcspec.full_height;

    float dstfx = dstspec.full_x;
    float dstfy = dstspec.full_y;
    float dstfw = dstspec.full_width;
    float dstfh = dstspec.full_height;
    float dstpixelwidth = 1.0f / dstfw;
    float dstpixelheight = 1.0f / dstfh;
    float *pel = ALLOCA (float, nchannels);

    ImageBuf::Iterator<DSTTYPE> out (dst, roi);
    ImageBuf::ConstIterator<SRCTYPE> srcpel (src);
    for (int y = roi.ybegin;  y < roi.yend;  ++y) {
        // s,t are NDC space
        float t = (y-dstfy+0.5f)*dstpixelheight;
        // src_xf, src_xf are image space float coordinates
        float src_yf = srcfy + t * srcfh - 0.5f;
        // src_x, src_y are image space integer coordinates of the floor
        int src_y;
        (void) floorfrac (src_yf, &src_y);
        for (int x = roi.xbegin;  x < roi.xend;  ++x) {
            float s = (x-dstfx+0.5f)*dstpixelwidth;
            float src_xf = srcfx + s * srcfw - 0.5f;
            int src_x;
            (void) floorfrac (src_xf, &src_x);

            if (interpolate) {
                src.interppixel (src_xf, src_yf, pel);
                for (int c = roi.chbegin; c < roi.chend; ++c)
                    out[c] = pel[c];
            } else {
                srcpel.pos (src_x, src_y, 0);
                for (int c = roi.chbegin; c < roi.chend; ++c)
                    out[c] = srcpel[c];
            }
            ++out;
        }
    }

    return true;
}

bool
ImageBufAlgo::resample (ImageBuf &dst, const ImageBuf &src,
                        bool interpolate, ROI roi, int nthreads)
{
    if (! IBAprep (roi, &dst, &src,
            IBAprep_REQUIRE_SAME_NCHANNELS | IBAprep_NO_SUPPORT_VOLUME |
            IBAprep_NO_COPY_ROI_FULL))
        return false;
    bool ok;
    OIIO_DISPATCH_TYPES2 (ok, "resample", resample_,
                          dst.spec().format, src.spec().format,
                          dst, src, interpolate, roi, nthreads);
    return ok;
}

#if 0
template<typename DSTTYPE, typename SRCTYPE>
static bool
affine_resample_ (ImageBuf &dst, const ImageBuf &src, const Imath::M33f &Minv,
                  ROI roi, int nthreads)
{
    if (nthreads != 1 && roi.npixels() >= 1000) {
        // Possible multiple thread case -- recurse via parallel_image
        ImageBufAlgo::parallel_image (
            boost::bind(affine_resample_<DSTTYPE,SRCTYPE>,
                        boost::ref(dst), boost::cref(src), Minv,
                        _1 /*roi*/, 1 /*nthreads*/),
            roi, nthreads);
        return true;
    }

    // Serial case
    ImageBuf::Iterator<DSTTYPE,DSTTYPE> d (dst, roi);
    ImageBuf::ConstIterator<SRCTYPE,DSTTYPE> s (src);
    for (  ;  ! d.done();  ++d) {
        Imath::V2f P (d.x()+0.5f, d.y()+0.5f);
        Minv.multVecMatrix (P, P);
        s.pos (int(floorf(P.x)), int(floorf(P.y)), d.z());
        for (int c = roi.chbegin;  c < roi.chend;  ++c)
            d[c] = s[c];
    }
    return true;
}
#endif

template<typename DSTTYPE, typename SRCTYPE>
static bool
warp_ (ImageBuf &dst, const ImageBuf &src, const Imath::M33f &M,
       const Filter2D *filter, ImageBuf::WrapMode wrap,
       ROI roi, int nthreads)
{
    if (nthreads != 1 && roi.npixels() >= 1000) {
        // Possible multiple thread case -- recurse via parallel_image
        ImageBufAlgo::parallel_image (
            boost::bind(warp_<DSTTYPE,SRCTYPE>,
                        boost::ref(dst), boost::cref(src), M,
                        filter, wrap, _1 /*roi*/, 1 /*nthreads*/),
            roi, nthreads);
        return true;
    }

    // Serial case
    int nc = dst.nchannels();
    float *pel = ALLOCA (float, nc);
    memset (pel, 0, nc*sizeof(float));
    Imath::M33f Minv = M.inverse();
    ImageBuf::Iterator<DSTTYPE> out (dst, roi);
    for (  ;  ! out.done();  ++out) {
        Dual2 x (out.x()+0.5f, 1.0f, 0.0f);
        Dual2 y (out.y()+0.5f, 0.0f, 1.0f);
        robust_multVecMatrix (Minv, x, y, x, y);
        filtered_sample<SRCTYPE> (src, x.val(), y.val(),
                                  x.dx(), y.dx(), x.dy(), y.dy(),
                                  filter, wrap, pel);
        for (int c = roi.chbegin;  c < roi.chend;  ++c)
            out[c] = pel[c];

    }
    return true;
}

bool
ImageBufAlgo::warp (ImageBuf &dst, const ImageBuf &src,
                    const Imath::M33f &M,
                    const Filter2D *filter,
                    bool recompute_roi, ImageBuf::WrapMode wrap,
                    ROI roi, int nthreads)
{
    ROI src_roi_full = src.roi_full();
    ROI dst_roi, dst_roi_full;
    if (dst.initialized()) {
        dst_roi = roi.defined() ? roi : dst.roi();
        dst_roi_full = dst.roi_full();
    } else {
        dst_roi = roi.defined() ? roi : (recompute_roi ? transform (M, src.roi()) : src.roi());
        dst_roi_full = src_roi_full;
    }
    dst_roi.chend = std::min (dst_roi.chend, src.nchannels());
    dst_roi_full.chend = std::min (dst_roi_full.chend, src.nchannels());

    if (! IBAprep (dst_roi, &dst, &src, IBAprep_NO_SUPPORT_VOLUME))
        return false;

    // Set up a shared pointer with custom deleter to make sure any
    // filter we allocate here is properly destroyed.
    boost::shared_ptr<Filter2D> filterptr ((Filter2D*)NULL, Filter2D::destroy);
    if (filter == NULL) {
        // If no filter was provided, punt and just linearly interpolate.
        filterptr.reset (Filter2D::create ("lanczos3", 6.0f, 6.0f));
        filter = filterptr.get();
    }

    bool ok;
    OIIO_DISPATCH_TYPES2 (ok, "warp", warp_,
                          dst.spec().format, src.spec().format,
                          dst, src, M, filter, wrap, dst_roi, nthreads);
    return ok;
}

bool
ImageBufAlgo::warp (ImageBuf &dst, const ImageBuf &src,
                    const Imath::M33f &M,
                    string_view filtername_, float filterwidth,
                    bool recompute_roi, ImageBuf::WrapMode wrap,
                    ROI roi, int nthreads)
{
    // Set up a shared pointer with custom deleter to make sure any
    // filter we allocate here is properly destroyed.
    boost::shared_ptr<Filter2D> filter ((Filter2D*)NULL, Filter2D::destroy);
    std::string filtername = filtername_.size() ? filtername_ : "lanczos3";
    for (int i = 0, e = Filter2D::num_filters();  i < e;  ++i) {
        FilterDesc fd;
        Filter2D::get_filterdesc (i, &fd);
        if (fd.name == filtername) {
            float w = filterwidth > 0.0f ? filterwidth : fd.width;
            float h = filterwidth > 0.0f ? filterwidth : fd.width;
            filter.reset (Filter2D::create (filtername, w, h));
            break;
        }
    }
    if (! filter) {
        dst.error ("Filter \"%s\" not recognized", filtername);
        return false;
    }

    return warp (dst, src, M, filter.get(), recompute_roi, wrap, roi, nthreads);
}

bool
ImageBufAlgo::rotate (ImageBuf &dst, const ImageBuf &src,
                      float angle, float center_x, float center_y,
                      Filter2D *filter, bool recompute_roi,
                      ROI roi, int nthreads)
{
    // Calculate the rotation matrix
    Imath::M33f M;
    M.translate(Imath::V2f(-center_x, -center_y));
    M.rotate(angle);
    M *= Imath::M33f().translate(Imath::V2f(center_x, center_y));
    return ImageBufAlgo::warp (dst, src, M, filter,
                               recompute_roi, ImageBuf::WrapBlack,
                               roi, nthreads);
}

bool
ImageBufAlgo::rotate (ImageBuf &dst, const ImageBuf &src,
                      float angle, float center_x, float center_y,
                      string_view filtername, float filterwidth,
                      bool recompute_roi, ROI roi, int nthreads)
{
    // Calculate the rotation matrix
    Imath::M33f M;
    M.translate(Imath::V2f(-center_x, -center_y));
    M.rotate(angle);
    M *= Imath::M33f().translate(Imath::V2f(center_x, center_y));
    return ImageBufAlgo::warp (dst, src, M, filtername, filterwidth,
                               recompute_roi, ImageBuf::WrapBlack,
                               roi, nthreads);
}

bool
ImageBufAlgo::rotate (ImageBuf &dst, const ImageBuf &src, float angle,
                      Filter2D *filter,
                      bool recompute_roi, ROI roi, int nthreads)
{
    ROI src_roi_full = src.roi_full();
    float center_x = 0.5f * (src_roi_full.xbegin + src_roi_full.xend);
    float center_y = 0.5f * (src_roi_full.ybegin + src_roi_full.yend);
    return ImageBufAlgo::rotate (dst, src, angle, center_x, center_y,
                                 filter, recompute_roi, roi, nthreads);
}

bool
ImageBufAlgo::rotate (ImageBuf &dst, const ImageBuf &src, float angle,
                      string_view filtername, float filterwidth,
                      bool recompute_roi,
                      ROI roi, int nthreads)
{
    ROI src_roi_full = src.roi_full();
    float center_x = 0.5f * (src_roi_full.xbegin + src_roi_full.xend);
    float center_y = 0.5f * (src_roi_full.ybegin + src_roi_full.yend);
    return ImageBufAlgo::rotate (dst, src, angle, center_x, center_y,
                                 filtername, filterwidth, recompute_roi,
                                 roi, nthreads);
}

} OIIO_NAMESPACE_EXIT


// SPDX-License-Identifier: BSD-2-Clause

#ifndef NMEA_SENTENCE_PARSER_HPP
#define NMEA_SENTENCE_PARSER_HPP

#include <cmath>
#include <string>
#include <vector>
#include <numeric>
#include <boost/algorithm/string.hpp>

namespace hdl_graph_slam {

struct GPRMC {
public:
  GPRMC() {
    status = 'V';
  }

  GPRMC(const std::vector<std::string>& tokens) {
    if(tokens[0] != "$GPRMC" || tokens.size() < 12) {
      status = 'V';
      return;
    }

    long time = std::stol(tokens[1]);
    hour = time / 10000;
    minute = (time % 10000) / 100;
    second = time % 100;

    status = tokens[2][0];

    latitude = degmin2deg(std::stod(tokens[3]));
    latitude = tokens[4] == "N" ? latitude : -latitude;

    longitude = degmin2deg(std::stod(tokens[5]));
    longitude = tokens[6] == "E" ? longitude : -longitude;

    speed_knots = std::stod(tokens[7]);
    track_angle_degree = std::stod(tokens[8]);

    long date = std::stol(tokens[9]);
    year = date % 100;
    month = (date / 100) % 100;
    day = (date / 10000) % 100;

    magnetic_variation = std::stod(tokens[10]);
    magnetic_variation = tokens[11][0] == 'E' ? magnetic_variation : -magnetic_variation;
  }

  double degmin2deg(double degmin) {
    double d = std::floor(degmin / 100.0);
    double m = (degmin - d * 100.0) / 60.0;
    return d + m;
  }

public:
  char status;  // Status A=active or V=Void.

  int hour;  // Fix taken at 12:35:19 UTC
  int minute;
  int second;

  double latitude;  //
  double longitude;

  double speed_knots;         // Speed over the ground in knots
  double track_angle_degree;  // Track angle in degrees True

  int year;
  int month;
  int day;

  double magnetic_variation;
};

class NmeaSentenceParser {
public:
  NmeaSentenceParser() {}
  ~NmeaSentenceParser() {}

  GPRMC parse(const std::string& sentence) const {
    int checksum_loc = sentence.find('*');
    if(checksum_loc == std::string::npos) {
      return GPRMC();
    }

    int checksum = std::stoul(sentence.substr(checksum_loc + 1), nullptr, 16);

    std::string substr = sentence.substr(1, checksum_loc - 1);
    int sum = std::accumulate(substr.begin(), substr.end(), static_cast<unsigned char>(0), [=](unsigned char n, unsigned char c) { return n ^ c; });

    if(checksum != (sum & 0xf)) {
      std::cerr << "checksum doesn't match!!" << std::endl;
      std::cerr << sentence << " " << sum << std::endl;
      return GPRMC();
    }

    std::vector<std::string> tokens;
    boost::split(tokens, sentence, boost::is_any_of(","));

    return GPRMC(tokens);
  }
};

}  // namespace hdl_graph_slam

#endif  // NMEA_SENTENCE_PARSER_HPP


#ifndef FAST_GICP_FAST_GICP_ST_HPP
#define FAST_GICP_FAST_GICP_ST_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/registration.h>
#include <fast_gicp/gicp/gicp_settings.hpp>

namespace fast_gicp {

/**
 * @brief Fast GICP algorithm optimized for single threading
 *
 *   作者对GICP的实现
 *
 */
template<typename PointSource, typename PointTarget>
class FastGICPSingleThread : public pcl::Registration<PointSource, PointTarget, float> {
public:
  using Scalar = float;
  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;

  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

  using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;

  using pcl::Registration<PointSource, PointTarget, Scalar>::tree_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::tree_reciprocal_;

  using pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;

  FastGICPSingleThread();
  virtual ~FastGICPSingleThread() override;

  void setRotationEpsilon(double eps);

  void setCorrespondenceRandomness(int k);

  void setRegularizationMethod(RegularizationMethod method);

  void swapSourceAndTarget();

  void clearSource();

  void clearTarget();

  virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;

  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;

protected:
  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;

private:
  bool is_converged(const Eigen::Matrix<float, 6, 1>& delta) const;

  void update_correspondences(const Eigen::Matrix<float, 6, 1>& x);

  Eigen::VectorXf loss_ls(const Eigen::Matrix<float, 6, 1>& x, Eigen::MatrixXf* J) const;

  template<typename PointT>
  bool calculate_covariances(const boost::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);

private:
  double rotation_epsilon_;
  int k_correspondences_;
  RegularizationMethod regularization_method_;

  std::unique_ptr<pcl::search::KdTree<PointSource>> source_kdtree;
  std::unique_ptr<pcl::search::KdTree<PointTarget>> target_kdtree;

  std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> source_covs;
  std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> target_covs;

  std::vector<int> correspondences;
  std::vector<float> sq_distances;

  std::vector<float> second_sq_distances;
  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> anchors;
};
}  // namespace fast_gicp

#endif


/*    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
 *      140106    J. Geul           File creatad, copied from unitTestOrbitalStateDerivativeModel.
 *
 *
 *    References
 *
 *    Notes:
 *      Test tolerance was set at 5.0e-15 instead of epsilon due to rounding errors in Eigen types
 *      with entries over a number of orders of magnitude, presumably causing the observed larger
 *      than epsilon relative differences between expected and computed values.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/assign/list_of.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testAccelerationModels.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/testBody.h"
#include "Tudat/Astrodynamics/StateDerivativeModels/orbitalStateDerivativeModel.h"

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

namespace tudat
{
namespace unit_tests
{

using boost::assign::list_of;
using basic_mathematics::Vector6d;

//! Rotate vector over arbitrary angles.
/*!
 * This function computes a composite rotation of the input vector over arbitrary angles about the
 * unit x-, y-, and z-axes. This is used in conjunction with rotateOverOtherArbitraryAngles() to
 * test the frame transformation features provided by the orbitalStateDerivativeModel.
 * \param inputVector Input vector before rotation.
 * \return Rotated vector.
 * \sa orbitalStateDerivativeModel, rotateOverOtherArbitraryAngles().
 */
Eigen::Vector3d rotateOverArbitraryAngles( const Eigen::Vector3d& inputVector )
{
    // Declare rotation matrix.
    Eigen::Matrix3d rotationMatrix;
    rotationMatrix = Eigen::AngleAxisd( -1.15, Eigen::Vector3d::UnitX( ) )
            * Eigen::AngleAxisd( 0.23, Eigen::Vector3d::UnitY( ) )
            * Eigen::AngleAxisd( 2.56, Eigen::Vector3d::UnitZ( ) );

    // Compute rotated matrix and return.
    return  rotationMatrix * inputVector;
}

//! Rotate vector over other arbitrary angles.
/*!
 * This function computes another composite rotation of the input vector over different arbitrary
 * angles (compared to the rotateOverArbitraryAngles() function) about the unit x-, y-, and z-axes.
 * This is used in conjunction with rotateOverArbitraryAngles() to test the frame transformation
 * features provided by the OrbitalStateDerivativeModel.
 * \param inputVector Input vector before rotation.
 * \return Rotated vector.
 * \sa orbitalStateDerivativeModel, rotateOverArbitraryAngles().
 */
Eigen::Vector3d rotateOverOtherArbitraryAngles( const Eigen::Vector3d& inputVector )
{
    // Declare rotation matrix.
    Eigen::Matrix3d rotationMatrix;
    rotationMatrix = Eigen::AngleAxisd( 0.24, Eigen::Vector3d::UnitX( ) )
            * Eigen::AngleAxisd( -1.55, Eigen::Vector3d::UnitY( ) )
            * Eigen::AngleAxisd( 2.13, Eigen::Vector3d::UnitZ( ) );

    // Compute rotated matrix and return.
    return  rotationMatrix * inputVector;
}

//! Perform Mapping from Acceleration to Cartesian State Derivative
/*!
 * This function is implemented in order to not use any external
 * mapping functions. The function was build completely from pieces of
 * code of the old cartesianStateDerivativeModel in order to use
 * completely different, but also verified methods of constructing a
 * stateDerivative from the total accelerations.
 * \param currentState The current state.
 * \param accelerations Total sum of accelerations.
 * \return stateDerivative
 */
Vector6d mapCartesian( const Vector6d& cartesianState, const Eigen::Vector3d& acceleration )
{

    // {{{ Snippets from cartesianStateDerivativeModel.h
    typedef Vector6d CartesianStateDerivativeType;

    // Declare Cartesian state derivative size.
    unsigned int stateDerivativeSize = cartesianState.rows( );

    // Declare Cartesian state derivative of the same size as Cartesian state.
    CartesianStateDerivativeType cartesianStateDerivative
	= CartesianStateDerivativeType::Zero( stateDerivativeSize );

    // Set derivative of position components to current Cartesian velocity.
    cartesianStateDerivative.segment( 0, stateDerivativeSize / 2 )
	= cartesianState.segment( stateDerivativeSize / 2, stateDerivativeSize / 2 );

    // Add transformed acceleration to state derivative.
    cartesianStateDerivative.segment( stateDerivativeSize / 2, stateDerivativeSize / 2 )
	+= acceleration;

    // Return assembled state derivative.
    return cartesianStateDerivative;

    // }}} End Snippets from cartesianStateDerivativeModel.h
}

BOOST_AUTO_TEST_SUITE( test_orbital_state_derivative_model )

//! Test whether 6D Cartesian state derivative model works correctly without frame transformations.
BOOST_AUTO_TEST_CASE( test_OrbitalStateDerivativeModelWithoutFrameTransformations )
{
    using basic_astrodynamics::AccelerationModel3dPointer;
    using state_derivative_models::OrbitalStateDerivativeModelType;
    using state_derivative_models::OrbitalStateDerivativeModelPointer;

    // Shortcuts.
    typedef TestBody< 3, double > TestBody3d;
    typedef boost::shared_ptr< TestBody3d > TestBody3dPointer;
    typedef DerivedAccelerationModel< > DerivedAccelerationModel3d;
    typedef AnotherDerivedAccelerationModel< > AnotherDerivedAccelerationModel3d;

    // Set current state.
    const Vector6d currentState = ( basic_mathematics::Vector6d( )
                                    << Eigen::Vector3d( -1.1, 2.2, -3.3 ),
                                    Eigen::Vector3d( 0.23, 1.67, -0.11 ) ).finished( );

    // Set current time.
    const double currentTime = 5.6;

    // Set current position.
    const Eigen::Vector3d currentPosition = currentState.segment( 0, 3 );

    // Set current velocity.
    const Eigen::Vector3d currentVelocity = currentState.segment( 3, 3 );

    // Create body with zombie time and state.
    TestBody3dPointer body = boost::make_shared< TestBody3d >( Eigen::VectorXd::Zero( 6 ), 0.0 );

    // Create acceleration models.
    AccelerationModel3dPointer firstAccelerationModel3d
            = boost::make_shared< DerivedAccelerationModel3d >(
                boost::bind( &TestBody3d::getCurrentPosition, body ),
                boost::bind( &TestBody3d::getCurrentTime, body ) );

    AccelerationModel3dPointer secondAccelerationModel3d
            = boost::make_shared< AnotherDerivedAccelerationModel3d >(
                boost::bind( &TestBody3d::getCurrentPosition, body ),
                boost::bind( &TestBody3d::getCurrentVelocity, body ),
                boost::bind( &TestBody3d::getCurrentTime, body ) );

    // Create list of acceleration models to provide to state derivative model.
    OrbitalStateDerivativeModelType::AccelerationModelPointerVector listOfAccelerations
            = list_of( firstAccelerationModel3d )( secondAccelerationModel3d );

    // Declare Cartesian state derivative model.
    OrbitalStateDerivativeModelPointer cartesianStateDerivativeModel
            = boost::make_shared< OrbitalStateDerivativeModelType >(
                listOfAccelerations,
                boost::bind( &TestBody3d::setCurrentTimeAndState, body, _1, _2 ),
	        boost::bind( &mapCartesian, _1, _2 ) );

    // // Set Earth gravitational parameter [m^3 s^-2].
    // const double earthGravitationalParameter = 3.986004415e14;

    // // Declare Keplerian state derivative model.
    // OrbitalStateDerivativeModelPointer keplerianStateDerivativeModel
    //         = boost::make_shared< OrbitalStateDerivativeModelType >(
    //             listOfAccelerations,
    //             boost::bind( &TestBody3d::setCurrentTimeAndState, body, _1, _2 ),
    // 	        boost::bind( &stateDerivativeMapKeplerian, _1, _2,
    // 		    earthGravitationalParameter ) );

    // // Declare Modified Equinoctial state derivative model.
    // OrbitalStateDerivativeModelPointer modifiedEquinoctialStateDerivativeModel
    //         = boost::make_shared< OrbitalStateDerivativeModelType >(
    //             listOfAccelerations,
    //             boost::bind( &TestBody3d::setCurrentTimeAndState, body, _1, _2 ),
    // 	        boost::bind( &stateDerivativeMapModifiedEquinoctial, _1, _2,
    // 		    earthGravitationalParameter ) );

    // Set expected accelerations.
    const Eigen::Vector3d expectedAccelerationFirstModel
            = currentPosition / ( currentTime * currentTime );
    const Eigen::Vector3d expectedAccelerationSecondModel
            = 0.5 * currentPosition / ( 3.2 * ( currentTime + 3.4 ) * currentTime )
            + currentVelocity / currentTime;

    // Set expected (cumulative) Cartesian state derivative.
    const Vector6d expectedCartesianStateDerivative
            = ( basic_mathematics::Vector6d( ) << currentVelocity,
                expectedAccelerationFirstModel + expectedAccelerationSecondModel ).finished( );

    // Compute Cartesian state derivative.
    const Vector6d computedCartesianStateDerivative
            = cartesianStateDerivativeModel->computeStateDerivative( currentTime, currentState );

    // Check that computed Cartesian state derivative matches expected values.
    TUDAT_CHECK_MATRIX_BASE( computedCartesianStateDerivative, expectedCartesianStateDerivative )
            BOOST_CHECK_SMALL( computedCartesianStateDerivative.coeff( row, col ) -
                               expectedCartesianStateDerivative.coeff( row, col ),
                               5.0e-15 );
}

//! Test whether 6D Cartesian state derivative model works correctly with frame transformations.
BOOST_AUTO_TEST_CASE( test_OrbitalStateDerivativeModelWithFrameTransformations )
{
    using basic_astrodynamics::AccelerationModel3dPointer;
    using state_derivative_models::OrbitalStateDerivativeModelType;
    using state_derivative_models::OrbitalStateDerivativeModelPointer;

    // Shortcuts.
    typedef TestBody< 3, double > TestBody3d;
    typedef boost::shared_ptr< TestBody3d > TestBody3dPointer;
    typedef DerivedAccelerationModel< > DerivedAccelerationModel3d;
    typedef AnotherDerivedAccelerationModel< > AnotherDerivedAccelerationModel3d;

    // Set current state.
    const Vector6d currentState = ( basic_mathematics::Vector6d( )
                                    << Eigen::Vector3d( -1.1, 2.2, -3.3 ),
                                    Eigen::Vector3d( 0.23, 1.67, -0.11 ) ).finished( );

    // Set current time.
    const double currentTime = 5.6;

    // Set current position.
    const Eigen::Vector3d currentPosition = currentState.segment( 0, 3 );

    // Set current velocity.
    const Eigen::Vector3d currentVelocity = currentState.segment( 3, 3 );

    // Create body with zombie time and state.
    TestBody3dPointer body = boost::make_shared< TestBody3d >( Eigen::VectorXd::Zero( 6 ), 0.0 );

    // Create acceleration models.
    AccelerationModel3dPointer firstAccelerationModel3d
            = boost::make_shared< DerivedAccelerationModel3d >(
                boost::bind( &TestBody3d::getCurrentPosition, body ),
                boost::bind( &TestBody3d::getCurrentTime, body ) );

    AccelerationModel3dPointer secondAccelerationModel3d
            = boost::make_shared< AnotherDerivedAccelerationModel3d >(
                boost::bind( &TestBody3d::getCurrentPosition, body ),
                boost::bind( &TestBody3d::getCurrentVelocity, body ),
                boost::bind( &TestBody3d::getCurrentTime, body ) );

    // Create list of reference frame transformations for first acceleration model.
    // NB: The order of this list is VERY important! The order of transformations executed is from
    // the beginning of the vector to the end sequentially.
    OrbitalStateDerivativeModelType::ListOfReferenceFrameTransformations listOfFrameTransformations
            = list_of( &rotateOverArbitraryAngles )( &rotateOverOtherArbitraryAngles );

    // Create list to pass to constructor of acceleration model/frame transformation list pairs.
    // In this case, there are two frame transformations executed for the first acceleration model,
    // and none for the second.
    OrbitalStateDerivativeModelType::ListOfAccelerationFrameTransformationPairs
            listOfAccelerationFrameTransformations
            = list_of( std::make_pair( firstAccelerationModel3d, listOfFrameTransformations ) )
            ( std::make_pair( secondAccelerationModel3d,
                              OrbitalStateDerivativeModelType::
                              ListOfReferenceFrameTransformations( ) ) );

    // Declare Cartesian state derivative model.
    OrbitalStateDerivativeModelPointer stateDerivativeModel
            = boost::make_shared< OrbitalStateDerivativeModelType >(
                listOfAccelerationFrameTransformations,
                boost::bind( &TestBody3d::setCurrentTimeAndState, body, _1, _2 ),
		boost::bind( &mapCartesian, _1, _2 ) );

    // Set expected accelerations.
    const Eigen::Vector3d expectedAccelerationFirstModel
            = rotateOverOtherArbitraryAngles(
                rotateOverArbitraryAngles( currentPosition / ( currentTime * currentTime ) ) );
    const Eigen::Vector3d expectedAccelerationSecondModel
            = 0.5 * currentPosition / ( 3.2 * ( currentTime + 3.4 ) * currentTime )
            + currentVelocity / currentTime;

    // Set expected (cumulative) Cartesian state derivative.
    const Vector6d expectedCartesianStateDerivative
            = ( basic_mathematics::Vector6d( ) << currentVelocity,
                expectedAccelerationFirstModel + expectedAccelerationSecondModel ).finished( );

    // Compute Cartesian state derivative.
    const Vector6d computedCartesianStateDerivative
            = stateDerivativeModel->computeStateDerivative( currentTime, currentState );

    // Check that computed Cartesian state derivative matches expected values.
    TUDAT_CHECK_MATRIX_BASE( computedCartesianStateDerivative, expectedCartesianStateDerivative )
            BOOST_CHECK_CLOSE_FRACTION( computedCartesianStateDerivative.coeff( row, col ),
                                        expectedCartesianStateDerivative.coeff( row, col ),
                                        5.0e-15 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#include <poplibs_support/TestDevice.hpp>

#include <iostream>

#include <boost/program_options.hpp>

#include <poplibs_test/Util.hpp>

#include <poplar/Engine.hpp>
#include <poplin/MatMul.hpp>
#include <poplin/TriangularSolve.hpp>
#include <poplin/codelets.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/exceptions.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace poplin;
using namespace poplibs_support;

namespace {

void writeTensor(const poplar::Target &target, poplar::Engine &engine,
                 poplar::Type type, StringRef handle,
                 const std::vector<float> &values) {
  if (type == HALF) {
    std::vector<char> buf(values.size() * target.getTypeSize(HALF));
    copyFloatToDeviceHalf(target, values.data(), buf.data(), values.size());
    engine.writeTensor(handle, buf.data(), buf.data() + buf.size());
  } else if (type == FLOAT) {
    engine.writeTensor(handle, values.data(), values.data() + values.size());
  } else {
    throw std::runtime_error("invalid type");
  }
}

void readTensor(const poplar::Target &target, poplar::Engine &engine,
                poplar::Type type, StringRef handle,
                std::vector<float> &values) {
  if (type == HALF) {
    std::vector<char> buf(values.size() * target.getTypeSize(HALF));
    engine.readTensor(handle, buf.data(), buf.data() + buf.size());
    copyDeviceHalfToFloat(target, buf.data(), values.data(), values.size());
  } else if (type == FLOAT) {
    engine.readTensor(handle, values.data(), values.data() + values.size());
  } else {
    throw std::runtime_error("invalid type");
  }
}

template <typename T, std::size_t Tiles = 4>
bool deviceTriangularSolve(poplar::Type type, const std::vector<T> &a,
                           std::vector<std::size_t> a_shape,
                           const std::vector<T> &b,
                           std::vector<std::size_t> b_shape, bool left_side,
                           bool lower, bool unit_diagonal,
                           std::size_t block_size) {
  auto device = createTestDevice(TEST_TARGET, 1, Tiles);
  auto &target = device.getTarget();

  Graph graph(target);
  popops::addCodelets(graph);
  poplin::addCodelets(graph);

  poplar::OptionFlags options;
  options.set("blockSize", std::to_string(block_size));

  auto matmuls = getTriangularSolveMatMulPrePlanParameters(
      type, type, a_shape, b_shape, left_side, lower, options);

  matmul::PlanningCache cache;
  std::set<MatMulPlanParams> params;
  assert(cache.size() == 0);
  for (auto &param : matmuls) {
    params.emplace(&target, param.first, &param.second);
  }
  preplanMatMuls(params, cache);
  assert(cache.size() == matmuls.size());

  Tensor tA = graph.addVariable(type, a_shape, "A");
  Tensor tB = graph.addVariable(type, b_shape, "B");

  assert(tA.numElements() == a.size());
  assert(tB.numElements() == b.size());

  assert(tA.rank() == tB.rank());
  assert(tA.rank() >= 2);

  assert(tA.dim(tA.rank() - 1) == tA.dim(tA.rank() - 2));
  assert(left_side ? tB.dim(tB.rank() - 2)
                   : tB.dim(tB.rank() - 1) == tA.dim(tA.rank() - 1));

  poputil::mapTensorLinearly(graph, tA);
  poputil::mapTensorLinearly(graph, tB);

  graph.createHostWrite("a", tA);
  graph.createHostWrite("b", tB);

  Sequence seq;
  Tensor tX = triangularSolve(graph, tA, tB, left_side, lower, unit_diagonal,
                              seq, "triangular-solve", options, &cache);

  assert(cache.size() == matmuls.size()); // All matmuls were preplanned.
  assert(tX.shape() == tB.shape());
  tA = poplin::triangularMask(graph, tA, lower, unit_diagonal, seq,
                              "triangular-mask-verify");

  tA = tA.rank() >= 3 ? tA.flatten(0, tA.rank() - 2) : tA.expand({0});
  tX = tX.rank() >= 3 ? tX.flatten(0, tX.rank() - 2) : tX.expand({0});

  Tensor tR = left_side
                  ? poplin::matMulGrouped(graph, tA, tX, seq, tA.elementType(),
                                          "triangular-solve-verify")
                  : poplin::matMulGrouped(graph, tX, tA, seq, tA.elementType(),
                                          "triangular-solve-verify");

  tR = tR.reshape(b_shape);
  graph.createHostRead("r", tR);

  std::vector<T> result(b.size());
  Engine eng(graph, seq);
  device.bind([&](const Device &d) {
    eng.load(d);
    writeTensor(target, eng, type, "a", a);
    writeTensor(target, eng, type, "b", b);
    eng.run();

    readTensor(target, eng, type, "r", result);
  });

  const double absTolerance = type == HALF ? 0.3 : 0.0001;
  const double relTolerance = type == HALF ? 0.01 : 0.001;
  bool matchesModel = poplibs_test::util::checkIsClose(
      "result", b.data(), {b.size()}, result.data(), b.size(), relTolerance,
      absTolerance);
  return matchesModel;
}

template <typename T, typename E>
std::vector<T> generateIota(std::vector<E> shape) {
  auto n =
      std::accumulate(shape.begin(), shape.end(), E(1), std::multiplies<E>());
  std::vector<T> data(n);
  int x = 0;
  for (auto &v : data)
    v = 1 + (x++ % 8) / 8.0f; // use small numbers for the test
  return data;
}

template <typename T, std::size_t Tiles = 4>
bool deviceTriangularSolveIota(poplar::Type type,
                               std::vector<std::size_t> a_shape,
                               std::vector<std::size_t> b_shape, bool left_side,
                               bool lower, bool unit_diagonal,
                               std::size_t block_size) {
  auto a = generateIota<T>(a_shape);
  auto b = generateIota<T>(b_shape);
  return deviceTriangularSolve<T, Tiles>(type, a, a_shape, b, b_shape,
                                         left_side, lower, unit_diagonal,
                                         block_size);
}

} // namespace

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType = DeviceType::IpuModel2;
  poplar::Type type = poplar::HALF;
  bool left_side = false;
  bool lower = false;
  bool unit_diagonal = false;
  bool block_solver = false;
  unsigned k = 1;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Produce help message")
    ("device-type", po::value<DeviceType>(&deviceType)->required(), "Device type")
    ("type", po::value<poplar::Type>(&type)->required(), "Data type")
    ("left-side", po::value<bool>(&left_side)->required())
    ("lower", po::value<bool>(&lower)->required())
    ("unit-diagonal", po::value<bool>(&unit_diagonal)->required())
    ("block-solver", po::value<bool>(&block_solver)->required())
    ("k", po::value<unsigned>(&k)->required())
    ;
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  static constexpr std::size_t n = 5;
  std::size_t block_size = block_solver ? 3 : n;
  using Shape = std::vector<std::size_t>;
  auto bShape = left_side ? Shape{1, 2, n, std::size_t(k)}
                          : Shape{1, 2, std::size_t(k), n};
  bool success = deviceTriangularSolveIota<float>(
      type, {1, 2, n, n}, bShape, left_side, lower, unit_diagonal, block_size);
  if (!success) {
    std::cerr << "Test failed\n";
    return 1;
  }

  return 0;
}


/*
 * Copyright 2014-2018 Jouni Roivas
 */

#include "blieng/bliobject.h"

#include <boost/format.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/lexical_cast.hpp>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/uuid/uuid_generators.hpp>

#ifdef DATA_MUTEX_LOCK
#include <boost/thread/locks.hpp>
#include <boost/thread/lock_guard.hpp>
#endif

#ifdef PSEUDO_RANDOM
#include <boost/random/mersenne_twister.hpp>
#else
#include <boost/random/random_device.hpp>
#endif

#include <iostream>
#include <map>
#include <sstream>
#include <fstream>
#include <string>
#include <utility>
#include <vector>
#include <limits>
#include <typeinfo>

#include "blieng/data.h"
#include "blieng/logging.h"

using blieng::BliObject;
using blieng::BliAny;

#ifdef PSEUDO_RANDOM
static std::time_t now = std::time(0);
boost::random::mt19937 BliObject::m_rand_gen{static_cast<std::uint32_t>(now)};
#else
boost::random::random_device BliObject::m_rand_gen;
#endif

BliObject::BliObject()
{
    m_uuid = genUUID();
}

BliObject::~BliObject()
{
    // Enforce deletion
    while (!m_values.empty()) {
        auto data = m_values.begin();
        m_values.erase(data);
    }
    m_uuid = "";
}

void BliObject::assignObject(const BliObject *another)
{
    if (another == nullptr) return;

#ifdef DATA_MUTEX_LOCK
    boost::lock_guard<boost::mutex> keylock(m_value_mutex);
#endif
    for (auto val : another->m_values) {
        m_values[val.first] = val.second;
    }
}

bool BliObject::isValue(const std::string &key) const
{
    auto value_iter = m_values.find(key);
    return (value_iter != m_values.end());
}

int BliObject::getRandomInt(int limit_low, int limit_max)
{
    boost::random::uniform_int_distribution<> dist(limit_low, limit_max);
    return dist(m_rand_gen);
}

double BliObject::getRandomDouble(double limit_low, double limit_max)
{
    boost::random::uniform_real_distribution<> dist(limit_low, limit_max);
    return dist(m_rand_gen);
}

bool BliObject::getRandomBoolean()
{
    return getRandomInt(0, 420000) % 2;
}

BliAny BliObject::getValue(const std::string &key) const
{
    auto value_iter = m_values.find(key);

    if (value_iter == m_values.end()) {
        std::string err = "Error, key not found: " + key;
        LOG_ERROR(err);
        throw std::string(err);
    }

    return value_iter->second;
}

void BliObject::setValue(const std::string &key, BliAny value)
{
    {
#ifdef DATA_MUTEX_LOCK
        boost::lock_guard<boost::mutex> keylock(m_value_mutex);
#endif

        m_values[key] = value;
    }
    valuesUpdated(key);
}

void BliObject::valuesUpdated(const std::string &)
{
}

template<typename A, typename B>
bool BliObject::fitsLimits(BliAny val, A &res) const
{
    if (val.type() != typeid(B)) {
        return false;
    }

    int a_digits = std::numeric_limits<A>::digits;
    int b_digits = std::numeric_limits<B>::digits;

    int a_is_signed = std::numeric_limits<A>::is_signed;
    int b_is_signed = std::numeric_limits<B>::is_signed;

    // These should always fit...
    uint64_t _max = static_cast<uint64_t>(std::numeric_limits<A>::max());
    int64_t _min = std::numeric_limits<A>::min();

    B _val = val.cast<B>();
    bool conv = false;
    if (a_digits >= b_digits
        && a_is_signed == b_is_signed) {
        conv = true;
    }
    else if (a_is_signed != b_is_signed) {
        if (b_is_signed && _val >= 0 && a_digits >= b_digits) {
            conv = true;
        }
        else if (a_is_signed && a_digits >= b_digits) {
            conv = true;
        }
        else if (_val >= 0 && static_cast<uint64_t>(_val) < _max) {
            conv = true;
        }
    }
    else {
        if (_min >=0
            && static_cast<uint64_t>(_val) >= static_cast<uint64_t>(_min)
            && static_cast<uint64_t>(_val) < _max) {
            conv = true;
        }
        if (_min <0
            && static_cast<int64_t>(_val) >= static_cast<int64_t>(_min)
            && static_cast<int64_t>(_val) < static_cast<int64_t>(_max)) {
            conv = true;
        }
    }

    if (conv) {
        res = static_cast<A>(val.cast<B>());
        return true;
    }
    return false;
}

template<typename T>
T BliObject::getValue(const std::string &key) const
{
    BliAny val = getValue(key);
    if (val.empty()) {
        std::string err = "Error, key not found: " + key;
        LOG_ERROR(err);
        throw err;
    }

    if (val.type() == typeid(T)) return val.cast<T>();

    T res = 0;
    if (fitsLimits<T, int>(val, res))
        return res;
    if (fitsLimits<T, unsigned int>(val, res))
        return res;
    if (fitsLimits<T, long>(val, res))
        return res;
    if (fitsLimits<T, unsigned long>(val, res))
        return res;
    if (fitsLimits<T, long long>(val, res))
        return res;
    if (fitsLimits<T, unsigned long long>(val, res))
        return res;
    if (fitsLimits<T, char>(val, res))
        return res;
    if (fitsLimits<T, unsigned char>(val, res))
        return res;
    if (fitsLimits<T, short>(val, res))
        return res;
    if (fitsLimits<T, unsigned short>(val, res))
        return res;
    if (fitsLimits<T, double>(val, res))
        return res;
    if (fitsLimits<T, float>(val, res))
        return res;

    std::ostringstream msg;
    msg << "Unsafe conversion of "
        << key
        << " from typeid("
        << val.type().name()
        << ")"
        << " to typeid("
        << typeid(T).name()
        << ")";
    LOG_ERROR(msg.str());
    throw msg.str();
}

int BliObject::getIntValue(const std::string &key) const
{
    return getValue<int>(key);
}

long long BliObject::getLongLongValue(const std::string &key) const
{
    return getValue<long long>(key);
}

char BliObject::getCharValue(const std::string &key) const
{
    return getValue<char>(key);
}

unsigned int BliObject::getUIntValue(const std::string &key) const
{
    return getValue<unsigned int>(key);
}

double BliObject::getDoubleValue(const std::string &key) const
{
    return getValue<double>(key);
}

std::string BliObject::getStringValue(const std::string &key) const
{
    BliAny val = getValue(key);
    try {
        return val.cast<std::string>();
    }
    catch (cdiggins::anyimpl::bad_any_cast &c) {}
    try {
        return val.cast<char*>();
    }
    catch (cdiggins::anyimpl::bad_any_cast &c) {}

    std::ostringstream msg;
    msg << "Unsafe conversion of "
        << key
        << " from typeid("
        << val.type().name()
        << ")"
        << " to string";
    LOG_ERROR(msg.str());
    throw msg.str();
}

bool BliObject::getBoolValue(const std::string &key) const
{
    BliAny val = getValue(key);
    try {
        return val.cast<bool>();
    }
    catch (cdiggins::anyimpl::bad_any_cast &c) {}

    std::ostringstream msg;
    msg << "Unsafe conversion of "
        << key
        << " from typeid("
        << val.type().name()
        << ")"
        << " to bool";
    LOG_ERROR(msg.str());
    throw msg.str();
}

std::vector<std::string> BliObject::getListValue(const std::string &key) const
{
    BliAny val = getValue(key);
    try {
        return val.cast<std::vector<std::string>>();
    }
    catch (cdiggins::anyimpl::bad_any_cast &c) {
        throw std::string("Not string list at " + key);
    }
}

std::vector<int> BliObject::getIntValues(const std::string &key) const
{
    BliAny val = getValue(key);
    try {
        return val.cast<std::vector<int>>();
    }
    catch (cdiggins::anyimpl::bad_any_cast &c) {
        throw std::string("Not int list at " + key);
    }
}

const std::type_info *BliObject::getValueType(const std::string &key) const
{
    BliAny val = getValue(key);
    if (val.empty()) {
        std::string err = "Error, key not found: " + key;
        LOG_ERROR(err);
        throw err;
    }
    return &val.type();
}

std::string BliObject::toString() const
{
    std::ostringstream res;

    for (auto item : m_values) {
        std::string key = item.first;
        BliAny val = item.second;

        res << key + ": ";
        res << val;
        res << "\n";
    }

    return res.str();
}

std::vector<std::string> BliObject::getKeys()
{
    std::vector<std::string> res;
#ifdef DATA_MUTEX_LOCK
    boost::lock_guard<boost::mutex> keylock(m_value_mutex);
#endif

    for (auto val : m_values) {
        res.push_back(val.first);
    }
    return res;
}

template<typename T>
bool BliObject::changeNumValue(std::string key, BliAny val, int diff)
{
    T __num = val.cast<T>();
    __num += diff;
    if (isValue(key + "-max")) {
        T __max = getIntValue(key + "-max");
        if (__num > __max)
            return false;
    }
    setValue(key, __num);
    return true;
}

bool BliObject::changeNumberValue(const std::string &key, int diff)
{
    if (!isValue(key))
        return false;

    BliAny val = getValue(key);

    // FIXME possible int overflows on multiple cases!
    if (val.type() == typeid(int)) {
        return changeNumValue<int>(key, val, diff);
    }
    else if (val.type() == typeid(unsigned int)) {
        return changeNumValue<unsigned int>(key, val, diff);
    }
    else if (val.type() == typeid(long)) {
        return changeNumValue<long>(key, val, diff);
    }
    else if (val.type() == typeid(unsigned long)) {
        return changeNumValue<unsigned long>(key, val, diff);
    }
    else if (val.type() == typeid(long long)) {
        return changeNumValue<long long>(key, val, diff);
    }
    else if (val.type() == typeid(unsigned long long)) {
        return changeNumValue<unsigned long long>(key, val, diff);
    }
    else if (val.type() == typeid(short)) {
        return changeNumValue<short>(key, val, diff);
    }
    else if (val.type() == typeid(unsigned short)) {
        return changeNumValue<unsigned short>(key, val, diff);
    }
    else if (val.type() == typeid(char)) {
        return changeNumValue<char>(key, val, diff);
    }
    else if (val.type() == typeid(unsigned char)) {
        return changeNumValue<unsigned char>(key, val, diff);
    }
    else if (val.type() == typeid(double)) {
        return changeNumValue<double>(key, val, diff);
    }
    else if (val.type() == typeid(float)) {
        return changeNumValue<float>(key, val, diff);
    }

    throw std::string("Not any known numerical value at: " + key);
}

bool BliObject::increase(const std::string &key)
{
    return changeNumberValue(key, 1);
}

bool BliObject::decrease(const std::string &key)
{
    return changeNumberValue(key, -1);
}

std::string blieng::toString(int value)
{
    return boost::lexical_cast<std::string>(value);
}

std::string blieng::toString(unsigned int value)
{
    return boost::lexical_cast<std::string>(value);
}

std::string blieng::toString(double value)
{
    return boost::lexical_cast<std::string>(value);
}

std::string blieng::percentageString(double value, unsigned int digits)
{
    int percentage = static_cast<int>(value * 100);
    std::string val = blieng::toString(percentage);
    if (digits > 0) {
        double norm = std::fabs(value * 100) - std::fabs(percentage);
        std::string rest = blieng::toString(norm);
        val += rest.substr(1, digits + 1);
    }
    val += "%";
    return val;
}

template<typename T>
void serializeObject(boost::archive::text_oarchive &arch, T obj)
{
    arch << obj;
}

template<typename T>
T deserializeObject(boost::archive::text_iarchive &arch)
{
    T obj;
    arch >> obj;
    return obj;
}

std::string BliObject::serialize(std::string type) const
{
    std::ostringstream ss;

    boost::archive::text_oarchive arch(ss);
    if (type == "") type = "BliObject";
    arch << type;

    serializeObject<unsigned int>(arch, m_values.size());

    for (auto val : m_values) {
        arch << val.first;

        BliAny any = val.second;
        std::string types = any.typeString();
        arch << types;

        if (types == "int")
            serializeObject<int>(arch, any.asInt());
        else if (types == "unsigned int")
            serializeObject<unsigned int>(arch, any.asUInt());
        else if (types == "long")
            serializeObject<long>(arch, any.asLong());
        else if (types == "unsigned long")
            serializeObject<unsigned long>(arch, any.asULong());
        else if (types == "char")
            serializeObject<char>(arch, any.asChar());
        else if (types == "unsigned char")
            serializeObject<unsigned char>(arch, any.asUChar());
        else if (types == "short")
            serializeObject<short>(arch, any.asShort());
        else if (types == "unsigned short")
            serializeObject<unsigned short>(arch, any.asUShort());
        else if (types == "long long")
            serializeObject<long long>(arch, any.asLongLong());
        else if (types == "unsigned long long")
            serializeObject<unsigned long long>(arch, any.asULongLong());
        else if (types == "string")
            serializeObject<std::string>(arch, any.asString());
        else if (types == "float")
            serializeObject<float>(arch, any.asFloat());
        else if (types == "double")
            serializeObject<double>(arch, any.asDouble());
        else if (types == "bool")
            serializeObject<bool>(arch, any.asBool());
        else
            throw std::string("Invalid value for key '" + val.first + "': " + types);
    }

    return ss.str();
}

std::string BliObject::serializedType(const std::string &data)
{
    std::istringstream ss(data);
    boost::archive::text_iarchive arch(ss);

    return deserializeObject<std::string>(arch);
}

bool BliObject::deserialize(const std::string &data, std::string type)
{
    std::istringstream ss(data);
    LOG_DEBUG("Deserializing data");

    boost::archive::text_iarchive arch(ss);
    LOG_DEBUG("Deserializing archive");
    if (type == "") {
        type = "BliObject";
    }
    LOG_DEBUG("Deserializing type");
    std::string data_type = deserializeObject<std::string>(arch);
    if (data_type != type) {
        return false;
    }

    LOG_DEBUG("Deserializing item count");
    unsigned int num_values = deserializeObject<unsigned int>(arch);
    unsigned int index = 0;
    while (index < num_values) {
        LOG_DEBUG("Deserializing name");
        std::string key_name = deserializeObject<std::string>(arch);
        LOG_DEBUG("Deserializing " + key_name);
        std::string types = deserializeObject<std::string>(arch);
        LOG_DEBUG("Type of " + key_name + ": " + types);
        if (types == "int")
            setValue(key_name, deserializeObject<int>(arch));
        else if (types == "unsigned int")
            setValue(key_name, deserializeObject<unsigned int>(arch));
        else if (types == "long")
            setValue(key_name, deserializeObject<long>(arch));
        else if (types == "unsigned long")
            setValue(key_name, deserializeObject<unsigned long>(arch));
        else if (types == "char")
            setValue(key_name, deserializeObject<char>(arch));
        else if (types == "unsigned char")
            setValue(key_name, deserializeObject<unsigned char>(arch));
        else if (types == "short")
            setValue(key_name, deserializeObject<short>(arch));
        else if (types == "unsigned short")
            setValue(key_name, deserializeObject<unsigned short>(arch));
        else if (types == "long long")
            setValue(key_name, deserializeObject<long long>(arch));
        else if (types == "unsigned long long")
            setValue(key_name, deserializeObject<unsigned long long>(arch));
        else if (types == "string")
            setValue(key_name, deserializeObject<std::string>(arch));
        else if (types == "float")
            setValue(key_name, deserializeObject<float>(arch));
        else if (types == "double")
            setValue(key_name, deserializeObject<double>(arch));
        else if (types == "bool")
            setValue(key_name, deserializeObject<bool>(arch));
        else
            throw std::string("Invalid value for key '" + key_name + "': " + types);
        ++index;
    }

    return true;
}

std::string BliObject::genUUID() const
{
#ifdef PSEUDO_RANDOM
    boost::uuids::basic_random_generator<boost::mt19937> gen;
#else
    boost::uuids::basic_random_generator<boost::random_device> gen(&m_rand_gen);
#endif
    boost::uuids::uuid gen_uuid = gen();
    std::ostringstream ss;

    ss << gen_uuid;
    return ss.str();
}

BliObject &BliObject::operator=(const BliObject *other)
{
    assignObject(other);
    return *this;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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 CRYPTO3_RIJNDAEL_IMPL_HPP
#define CRYPTO3_RIJNDAEL_IMPL_HPP

#include <boost/crypto3/block/algorithm/move.hpp>

#include <boost/crypto3/detail/stream_endian.hpp>
#include <boost/crypto3/detail/pack.hpp>

namespace boost {
    namespace crypto3 {
        namespace block {
            /*!
             * @cond DETAIL_IMPL
             */
            namespace detail {
                template<std::size_t KeyBitsImpl, std::size_t BlockBitsImpl, typename PolicyType>
                class rijndael_impl {
                    typedef PolicyType policy_type;

                    typedef typename policy_type::key_type key_type;
                    typedef typename policy_type::key_schedule_type key_schedule_type;
                    typedef typename policy_type::key_schedule_word_type key_schedule_word_type;

                    typedef typename policy_type::block_type block_type;

                    typedef typename policy_type::constants_type constants_type;
                    typedef typename policy_type::shift_offsets_type shift_offsets_type;
                    typedef typename policy_type::mm_type mm_type;

                    BOOST_STATIC_ASSERT(KeyBitsImpl == PolicyType::key_bits);
                    BOOST_STATIC_ASSERT(BlockBitsImpl == PolicyType::block_bits);

                    static inline key_schedule_word_type sub_word(const key_schedule_word_type &x,
                                                                  const constants_type &constants) {
                        key_schedule_word_type result = {0};
#pragma clang loop unroll(full)
                        for (std::size_t i = 0; i < policy_type::word_bytes; ++i) {
                            result =
                                result << CHAR_BIT | constants[::boost::crypto3::detail::extract_uint_t<CHAR_BIT>(x, i)];
                        }

                        return result;
                    }

                    static inline void sub_bytes(block_type &state, const constants_type &sbox) {
#pragma clang loop unroll(full)
                        for (std::size_t i = 0; i < policy_type::word_bytes * policy_type::block_words; ++i) {
                            state[i] = sbox[state[i]];
                        }
                    }

                    static inline void shift_rows(block_type &state, const shift_offsets_type &offset) {
                        std::array<typename policy_type::byte_type, policy_type::block_words> tmp = {0};

                        // row 0 never gets shifted
#pragma clang loop unroll(full)
                        for (std::size_t row = 1; row < policy_type::word_bytes; ++row) {
                            const std::size_t off = offset[row - 1];
#pragma clang loop unroll(full)
                            for (std::size_t i = 0; i < off; ++i) {
                                tmp[i] = state[i * policy_type::word_bytes + row];
                            }
#pragma clang loop unroll(full)
                            for (std::size_t i = 0; i < policy_type::block_words - 1; ++i) {
                                state[i * policy_type::word_bytes + row] =
                                    state[(i + off) * policy_type::word_bytes + row];
                            }
#pragma clang loop unroll(full)
                            for (std::size_t i = 0; i < off; ++i) {
                                state[(policy_type::block_words - 1 - i) * policy_type::word_bytes + row] =
                                    tmp[off - 1 - i];
                            }
                        }
                    }

                    template<typename StateType>
                    static inline block_type mix_columns(const StateType &state, const mm_type &mm) {
                        block_type tmp = {0};

#pragma clang loop unroll(full)
                        for (std::size_t col = 0; col < policy_type::block_words; ++col) {
#pragma clang loop unroll(full)
                            for (std::size_t row = 0; row < policy_type::word_bytes; ++row) {
#pragma clang loop unroll(full)
                                for (std::size_t k = 0; k < policy_type::word_bytes; ++k) {
                                    tmp[col * policy_type::word_bytes + row] ^=
                                        policy_type::mul(mm[row * policy_type::word_bytes + k],
                                                         state[col * policy_type::word_bytes + k]);
                                }
                            }
                        }

                        return tmp;
                    }

                    template<typename InputIterator>
                    static inline void add_round_key(block_type &state, InputIterator first, InputIterator last) {
                        BOOST_ASSERT(std::distance(first, last) == policy_type::block_words &&
                                     state.size() == policy_type::block_bytes);

#pragma clang loop unroll(full)
                        for (std::size_t i = 0; i < policy_type::block_words && first != last; ++i && ++first) {
#pragma clang loop unroll(full)
                            for (std::size_t j = 0; j < policy_type::word_bytes; ++j) {
                                state[i * policy_type::word_bytes + j] ^=
                                    ::boost::crypto3::detail::extract_uint_t<CHAR_BIT>(*first,
                                                                                policy_type::word_bytes - (j + 1));
                            }
                        }
                    }

                    static inline void apply_round(std::uint8_t round, block_type &state, const key_schedule_type &w,
                                                   const constants_type &sbox, const shift_offsets_type &offsets,
                                                   const mm_type &mm) {
                        sub_bytes(state, sbox);
                        shift_rows(state, offsets);
                        state = mix_columns(state, mm);
                        add_round_key(state, w.begin() + round * policy_type::block_words,
                                      w.begin() + (round + 1) * policy_type::block_words);
                    }

                public:
                    static typename policy_type::block_type encrypt_block(const block_type &plaintext,
                                                                          const key_schedule_type &encryption_key) {
                        block_type state = plaintext;

                        add_round_key(state, encryption_key.begin(), encryption_key.begin() + policy_type::block_words);

#pragma clang loop unroll(full)
                        for (std::size_t round = 1; round < policy_type::rounds; ++round) {
                            apply_round(round, state, encryption_key, policy_type::constants,
                                        policy_type::shift_offsets, policy_type::mm);
                        }

                        sub_bytes(state, policy_type::constants);
                        shift_rows(state, policy_type::shift_offsets);
                        add_round_key(state, encryption_key.begin() + policy_type::rounds * policy_type::block_words,
                                      encryption_key.begin() + (policy_type::rounds + 1) * policy_type::block_words);

                        return state;
                    }

                    static typename policy_type::block_type decrypt_block(const block_type &plaintext,
                                                                          const key_schedule_type &decryption_key) {
                        block_type state = plaintext;

                        add_round_key(state, decryption_key.begin() + policy_type::rounds * policy_type::block_words,
                                      decryption_key.begin() + (policy_type::rounds + 1) * policy_type::block_words);

#pragma clang loop unroll(full)
                        for (std::size_t round = policy_type::rounds - 1; round > 0; --round) {
                            apply_round(round, state, decryption_key, policy_type::inverted_constants,
                                        policy_type::inverted_shift_offsets, policy_type::inverted_mm);
                        }

                        sub_bytes(state, policy_type::inverted_constants);
                        shift_rows(state, policy_type::inverted_shift_offsets);
                        add_round_key(state, decryption_key.begin(), decryption_key.begin() + policy_type::block_words);

                        return state;
                    }

                    static void schedule_key(const key_type &key, key_schedule_type &encryption_key,
                                             key_schedule_type &decryption_key) {
                        // the first key_words words are the original key
                        ::boost::crypto3::detail::pack<stream_endian::big_octet_big_bit,
                                                     stream_endian::little_octet_big_bit, CHAR_BIT,
                                                     policy_type::word_bits>(
                            key.begin(), key.begin() + policy_type::key_words * policy_type::word_bytes,
                            encryption_key.begin());

#pragma clang loop unroll(full)
                        for (std::size_t i = policy_type::key_words; i < policy_type::key_schedule_words; ++i) {
                            typename policy_type::key_schedule_word_type tmp = encryption_key[i - 1];
                            if (i % policy_type::key_words == 0) {
                                tmp = sub_word(policy_type::rotate_left(tmp), policy_type::constants) ^
                                      policy_type::round_constants[i / policy_type::key_words - 1];
                            } else if (policy_type::key_words > 6 && i % policy_type::key_words == 4) {
                                tmp = sub_word(tmp, policy_type::constants);
                            }
                            encryption_key[i] = encryption_key[i - policy_type::key_words] ^ tmp;
                        }

                        std::array<typename policy_type::byte_type, policy_type::key_schedule_bytes> bekey = {0};
                        ::boost::crypto3::detail::pack<stream_endian::little_octet_big_bit,
                                                     stream_endian::big_octet_big_bit, policy_type::word_bits,
                                                     CHAR_BIT>(encryption_key.begin(), encryption_key.end(),
                                                               bekey.begin());

#pragma clang loop unroll(full)
                        for (std::uint8_t round = 1; round < policy_type::rounds; ++round) {
                            move(mix_columns(boost::adaptors::slice(bekey, round * policy_type::block_bytes,
                                                                    (round + 1) * policy_type::block_bytes),
                                             policy_type::inverted_mm),
                                 bekey.begin() + round * policy_type::block_bytes);
                        }

                        ::boost::crypto3::detail::pack<stream_endian::big_octet_big_bit,
                                                     stream_endian::little_octet_big_bit, CHAR_BIT,
                                                     policy_type::word_bits>(bekey.begin(), bekey.end(),
                                                                             decryption_key.begin());
                    }
                };
            }    // namespace detail
            /*!
             * @endcond
             */
        }    // namespace block
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_RIJNDAEL_IMPL_HPP


/*
    AtomBall.hpp
    ============
        Class AtomBall implementation.
*/

#ifndef __YSASA_ATOM_BALL_HPP
#define __YSASA_ATOM_BALL_HPP

#include <vector>
#include <Eigen/Dense>
#include <PDBToolsCpp/PDBTools>
#include "AtomBall.h"
#include "Constants.hpp"
#include "MathUtil.hpp"

namespace YSASA
{

////////////////////////////////////////////////////////////////////////////////
// Using
////////////////////////////////////////////////////////////////////////////////

using std::vector;
using Eigen::RowVector3d;
using PDBTools::Atom;

////////////////////////////////////////////////////////////////////////////////
// Constructor
////////////////////////////////////////////////////////////////////////////////

AtomBall::AtomBall(const Atom *atomObj, const vector<RowVector3d> &unitBallCoord,
    double probeR):
    __atomObj(atomObj), __VDWR(VDW_RADII_HASH.at(atomObj->name)), __probeR(probeR),
    __realR(__VDWR + __probeR), __fullS(CalcBallS(__realR)),
    __dS(__fullS / unitBallCoord.size()), __ballCoord(unitBallCoord)
{
    for (auto &dotCoord: __ballCoord)
    {
        dotCoord = dotCoord * __realR + __atomObj->coord;
    }
}

////////////////////////////////////////////////////////////////////////////////
// Calc loss S with another ball
////////////////////////////////////////////////////////////////////////////////

double AtomBall::CalcLossS(AtomBall &rhs)
{
    double lossS = 0.;

    for (auto dotCoordIter = __ballCoord.begin(); dotCoordIter != __ballCoord.end();
        dotCoordIter++)
    {
        if ((*dotCoordIter - (rhs.__atomObj)->coord).norm() < rhs.__realR)
        {
            lossS += __dS;
            __ballCoord.erase(dotCoordIter--);
        }
    }

    for (auto dotCoordIter = rhs.__ballCoord.begin(); dotCoordIter != rhs.__ballCoord.end();
        dotCoordIter++)
    {
        if ((*dotCoordIter - __atomObj->coord).norm() < __realR)
        {
            lossS += rhs.__dS;
            rhs.__ballCoord.erase(dotCoordIter--);
        }
    }

    return lossS;
}

}  // End namespace YSASA

#endif  // __YSASA_ATOM_BALL_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2021, 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_ALGORITHMS_DETAIL_CLOSEST_POINTS_LINEAR_OR_AREAL_TO_AREAL_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_LINEAR_OR_AREAL_TO_AREAL_HPP

#include <boost/geometry/algorithms/detail/closest_points/linear_to_linear.hpp>
#include <boost/geometry/algorithms/detail/closest_points/utilities.hpp>

#include <boost/geometry/algorithms/intersection.hpp>

#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/geometries/geometries.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace closest_points
{

struct linear_to_areal
{
    template <typename Linear, typename Areal, typename Segment, typename Strategies>
    static inline void apply(Linear const& linear,
                             Areal const& areal,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        using most_precise_type = typename select_coordinate_type<Linear, Areal>::type;

        using point_type = typename std::conditional
            <
                std::is_same<typename coordinate_type<Linear>::type, most_precise_type>::value,
                typename point_type<Linear>::type,
                typename point_type<Areal>::type
            >::type;

        using linestring_type = geometry::model::linestring<point_type>;

        /* TODO: currently intersection does not support some cases of tupled input
         *       such as linestring - multipolygon
         *       this could be implemented directly with dynamic geometries
        using polygon_type = geometry::model::polygon<point_type>;
        std::tuple
        <
            geometry::model::multi_point<point_type>,
            geometry::model::multi_linestring<linestring_type>,
            geometry::model::multi_polygon<polygon_type>
        > tp;
        bool intersect_tp = geometry::intersection(linear, areal, tp, strategies);
        */

        geometry::model::multi_point<point_type> mp_out;
        geometry::intersection(linear, areal, mp_out, strategies);

        if (! boost::empty(mp_out))
        {
            set_segment_from_points::apply(*boost::begin(mp_out),
                                           *boost::begin(mp_out),
                                           shortest_seg);
            return;
        }

        // if there are no intersection points then check if the linear geometry
        // (or part of it) is inside the areal and return any point of this part
        geometry::model::multi_linestring<linestring_type> ln_out;
        geometry::intersection(linear, areal, ln_out, strategies);

        if (! boost::empty(ln_out))
        {
            set_segment_from_points::apply(*boost::begin(*boost::begin(ln_out)),
                                           *boost::begin(*boost::begin(ln_out)),
                                           shortest_seg);
            return;
        }

        linear_to_linear::apply(linear, areal, shortest_seg, strategies, false);
    }
};

struct areal_to_linear
{
    template <typename Linear, typename Areal, typename Segment, typename Strategies>
    static inline void apply(Areal const& areal,
                             Linear const& linear,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        linear_to_areal::apply(linear, areal, shortest_seg, strategies);
        detail::closest_points::swap_segment_points::apply(shortest_seg);
    }
};

struct segment_to_areal
{
    template <typename Segment, typename Areal, typename OutSegment, typename Strategies>
    static inline void apply(Segment const& segment,
                             Areal const& areal,
                             OutSegment& shortest_seg,
                             Strategies const& strategies,
                             bool = false)
    {
        using linestring_type = geometry::model::linestring<typename point_type<Segment>::type>;
        linestring_type linestring;
        convert(segment, linestring);
        linear_to_areal::apply(linestring, areal, shortest_seg, strategies);
    }
};

struct areal_to_segment
{
    template <typename Areal, typename Segment, typename OutSegment, typename Strategies>
    static inline void apply(Areal const& areal,
                             Segment const& segment,
                             OutSegment& shortest_seg,
                             Strategies const& strategies,
                             bool = false)
    {
        segment_to_areal::apply(segment, areal, shortest_seg, strategies);
        detail::closest_points::swap_segment_points::apply(shortest_seg);
    }
};

struct areal_to_areal
{
    template <typename Areal1, typename Areal2, typename Segment, typename Strategies>
    static inline void apply(Areal1 const& areal1,
                             Areal2 const& areal2,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        using most_precise_type = typename select_coordinate_type<Areal1, Areal2>::type;

        using point_type = typename std::conditional
            <
                std::is_same<typename coordinate_type<Areal1>::type, most_precise_type>::value,
                typename point_type<Areal1>::type,
                typename point_type<Areal2>::type
            >::type;

        using linestring_type = geometry::model::linestring<point_type>;
        using polygon_type = geometry::model::polygon<point_type>;

        /* TODO: currently intersection does not support tupled input
         *       this should be implemented directly with dynamic geometries
        */

        geometry::model::multi_point<point_type> mp_out;
        geometry::intersection(areal1, areal2, mp_out, strategies);

        if (! boost::empty(mp_out))
        {
            set_segment_from_points::apply(*boost::begin(mp_out),
                                           *boost::begin(mp_out),
                                           shortest_seg);
            return;
        }

        // if there are no intersection points then the linear geometry (or part of it)
        // is inside the areal; return any point of this part
        geometry::model::multi_linestring<linestring_type> ln_out;
        geometry::intersection(areal1, areal2, ln_out, strategies);

        if (! boost::empty(ln_out))
        {
            set_segment_from_points::apply(*boost::begin(*boost::begin(ln_out)),
                                           *boost::begin(*boost::begin(ln_out)),
                                           shortest_seg);
            return;
        }

        geometry::model::multi_polygon<polygon_type> pl_out;
        geometry::intersection(areal1, areal2, pl_out, strategies);

        if (! boost::empty(pl_out))
        {
            set_segment_from_points::apply(
                *boost::begin(boost::geometry::exterior_ring(*boost::begin(pl_out))),
                *boost::begin(boost::geometry::exterior_ring(*boost::begin(pl_out))),
                shortest_seg);
            return;
        }

        linear_to_linear::apply(areal1, areal2, shortest_seg, strategies, false);
    }
};

}} // namespace detail::closest_points
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Linear, typename Areal>
struct closest_points
    <
        Linear, Areal,
        linear_tag, areal_tag,
        false
    >
    : detail::closest_points::linear_to_areal
{};

template <typename Areal, typename Linear>
struct closest_points
    <
        Areal, Linear,
        areal_tag, linear_tag,
        false
    >
    : detail::closest_points::areal_to_linear
{};

template <typename Segment, typename Areal>
struct closest_points
    <
        Segment, Areal,
        segment_tag, areal_tag,
        false
    >
    : detail::closest_points::segment_to_areal
{};

template <typename Areal, typename Segment>
struct closest_points
    <
        Areal, Segment,
        areal_tag, segment_tag,
        false
    >
    : detail::closest_points::areal_to_segment
{};

template <typename Areal1, typename Areal2>
struct closest_points
    <
        Areal1, Areal2,
        areal_tag, areal_tag,
        false
    >
    : detail::closest_points::areal_to_areal
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_LINEAR_OR_AREAL_TO_AREAL_HPP


//
// Copyright (c) 2021 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//
// Official repository: https://github.com/CPPAlliance/http_proto
//

#ifndef BOOST_HTTP_PROTO_RULE_NUMBER_HPP
#define BOOST_HTTP_PROTO_RULE_NUMBER_HPP

#include <boost/http_proto/detail/config.hpp>
#include <boost/http_proto/error.hpp>

namespace boost {
namespace http_proto {
namespace bnf {

/** Rule for 1*DIGIT
*/
struct dec_number
{
    using value_type = std::uint64_t;
    using reference = value_type;

    std::uint64_t v;

    reference
    value() const noexcept
    {
        return v;
    }

    BOOST_HTTP_PROTO_DECL
    char const*
    parse(
        char const* const start,
        char const* const end,
        error_code& ec);
};

/** Rule for 1*HEXDIG
*/
class hex_number
{
public:
    using value_type = std::uint64_t;

    std::uint64_t
    value() const noexcept
    {
        return v_;
    }

    BOOST_HTTP_PROTO_DECL
    char const*
    parse(
        char const* const start,
        char const* const end,
        error_code& ec);

private:
    std::uint64_t v_;
};

} // bnf
} // http_proto
} // boost

#endif