//
// Copyright 2020 Debabrata Mandal <mandaldebabrata123@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_ADAPTIVE_HISTOGRAM_EQUALIZATION_HPP
#define BOOST_GIL_IMAGE_PROCESSING_ADAPTIVE_HISTOGRAM_EQUALIZATION_HPP

#include <boost/gil/algorithm.hpp>
#include <boost/gil/histogram.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/image_processing/histogram_equalization.hpp>
#include <boost/gil/image_view_factory.hpp>

#include <cmath>
#include <map>
#include <vector>

namespace boost { namespace gil {

/////////////////////////////////////////
/// Adaptive Histogram Equalization(AHE)
/////////////////////////////////////////
/// \defgroup AHE AHE
/// \brief Contains implementation and description of the algorithm used to compute
///        adaptive histogram equalization of input images. Naming for the AHE functions
///        are done in the following way
///             <feature-1>_<feature-2>_.._<feature-n>ahe
///        For example, for AHE done using local (non-overlapping) tiles/blocks and
///        final output interpolated among tiles , it is called
///             non_overlapping_interpolated_clahe
///

namespace detail {

/// \defgroup AHE-helpers AHE-helpers
/// \brief AHE helper functions

/// \fn double actual_clip_limit
/// \ingroup AHE-helpers
/// \brief Computes the actual clip limit given a clip limit value using binary search.
///        Reference -  Adaptive Histogram Equalization and Its Variations
///                     (http://www.cs.unc.edu/techreports/86-013.pdf, Pg - 15)
///
template <typename SrcHist>
double actual_clip_limit(SrcHist const& src_hist, double cliplimit = 0.03)
{
    double epsilon       = 1.0;
    using value_t        = typename SrcHist::value_type;
    double sum           = src_hist.sum();
    std::size_t num_bins = src_hist.size();

    cliplimit = sum * cliplimit;
    long low = 0, high = cliplimit, middle = low;
    while (high - low >= 1)
    {
        middle      = (low + high + 1) >> 1;
        long excess = 0;
        std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
            if (v.second > middle)
                excess += v.second - middle;
        });
        if (std::abs(excess - (cliplimit - middle) * num_bins) < epsilon)
            break;
        else if (excess > (cliplimit - middle) * num_bins)
            high = middle - 1;
        else
            low = middle + 1;
    }
    return middle / sum;
}

/// \fn void clip_and_redistribute
/// \ingroup AHE-helpers
/// \brief Clips and redistributes excess pixels based on the actual clip limit value
///        obtained from the other helper function actual_clip_limit
///        Reference - Graphic Gems 4, Pg. 474
///        (http://cas.xav.free.fr/Graphics%20Gems%204%20-%20Paul%20S.%20Heckbert.pdf)
///
template <typename SrcHist, typename DstHist>
void clip_and_redistribute(SrcHist const& src_hist, DstHist& dst_hist, double clip_limit = 0.03)
{
    using value_t            = typename SrcHist::value_type;
    double sum               = src_hist.sum();
    double actual_clip_value = detail::actual_clip_limit(src_hist, clip_limit);
    // double actual_clip_value = clip_limit;
    long actual_clip_limit = actual_clip_value * sum;
    double excess          = 0;
    std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
        if (v.second > actual_clip_limit)
            excess += v.second - actual_clip_limit;
    });
    std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
        if (v.second >= actual_clip_limit)
            dst_hist[dst_hist.key_from_tuple(v.first)] = clip_limit * sum;
        else
            dst_hist[dst_hist.key_from_tuple(v.first)] = v.second + excess / src_hist.size();
    });
    long rem = long(excess) % src_hist.size();
    if (rem == 0)
        return;
    long period       = round(src_hist.size() / rem);
    std::size_t index = 0;
    while (rem)
    {
        if (dst_hist(index) >= clip_limit * sum)
        {
            index = (index + 1) % src_hist.size();
        }
        dst_hist(index)++;
        rem--;
        index = (index + period) % src_hist.size();
    }
}

}  // namespace detail

/// \fn void non_overlapping_interpolated_clahe
/// \ingroup AHE
/// @param src_view      Input   Source image view
/// @param dst_view      Output  Output image view
/// @param tile_width_x  Input   Tile width along x-axis to apply HE
/// @param tile_width_y  Input   Tile width along x-axis to apply HE
/// @param clip_limit    Input   Clipping limit to be applied
/// @param bin_width     Input   Bin widths for histogram
/// @param mask          Input   Specify if mask is to be used
/// @param src_mask      Input   Mask on input image to ignore specified pixels
/// \brief Performs local histogram equalization on tiles of size (tile_width_x, tile_width_y)
///        Then uses the clip limit to redistribute excess pixels above the limit uniformly to
///        other bins. The clip limit is specified as a fraction i.e. a bin's value is clipped
///        if bin_value >= clip_limit * (Total number of pixels in the tile)
///
template <typename SrcView, typename DstView>
void non_overlapping_interpolated_clahe(
    SrcView const& src_view,
    DstView const& dst_view,
    std::size_t tile_width_x                = 20,
    std::size_t tile_width_y                = 20,
    double clip_limit                       = 0.03,
    std::size_t bin_width                   = 1.0,
    bool mask                               = false,
    std::vector<std::vector<bool>> src_mask = {})
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();

    static_assert(
        color_spaces_are_compatible<
            typename color_space_type<SrcView>::type,
            typename color_space_type<DstView>::type>::value,
        "Source and destination views must have same color space");

    using source_channel_t = typename channel_type<SrcView>::type;
    using dst_channel_t    = typename channel_type<DstView>::type;
    using coord_t          = typename SrcView::x_coord_t;

    std::size_t const channels = num_channels<SrcView>::value;
    coord_t const width        = src_view.width();
    coord_t const height       = src_view.height();

    // Find control points

    std::vector<coord_t> sample_x;
    coord_t sample_x1 = tile_width_x / 2;
    coord_t sample_x2 = (tile_width_x + 1) / 2;
    coord_t sample_y1 = tile_width_y / 2;
    coord_t sample_y2 = (tile_width_y + 1) / 2;

    auto extend_left   = tile_width_x;
    auto extend_top    = tile_width_y;
    auto extend_right  = (tile_width_x - width % tile_width_x) % tile_width_x + tile_width_x;
    auto extend_bottom = (tile_width_y - height % tile_width_y) % tile_width_y + tile_width_y;

    auto new_width  = width + extend_left + extend_right;
    auto new_height = height + extend_top + extend_bottom;

    image<typename SrcView::value_type> padded_img(new_width, new_height);

    auto top_left_x     = tile_width_x;
    auto top_left_y     = tile_width_y;
    auto bottom_right_x = tile_width_x + width;
    auto bottom_right_y = tile_width_y + height;

    copy_pixels(src_view, subimage_view(view(padded_img), top_left_x, top_left_y, width, height));

    for (std::size_t k = 0; k < channels; k++)
    {
        std::vector<histogram<source_channel_t>> prev_row(new_width / tile_width_x),
            next_row((new_width / tile_width_x));
        std::vector<std::map<source_channel_t, source_channel_t>> prev_map(
            new_width / tile_width_x),
            next_map((new_width / tile_width_x));

        coord_t prev = 0, next = 1;
        auto channel_view = nth_channel_view(view(padded_img), k);

        for (std::ptrdiff_t i = top_left_y; i < bottom_right_y; ++i)
        {
            if ((i - sample_y1) / tile_width_y >= next || i == top_left_y)
            {
                if (i != top_left_y)
                {
                    prev = next;
                    next++;
                }
                prev_row = next_row;
                prev_map = next_map;
                for (std::ptrdiff_t j = sample_x1; j < new_width; j += tile_width_x)
                {
                    auto img_view = subimage_view(
                        channel_view, j - sample_x1, next * tile_width_y,
                        std::max<int>(
                            std::min<int>(tile_width_x + j - sample_x1, bottom_right_x) -
                                (j - sample_x1),
                            0),
                        std::max<int>(
                            std::min<int>((next + 1) * tile_width_y, bottom_right_y) -
                                next * tile_width_y,
                            0));

                    fill_histogram(
                        img_view, next_row[(j - sample_x1) / tile_width_x], bin_width, false,
                        false);

                    detail::clip_and_redistribute(
                        next_row[(j - sample_x1) / tile_width_x],
                        next_row[(j - sample_x1) / tile_width_x], clip_limit);

                    next_map[(j - sample_x1) / tile_width_x] =
                        histogram_equalization(next_row[(j - sample_x1) / tile_width_x]);
                }
            }
            bool prev_row_mask = 1, next_row_mask = 1;
            if (prev == 0)
                prev_row_mask = false;
            else if (next + 1 == new_height / tile_width_y)
                next_row_mask = false;
            for (std::ptrdiff_t j = top_left_x; j < bottom_right_x; ++j)
            {
                bool prev_col_mask = true, next_col_mask = true;
                if ((j - sample_x1) / tile_width_x == 0)
                    prev_col_mask = false;
                else if ((j - sample_x1) / tile_width_x + 1 == new_width / tile_width_x - 1)
                    next_col_mask = false;

                // Bilinear interpolation
                point_t top_left(
                    (j - sample_x1) / tile_width_x * tile_width_x + sample_x1,
                                    prev * tile_width_y + sample_y1);
                point_t top_right(top_left.x + tile_width_x, top_left.y);
                point_t bottom_left(top_left.x, top_left.y + tile_width_y);
                point_t bottom_right(top_left.x + tile_width_x, top_left.y + tile_width_y);

                long double x_diff = top_right.x - top_left.x;
                long double y_diff = bottom_left.y - top_left.y;

                long double x1 = (j - top_left.x) / x_diff;
                long double x2 = (top_right.x - j) / x_diff;
                long double y1 = (i - top_left.y) / y_diff;
                long double y2 = (bottom_left.y - i) / y_diff;

                if (prev_row_mask == 0)
                    y1 = 1;
                else if (next_row_mask == 0)
                    y2 = 1;
                if (prev_col_mask == 0)
                    x1 = 1;
                else if (next_col_mask == 0)
                    x2 = 1;

                long double numerator =
                    ((prev_row_mask & prev_col_mask) * x2 *
                         prev_map[(top_left.x - sample_x1) / tile_width_x][channel_view(j, i)] +
                     (prev_row_mask & next_col_mask) * x1 *
                         prev_map[(top_right.x - sample_x1) / tile_width_x][channel_view(j, i)]) *
                        y2 +
                    ((next_row_mask & prev_col_mask) * x2 *
                         next_map[(bottom_left.x - sample_x1) / tile_width_x][channel_view(j, i)] +
                     (next_row_mask & next_col_mask) * x1 *
                         next_map[(bottom_right.x - sample_x1) / tile_width_x][channel_view(j, i)]) *
                        y1;

                if (mask && !src_mask[i - top_left_y][j - top_left_x])
                {
                    dst_view(j - top_left_x, i - top_left_y) =
                        channel_convert<dst_channel_t>(
                            static_cast<source_channel_t>(channel_view(i, j)));
                }
                else
                {
                    dst_view(j - top_left_x, i - top_left_y) =
                        channel_convert<dst_channel_t>(static_cast<source_channel_t>(numerator));
                }
            }
        }
    }
}

}}  //namespace boost::gil

#endif


/**
 * @file Tools/Math/Rotation.hpp
 * Rotation related functionality
 * @author Alexis Tsogias
 */

#pragma once

#include <Eigen/Dense>
#include "nao_ik/bhuman/Angle.hpp"
#include "nao_ik/bhuman/Approx.hpp"

using namespace Eigen;

using Vector3a = Matrix<Angle, 3, 1>;
typedef Quaternion<float> Quaternionf;

namespace Rotation
{
  Quaternionf aroundX(float angle);
  Quaternionf aroundY(float angle);
  Quaternionf aroundZ(float angle);

  /**
   * The spherical linear interpolation (slerp) between the two rotations.
   * Where interpolate(0.0f, q1, q2) = q1 and interpolate(1.0f, q1, q2) = q2.
   * @param t interpolation factor. Range: [0.0f, 1.0f].
   */
  Quaternionf interpolate(float t, const Quaternionf& q1, const Quaternionf& q2);

  Quaternionf removeZRotation(const Quaternionf& rotation);
  Quaternionf splitOffZRotation(const Quaternionf& rotation, Quaternionf& zRot);

  namespace Euler
  {
    // Euler angles are expressed in z y x manner.

    Quaternionf fromAngles(float x, float y, float z);
    Quaternionf fromAngles(const Vector3a& rotation);
    Quaternionf fromAngles(const Vector3f& rotation);
    Vector3f getAngles(const Quaternionf& rot);
    float getXAngle(const Quaternionf& rot);
    float getYAngle(const Quaternionf& rot);
    float getZAngle(const Quaternionf& rot);
  }

  namespace Aldebaran
  {
    // Aldebaran uses these angles when calculating the orientation of the robot via the imu
    float getXAngle(const Quaternionf& rot);
    float getXAngle(const Matrix3f& rot);
    float getYAngle(const Quaternionf& rot);
    float getYAngle(const Matrix3f& rot);
  }

  namespace AngleAxis
  {
    // Do not missinterpret the vectors as Euler angles!
    // Vector3f pack(const AngleAxisf& angleAxis);
    AngleAxisf unpack(const Vector3f& angleAxisVec);
  }
}

inline Quaternionf Rotation::aroundX(float angle)
{
  return Quaternionf(AngleAxisf(angle, Vector3f::UnitX()));
}

inline Quaternionf Rotation::aroundY(float angle)
{
  return Quaternionf(AngleAxisf(angle, Vector3f::UnitY()));
}

inline Quaternionf Rotation::aroundZ(float angle)
{
  return Quaternionf(AngleAxisf(angle, Vector3f::UnitZ()));
}

inline Quaternionf Rotation::interpolate(float t, const Quaternionf& q1, const Quaternionf& q2)
{
  return q1.slerp(t, q2);
}

inline Quaternionf Rotation::removeZRotation(const Quaternionf& rotation)
{
  const Vector3f& z = Vector3f::UnitZ();
  const Vector3f zR = rotation.inverse() * z;
  const Vector3f c = zR.cross(z);
  const float sin = c.norm();
  const float cos = zR.dot(z);
  if(Approx::isZero(sin))
    if(cos < 0.f) // 180 degree rotation
      return rotation; // There's no unique decomposition.
    else
      return Quaternionf::Identity();
  else
  {
    const float angle = std::atan2(sin, cos);
    return Quaternionf(AngleAxisf(angle, c.normalized()));
  }
}

inline Quaternionf Rotation::splitOffZRotation(const Quaternionf& rotation, Quaternionf& zRot)
{
  const Quaternionf xyRot = removeZRotation(rotation);
  zRot = rotation * xyRot.inverse();
  return xyRot;
}

inline Quaternionf Rotation::Euler::fromAngles(float x, float y, float z)
{
  return AngleAxisf(z, Vector3f::UnitZ()) * AngleAxisf(y, Vector3f::UnitY()) * AngleAxisf(x, Vector3f::UnitX());
}

inline Quaternionf Rotation::Euler::fromAngles(const Vector3a& rotation)
{
  return fromAngles(rotation.x(), rotation.y(), rotation.z());
}

inline Quaternionf Rotation::Euler::fromAngles(const Vector3f& rotation)
{
  return fromAngles(rotation.x(), rotation.y(), rotation.z());
}

inline Vector3f Rotation::Euler::getAngles(const Quaternionf& rot)
{
  const Matrix3f mat = rot.normalized().toRotationMatrix();
  const float m20 = mat(2, 0);

  if(std::abs(m20) < 0.999999f)
  {
    const float m00 = mat(0, 0);
    const float m10 = mat(1, 0);
    const float m21 = mat(2, 1);
    const float m22 = mat(2, 2);

    const float y1 = -std::asin(m20);
    const float y2 = pi - y1;
    const float cy1 = std::cos(y1);
    const float cy2 = std::cos(y2);
    const float x1 = std::atan2(m21 / cy1, m22 / cy1);
    const float x2 = std::atan2(m21 / cy2, m22 / cy2);
    const float z1 = std::atan2(m10 / cy1, m00 / cy1);
    const float z2 = std::atan2(m10 / cy2, m00 / cy2);

    Vector3f v1(x1, y1, z1);
    Vector3f v2(x2, y2, z2);

    return v1.norm() < v2.norm() ? v1 : v2;
  }
  else
  {
    const float x = std::atan2(mat(0, 1), mat(0, 2)); // x = +-z + atan2(...) , but we set z = 0...
    const float y = -std::asin(m20);

    return Vector3f(x, y, 0.f);
  }
}

inline float Rotation::Euler::getXAngle(const Quaternionf& rot)
{
  return getAngles(rot).x();
}

inline float Rotation::Euler::getYAngle(const Quaternionf& rot)
{
  return getAngles(rot).y();
}

inline float Rotation::Euler::getZAngle(const Quaternionf& rot)
{
  return getAngles(rot).z();
}

inline float Rotation::Aldebaran::getXAngle(const Quaternionf& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | 0 |
  // v into  : v -> | h | = v'
  // yz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(h * h + i * i))

  const float tx = 2.f * rot.x();
  const float h = 2.f * rot.z() * rot.y() + tx * rot.w();
  const float i = 1.f - (tx * rot.x() + 2.f * rot.y() * rot.y());

  const float len = std::sqrt(h * h + i * i);
  return len > 1e-5f ? std::acos(i / len) * (h > 0.f ? 1.f : -1.f) : 0.f;
}

inline float Rotation::Aldebaran::getXAngle(const Matrix3f& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | 0 |
  // v into  : v -> | h | = v'
  // yz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(h * h + i * i))

  const float h = rot(2, 1);
  const float i = rot(2, 2);

  const float len = std::sqrt(h * h + i * i);
  return len > 1e-5f ? std::acos(i / len) * (h > 0.f ? 1.f : -1.f) : 0.f;
}

inline float Rotation::Aldebaran::getYAngle(const Quaternionf& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | g |
  // v into  : v -> | 0 | = v'
  // xz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(g * g + i * i))

  // calculate g and i

  const float ty = 2.f * rot.y();
  const float g = 2.f * rot.z() * rot.x() - ty * rot.w();
  const float i = 1.f - (2.f * rot.x() * rot.x() + ty * rot.y());

  const float len = std::sqrt(g * g + i * i);
  return len > 1e-5f ? std::acos(i / len) * (g > 0.f ? -1.f : 1.f) : 0.f;
}

inline float Rotation::Aldebaran::getYAngle(const Matrix3f& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | g |
  // v into  : v -> | 0 | = v'
  // xz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(g * g + i * i))

  const float g = rot(2, 0);
  const float i = rot(2, 2);

  const float len = std::sqrt(g * g + i * i);
  return len > 1e-5f ? std::acos(i / len) * (g > 0.f ? -1.f : 1.f) : 0.f;
}

// inline Vector3f Rotation::AngleAxis::pack(const AngleAxisf& angleAxis)
// {
//   const float angle = angleAxis.angle();
//   if(Approx::isZero(angle))
//     return Vector3f::Zero();
//   else
//     return angleAxis.axis().normalized(angle);
// }

inline AngleAxisf Rotation::AngleAxis::unpack(const Vector3f& angleAxisVec)
{
  const float angle = angleAxisVec.norm();
  if(Approx::isZero(angle))
    return AngleAxisf::Identity();
  else
    return AngleAxisf(angle, angleAxisVec.normalized());
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file sabrvolsurface.hpp
    \brief SABR volatility (smile) surface
*/

#ifndef quantlib_sabr_vol_surface_hpp
#define quantlib_sabr_vol_surface_hpp

#include <ql/experimental/volatility/interestratevolsurface.hpp>
#include <ql/experimental/volatility/blackatmvolcurve.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/volatility/sabrinterpolatedsmilesection.hpp>
#include <boost/array.hpp>

namespace QuantLib {

    //! SABR volatility (smile) surface
    /*! blah blah
    */
    class SabrVolSurface : public InterestRateVolSurface {
      public:
        SabrVolSurface(
                const ext::shared_ptr<InterestRateIndex>&,
                const Handle<BlackAtmVolCurve>&,
                const std::vector<Period>& optionTenors,
                const std::vector<Spread>& atmRateSpreads,
                const std::vector<std::vector<Handle<Quote> > >& volSpreads);
        //@}
        // All virtual methods of base classes must be forwarded
        //! \name TermStructure interface
        //@{
        DayCounter dayCounter() const override;
        Date maxDate() const override;
        Time maxTime() const override;
        const Date& referenceDate() const override;
        Calendar calendar() const override;
        Natural settlementDays() const override;
        //@}
        //! \name VolatilityTermStructure interface
        //@{
        Real minStrike() const override;
        Real maxStrike() const override;
        //@}
        const Handle<BlackAtmVolCurve>& atmCurve() const;
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
        std::vector<Volatility> volatilitySpreads(const Period&) const;
        std::vector<Volatility> volatilitySpreads(const Date&) const;
      protected:
        boost::array<Real, 4> sabrGuesses(const Date&) const;
      public:
        //@}
        //! \name BlackVolSurface interface
        //@{
        ext::shared_ptr<SmileSection> smileSectionImpl(Time) const override;
        //@}
      protected:
        //@}
        //! \name LazyObject interface
        //@{
        void performCalculations () const;
        void update() override;
        //@}
      private:
        void registerWithMarketData();
        void checkInputs() const;
        void updateSabrGuesses(const Date& d, boost::array<Real, 4> newGuesses) const;
        Handle<BlackAtmVolCurve> atmCurve_;
        std::vector<Period> optionTenors_;
        std::vector<Time> optionTimes_;
        std::vector<Date> optionDates_;
        std::vector<Spread> atmRateSpreads_;
        std::vector<std::vector<Handle<Quote> > > volSpreads_;
        //
        bool isAlphaFixed_;
        bool isBetaFixed_;
        bool isNuFixed_;
        bool isRhoFixed_;
        bool vegaWeighted_;
        //
        mutable std::vector<boost::array<Real,4> > sabrGuesses_;
    };

    // inline

    inline DayCounter SabrVolSurface::dayCounter() const {
        return atmCurve_->dayCounter();
    }

    inline Date SabrVolSurface::maxDate() const {
        return atmCurve_->maxDate();
    }

    inline Time SabrVolSurface::maxTime() const {
        return atmCurve_->maxTime();
    }

    inline const Date& SabrVolSurface::referenceDate() const {
        return atmCurve_->referenceDate();
    }

    inline Calendar SabrVolSurface::calendar() const {
        return atmCurve_->calendar();
    }

    inline Natural SabrVolSurface::settlementDays() const {
        return atmCurve_->settlementDays();
    }

    inline Real SabrVolSurface::minStrike() const {
        return QL_MIN_REAL;
    }

    inline Real SabrVolSurface::maxStrike() const {
        return QL_MAX_REAL;
    }

    inline const Handle<BlackAtmVolCurve>& SabrVolSurface::atmCurve() const {
        return atmCurve_;
    }

    inline std::vector<Volatility>
    SabrVolSurface::volatilitySpreads(const Period& p) const {
        return volatilitySpreads(optionDateFromTenor(p));
    }
}

#endif


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GeneralSphericalSpatialCoordinateConversionPolicy.hpp
//! \author Alex Robinson
//! \brief  General spherical spatial coordinate conversion policy declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_GENERAL_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP
#define UTILITY_GENERAL_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP

// Boost Includes
#include <boost/serialization/array_wrapper.hpp>

// FRENSIE Includes
#include "Utility_SphericalSpatialCoordinateConversionPolicy.hpp"

namespace Utility{

//! The general spherical spatial coordinate conversion policy class
class GeneralSphericalSpatialCoordinateConversionPolicy : public SphericalSpatialCoordinateConversionPolicy
{

public:

  //! Constructor
  GeneralSphericalSpatialCoordinateConversionPolicy( const double origin[3],
                                                     const double axis[3] );

  //! Destructor
  ~GeneralSphericalSpatialCoordinateConversionPolicy()
  { /* ... */ }

  //! Convert the spatial coordinates to cartesian coordinates
  void convertToCartesianSpatialCoordinates(
                                      const double primary_spatial_coord,
                                      const double secondary_spatial_coord,
                                      const double tertiary_spatial_coord,
                                      double& x_spatial_coord,
                                      double& y_spatial_coord,
                                      double& z_spatial_coord ) const override;

  //! Convert the cartesian coordinates to the spatial coordinate system
  void convertFromCartesianSpatialCoordinates(
                               const double x_spatial_coord,
                               const double y_spatial_coord,
                               const double z_spatial_coord,
                               double& primary_spatial_coord,
                               double& secondary_spatial_coord,
                               double& tertiary_spatial_coord ) const override;

  //! Convert the spatial coordinates to cartesian coordinates
  using SphericalSpatialCoordinateConversionPolicy::convertToCartesianSpatialCoordinates;

  //! Convert the cartesian coordinates to the spatial coordinate system
  using SphericalSpatialCoordinateConversionPolicy::convertFromCartesianSpatialCoordinates;

private:

  // The default constructor should not be used - if the axis and origin
  // correspond to the global coordinate system use the basic conversion
  // policy
  GeneralSphericalSpatialCoordinateConversionPolicy()
  { /* ... */ }

  // We have C-arrays as members - hide the copy constructor and assignment
  // operator
  GeneralSphericalSpatialCoordinateConversionPolicy( const GeneralSphericalSpatialCoordinateConversionPolicy& that );
  GeneralSphericalSpatialCoordinateConversionPolicy& operator=( const GeneralSphericalSpatialCoordinateConversionPolicy& that );

  // Save the policy to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const;

  // Load the policy from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version );

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

  // The origin of the spherical coordinate system w.r.t. the global Cartesian
  // coordinate system
  double d_origin[3];

  // The z-axis (unit vector) of the spherical coordinate system w.r.t. the
  // global Cartesian coordinate system
  double d_axis[3];
};

// Save the policy to an archive
template<typename Archive>
void GeneralSphericalSpatialCoordinateConversionPolicy::save( Archive& ar, const unsigned version ) const
{
  // Save the base class
  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( SphericalSpatialCoordinateConversionPolicy );

  // Save the local data
  ar & boost::serialization::make_nvp( "d_origin", boost::serialization::make_array( d_origin, 3 ) );
  ar & boost::serialization::make_nvp( "d_axis", boost::serialization::make_array( d_axis, 3 ) );
}

// Load the policy from an archive
template<typename Archive>
void GeneralSphericalSpatialCoordinateConversionPolicy::load( Archive& ar, const unsigned version )
{
  // Load the base class
  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( SphericalSpatialCoordinateConversionPolicy );

  // Load the local data
  ar & boost::serialization::make_nvp( "d_origin", boost::serialization::make_array( d_origin, 3 ) );
  ar & boost::serialization::make_nvp( "d_axis", boost::serialization::make_array( d_axis, 3 ) );
}

} // end Utility namespace

BOOST_SERIALIZATION_CLASS_VERSION( GeneralSphericalSpatialCoordinateConversionPolicy, Utility, 0 );
BOOST_SERIALIZATION_CLASS_EXPORT_STANDARD_KEY( GeneralSphericalSpatialCoordinateConversionPolicy, Utility );
EXTERN_EXPLICIT_CLASS_SAVE_LOAD_INST( Utility, GeneralSphericalSpatialCoordinateConversionPolicy );

#endif // end UTILITY_GENERAL_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP

//---------------------------------------------------------------------------//
// end Utility_GeneralSphericalSpatialCoordinateConversionPolicy.hpp
//---------------------------------------------------------------------------//


/*
 * From:
 * http://stackoverflow.com/questions/6142576/sample-from-multivariate-normal-gaussian-distribution-in-c
 */

#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <omp.h>
#include <chrono>

#include "mvnormal.h"

using namespace std;
using namespace Eigen;
using namespace std::chrono;

std::mt19937 **bmrngs;

/*
  We need a functor that can pretend it's const,
  but to be a good random number generator
  it needs mutable state.
*/

#ifdef __INTEL_COMPILER
std::random_device srd;
#pragma omp threadprivate(srd)
#else
// use thread_local for gcc
thread_local static std::random_device srd;
#endif

#ifndef __clang__
thread_local
#endif
static std::mt19937 rng(srd());

#ifndef __clang__
thread_local
#endif
static std::normal_distribution<> nd;

double randn(double) {
  return nd(rng);
}

double randn0() {
  return nd(rng);
}

auto
nrandn(int n) -> decltype( VectorXd::NullaryExpr(n, std::cref(randn)) )
{
    return VectorXd::NullaryExpr(n, std::cref(randn));
}

void init_bmrng(int seed) {
   int nthreads = -1;
#pragma omp parallel
   {
#pragma omp single
      {
         nthreads = omp_get_num_threads();
      }
   }
   bmrngs = new std::mt19937*[nthreads];
   for (int i = 0; i < nthreads; i++) {
      bmrngs[i] = new std::mt19937(seed + i * 1999);
   }
}

void init_bmrng() {
   auto ms = (duration_cast< milliseconds >(
             system_clock::now().time_since_epoch()
         )).count();
   init_bmrng(ms);
}

void bmrandn(double* x, long n) {
#pragma omp parallel
  {
    std::uniform_real_distribution<double> unif(-1.0, 1.0);
    std::mt19937* bmrng = bmrngs[omp_get_thread_num()];
#pragma omp for schedule(static)
    for (long i = 0; i < n; i += 2) {
      double x1, x2, w;
      do {
        x1 = unif(*bmrng);
        x2 = unif(*bmrng);
        w = x1 * x1 + x2 * x2;
      } while ( w >= 1.0 );

      w = sqrt( (-2.0 * log( w ) ) / w );
      x[i] = x1 * w;
      if (i + 1 < n) {
        x[i+1] = x2 * w;
      }
    }
  }
}

double rand_unif() {
  std::uniform_real_distribution<double> unif(0.0, 1.0);
	std::mt19937* bmrng = bmrngs[omp_get_thread_num()];
	return unif(*bmrng);
}

double rand_unif(double low, double high) {
  std::uniform_real_distribution<double> unif(low, high);
	std::mt19937* bmrng = bmrngs[omp_get_thread_num()];
	return unif(*bmrng);
}

void bmrandn(MatrixXd & X) {
  long n = X.rows() * (long)X.cols();
  bmrandn(X.data(), n);
}

// to be called within OpenMP parallel loop (also from serial code is fine)
void bmrandn_single(double* x, long n) {
  std::uniform_real_distribution<double> unif(-1.0, 1.0);
  std::mt19937* bmrng = bmrngs[omp_get_thread_num()];
  for (long i = 0; i < n; i += 2) {
    double x1, x2, w;
    do {
      x1 = unif(*bmrng);
      x2 = unif(*bmrng);
      w = x1 * x1 + x2 * x2;
    } while ( w >= 1.0 );

    w = sqrt( (-2.0 * log( w ) ) / w );
    x[i] = x1 * w;
    if (i + 1 < n) {
      x[i+1] = x2 * w;
    }
  }
}

double bmrandn_single() {
  //TODO: add bmrng as input
  std::uniform_real_distribution<double> unif(-1.0, 1.0);
  std::mt19937* bmrng = bmrngs[omp_get_thread_num()];

    double x1, x2, w;
    do {
      x1 = unif(*bmrng);
      x2 = unif(*bmrng);
      w = x1 * x1 + x2 * x2;
    } while ( w >= 1.0 );

    w = sqrt( (-2.0 * log( w ) ) / w );
    return x1 * w;
}

void bmrandn_single(Eigen::VectorXd & x) {
  bmrandn_single(x.data(), x.size());
}

/** returns random number according to Gamma distribution
 *  with the given shape (k) and scale (theta). See wiki. */
double rgamma(double shape, double scale) {
  std::gamma_distribution<double> gamma(shape, scale);
  return gamma(*bmrngs[0]);
}

/** Normal(0, Lambda^-1) for nn columns */
MatrixXd MvNormal_prec(const MatrixXd & Lambda, int nn = 1)
{
  int size = Lambda.rows(); // Dimensionality (rows)

  LLT<MatrixXd> chol(Lambda);

  MatrixXd r = MatrixXd::NullaryExpr(size, nn, std::cref(randn));
	chol.matrixU().solveInPlace(r);
  return r;
}

MatrixXd MvNormal_prec(const MatrixXd & Lambda, const VectorXd & mean, int nn = 1)
{
  MatrixXd r = MvNormal_prec(Lambda, nn);
  return r.colwise() + mean;
}

MatrixXd MvNormal_prec_omp(const MatrixXd & Lambda, int nn = 1)
{
  int size = Lambda.rows(); // Dimensionality (rows)

  LLT<MatrixXd> chol(Lambda);

  MatrixXd r(size, nn);
  bmrandn(r);
  // TODO: check if solveInPlace is parallelized:
	chol.matrixU().solveInPlace(r);
  return r;
}

/*
  Draw nn samples from a size-dimensional normal distribution
  with a specified mean and covariance
*/
MatrixXd MvNormal(const MatrixXd covar, const VectorXd mean, int nn = 1)
{
  int size = mean.rows(); // Dimensionality (rows)
  MatrixXd normTransform(size,size);

  LLT<MatrixXd> cholSolver(covar);
  normTransform = cholSolver.matrixL();

  auto normSamples = MatrixXd::NullaryExpr(size, nn, std::cref(randn));
  MatrixXd samples = (normTransform * normSamples).colwise() + mean;

  return samples;
}

MatrixXd WishartUnit(int m, int df)
{
    MatrixXd c(m,m);
    c.setZero();

    for ( int i = 0; i < m; i++ ) {
        std::gamma_distribution<> gam(0.5*(df - i));
        c(i,i) = sqrt(2.0 * gam(rng));
        VectorXd r = nrandn(m-i-1);
        c.block(i,i+1,1,m-i-1) = r.transpose();
    }

    MatrixXd ret = c.transpose() * c;

#ifdef TEST_MVNORMAL
    cout << "WISHART UNIT {\n" << endl;
    cout << "  m:\n" << m << endl;
    cout << "  df:\n" << df << endl;
    cout << "  ret;\n" << ret << endl;
    cout << "  c:\n" << c << endl;
    cout << "}\n" << ret << endl;
#endif

    return ret;
}

MatrixXd Wishart(const MatrixXd &sigma, const int df)
{
//  Get R, the upper triangular Cholesky factor of SIGMA.
  auto chol = sigma.llt();

//  Get AU, a sample from the unit Wishart distribution.
  MatrixXd au = WishartUnit(sigma.cols(), df);

//  Construct the matrix A = R' * AU * R.
  MatrixXd a = chol.matrixL() * au * chol.matrixU();

#ifdef TEST_MVNORMAL
    cout << "WISHART {\n" << endl;
    cout << "  sigma::\n" << sigma << endl;
    cout << "  r:\n" << r << endl;
    cout << "  au:\n" << au << endl;
    cout << "  df:\n" << df << endl;
    cout << "  a:\n" << a << endl;
    cout << "}\n" << endl;
#endif

  return a;
}

// from julia package Distributions: conjugates/normalwishart.jl
std::pair<VectorXd, MatrixXd> NormalWishart(const VectorXd & mu, double kappa, const MatrixXd & T, double nu)
{
  MatrixXd Lam = Wishart(T, nu);
  MatrixXd mu_o = MvNormal_prec(Lam * kappa, mu);

#ifdef TEST_MVNORMAL
    cout << "NORMAL WISHART {\n" << endl;
    cout << "  mu:\n" << mu << endl;
    cout << "  kappa:\n" << kappa << endl;
    cout << "  T:\n" << T << endl;
    cout << "  nu:\n" << nu << endl;
    cout << "  mu_o\n" << mu_o << endl;
    cout << "  Lam\n" << Lam << endl;
    cout << "}\n" << endl;
#endif

  return std::make_pair(mu_o , Lam);
}

std::pair<VectorXd, MatrixXd> OldCondNormalWishart(const MatrixXd &U, const VectorXd &mu, const double kappa, const MatrixXd &T, const int nu)
{
  int N = U.cols();

  auto Um = U.rowwise().mean();

  // http://stackoverflow.com/questions/15138634/eigen-is-there-an-inbuilt-way-to-calculate-sample-covariance
  MatrixXd C = U.colwise() - Um;
  MatrixXd S = (C * C.adjoint()) / double(N - 1);
  VectorXd mu_c = (kappa*mu + N*Um) / (kappa + N);
  double kappa_c = kappa + N;
  MatrixXd T_c = ( T + N * S.transpose() + (kappa * N)/(kappa + N) * (mu - Um) * ((mu - Um).transpose())).inverse();
  int nu_c = nu + N;

#ifdef TEST_MVNORMAL
  cout << "mu_c:\n" << mu_c << endl;
  cout << "kappa_c:\n" << kappa_c << endl;
  cout << "T_c:\n" << T_c << endl;
  cout << "nu_c:\n" << nu_c << endl;
#endif

  return NormalWishart(mu_c, kappa_c, T_c, nu_c);
}

// from bpmf.jl -- verified
std::pair<VectorXd, MatrixXd> CondNormalWishart(const MatrixXd &U, const VectorXd &mu, const double kappa, const MatrixXd &T, const int nu)
{
  /// TODO: parallelize (for computing C and C * C')
  int N = U.cols();

	auto NU = U.rowwise().sum();
	auto NS = U * U.adjoint();

	int nu_c = nu + N;
	double kappa_c = kappa + N;
	auto mu_c = (kappa * mu + NU) / (kappa + N);
	auto X    = (T + NS + kappa * mu * mu.adjoint() - kappa_c * mu_c * mu_c.adjoint());
	Eigen::MatrixXd T_c = X.inverse();

#ifdef TEST_MVNORMAL
  cout << "mu_c:\n" << mu_c << endl;
  cout << "kappa_c:\n" << kappa_c << endl;
  cout << "T_c:\n" << T_c << endl;
  cout << "nu_c:\n" << nu_c << endl;
#endif

  return NormalWishart(mu_c, kappa_c, T_c, nu_c);
}

#if defined(TEST_MVNORMAL) || defined (BENCH_MVNORMAL)

int main()
{

    MatrixXd U(32,32 * 1024);
    U.setOnes();

    VectorXd mu(32);
    mu.setZero();

    double kappa = 2;

    MatrixXd T(32,32);
    T.setIdentity(32,32);
    T.array() /= 4;

    int nu = 3;

    VectorXd mu_out;
    MatrixXd T_out;

#ifdef BENCH_MVNORMAL
    for(int i=0; i<300; ++i) {
        tie(mu_out, T_out) = CondNormalWishart(U, mu, kappa, T, nu);
        cout << i << "\r" << flush;
    }
    cout << endl << flush;

    for(int i=0; i<7; ++i) {
        cout << i << ": " << (int)(100.0 * acc[i] / acc[7])  << endl;
    }
    cout << "total: " << acc[7] << endl;

    for(int i=0; i<300; ++i) {
        tie(mu_out, T_out) = OldCondNormalWishart(U, mu, kappa, T, nu);
        cout << i << "\r" << flush;
    }
    cout << endl << flush;

    cout << "total: " << acc[8] << endl;

#else
#if 1
    cout << "COND NORMAL WISHART\n" << endl;

    tie(mu_out, T_out) = CondNormalWishart(U, mu, kappa, T, nu);

    cout << "mu_out:\n" << mu_out << endl;
    cout << "T_out:\n" << T_out << endl;

    cout << "\n-----\n\n";
#endif

#if 0
    cout << "NORMAL WISHART\n" << endl;

    tie(mu_out, T_out) = NormalWishart(mu, kappa, T, nu);
    cout << "mu_out:\n" << mu_out << endl;
    cout << "T_out:\n" << T_out << endl;

#endif

#if 0
    cout << "MVNORMAL\n" << endl;
    MatrixXd out = MvNormal(T, mu, 10);
    cout << "mu:\n" << mu << endl;
    cout << "T:\n" << T << endl;
    cout << "out:\n" << out << endl;
#endif
#endif
}

#endif


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

// Program to generate test values for the modified Bessel function of
// the first kind using boost.  Usage:
//
//      c++ -o bessel bessel.cpp
//      ./bessel

#include <boost/math/distributions/normal.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <cstdio>

int main(int argc, char **argv) {
    const size_t n = 100;
    const double lo = -709;
    const double hi = +709;
    const double w = (hi - lo)/n;
    unsigned nu;
    size_t i;

    (void)argc;
    (void)argv;

    for (nu = 0; nu < 2; nu++) {
        if (printf("static const double i%ue[][2] = {\n", nu) < 0) {
            perror("printf");
            return 1;
        }
        if (printf("    // Uniform [-709, +709] grid\n") < 0) {
            perror("printf");
            return 1;
        }
        for (i = 0; i < n; i++) {
            const double x = lo + i*w;
            const double y = boost::math::cyl_bessel_i(nu, x);
            if (printf("    { %.17e, %.17e },\n", x, y) < 0) {
                perror("printf");
                return 1;
            }
        }
        if (printf("    // N(0, 1e-4) grid\n") < 0) {
            perror("printf");
            return 1;
        }
        for (i = 0; i < n; i++) {
            boost::math::normal norm(0, 1e-4);
            const double p = static_cast<double>(i + 1)/(n + 2);
            const double x = boost::math::quantile(norm, p);
            const double y = boost::math::cyl_bessel_i(nu, x);
            if (printf("    { %.17e, %.17e },\n", x, y) < 0) {
                perror("printf");
                return 1;
            }
        }
        if (printf("    // N(15, 1e-4) grid\n") < 0) {
            perror("printf");
            return 1;
        }
        for (i = 0; i < n; i++) {
            boost::math::normal norm(15, 1e-4);
            const double p = static_cast<double>(i + 1)/(n + 2);
            const double x = boost::math::quantile(norm, p);
            const double y = boost::math::cyl_bessel_i(nu, x);
            if (printf("    { %.17e, %.17e },\n", x, y) < 0) {
                perror("printf");
                return 1;
            }
        }
        if (printf("};\n") < 0) {
            perror("printf");
            return 1;
        }
    }

    return 0;
}


#include <mitsuba/render/fresnel.h>
#include <mitsuba/layer/microfacet.h>
#include <mitsuba/layer/fourier.h>
#include <mitsuba/core/frame.h>
#include <mitsuba/core/math.h>
#include <enoki/special.h>
#include <Eigen/SVD>
#include <Eigen/LU>
#include <cmath>
#include <chrono>
#include <atomic>

using namespace std::chrono;

#if defined HAVE_FFTW
    #include <fftw3.h>
    // #define USE_FFTW
#endif

NAMESPACE_BEGIN(mitsuba)

std::atomic<double> microfacet_timer;

double microfacet(double mu_o, double mu_i, double phi_s, double phi_d,
                  double alpha_u, double alpha_v,
                  std::complex<double> eta_,
                  bool isotropic_g) {
    double phi_i = 0.5 * (phi_s - phi_d);
    double phi_o = 0.5 * (phi_s + phi_d);

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi_i = std::cos(phi_i),
           sin_phi_i = std::sin(phi_i),
           cos_phi_o = std::cos(phi_o),
           sin_phi_o = std::sin(phi_o);

    Vector3f wi(-sin_theta_i*cos_phi_i,
                -sin_theta_i*sin_phi_i,
                -mu_i);
    Vector3f wo(sin_theta_o*cos_phi_o,
                sin_theta_o*sin_phi_o,
                mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    /* The following two visibility checks are usually part of the smith_G1
       implementations, but are considered to be part of the "Fresnel + Remainder"
       term in this framework. */
    if (dot(wi, wh) * Frame3f::cos_theta(wi) <= 0 || dot(wo, wh) * Frame3f::cos_theta(wo) <= 0)
        return 0;

    double cos_theta_h = (double)Frame3f::cos_theta(wh),
           cos_theta_h_2 = cos_theta_h * cos_theta_h;

    double exponent = ((double) (wh.x() * wh.x()) / (alpha_u * alpha_u) +
                       (double) (wh.y() * wh.y()) / (alpha_v * alpha_v)) / cos_theta_h_2;

    /* For the isotropic shadowing-masking function, take max{alpha_u, alpha_v} to ensure energy conservation. */
    double alpha_u_g = isotropic_g ? std::max(alpha_u, alpha_v) : alpha_u;
    double alpha_v_g = isotropic_g ? alpha_u_g : alpha_v;

    double eta_real = std::real(eta),
           eta_imag = std::imag(eta);

    double D = cos_theta_h <= 0 ? 0.0
                                : std::exp(-exponent) / (math::Pi<double> * alpha_u * alpha_v * cos_theta_h_2 * cos_theta_h_2),
           F = !conductor ? (double) std::get<0>(fresnel(float(dot(wi, wh)), float(std::real(eta_))))
                          : (double) fresnel_conductor(float(dot(wi, wh)), enoki::Complex<float>(eta_real, eta_imag)),
           G = smith_G1(wi, wh, alpha_u_g, alpha_v_g) * smith_G1(wo, wh, alpha_u_g, alpha_v_g);

    if (reflect) {
        return F * D * G / (4 * std::abs(mu_i*mu_o));
    } else {
        double sqrt_denom = (double) dot(wi, wh) + std::real(eta) * (double) dot(wo, wh);

        return std::abs(((1 - F) * D * G * std::real(eta) * std::real(eta) * (double) dot(wi, wh)
                         * (double) dot(wo, wh)) / (mu_i*mu_o * sqrt_denom * sqrt_denom));
    }
}

double microfacet_exp(double mu_o, double mu_i, double phi_s, double phi_d,
                      double alpha_u, double alpha_v,
                      std::complex<double> eta_) {
    double phi_i = 0.5 * (phi_s - phi_d);
    double phi_o = 0.5 * (phi_s + phi_d);

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi_i = std::cos(phi_i),
           sin_phi_i = std::sin(phi_i),
           cos_phi_o = std::cos(phi_o),
           sin_phi_o = std::sin(phi_o);

    Vector3f wi(-sin_theta_i*cos_phi_i,
                -sin_theta_i*sin_phi_i,
                -mu_i);
    Vector3f wo(sin_theta_o*cos_phi_o,
                sin_theta_o*sin_phi_o,
                mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    double cos_theta_h_2 = (double) Frame3f::cos_theta_2(wh);

    double exponent = ((double) (wh.x() * wh.x()) / (alpha_u * alpha_u) +
                       (double) (wh.y() * wh.y()) / (alpha_v * alpha_v)) / cos_theta_h_2;

    return std::exp(-exponent);
}

double microfacet_fresnel(double mu_o, double mu_i, double phi_s, double phi_d,
                          double alpha_u, double alpha_v,
                          std::complex<double> eta_,
                          bool fresnel_only) {
    (void)phi_s; // Unused

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi = std::cos(phi_d),
           sin_phi = std::sin(phi_d);

    Vector3f wi(-sin_theta_i, 0, -mu_i);
    Vector3f wo(sin_theta_o*cos_phi, sin_theta_o*sin_phi, mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    /* The following two visibility checks are usually part of the smith_G1
       implementations, but are considered to be part of the "Fresnel + Remainder"
       term in this framework. */
    if (dot(wi, wh) * Frame3f::cos_theta(wi) <= 0 || dot(wo, wh) * Frame3f::cos_theta(wo) <= 0)
        return 0;

    double cos_theta_h = (double) Frame3f::cos_theta(wh),
           cos_theta_h_2 = cos_theta_h * cos_theta_h;

    double eta_real = std::real(eta),
           eta_imag = std::imag(eta);

    double D = cos_theta_h == 0 ? 0.0
                                : 1.0 / (math::Pi<double> * alpha_u * alpha_v * cos_theta_h_2 * cos_theta_h_2),
           F = !conductor ? (double) std::get<0>(fresnel(float(dot(wi, wh)), float(std::real(eta_))))
                          : (double) fresnel_conductor(float(dot(wi, wh)), enoki::Complex<float>(eta_real, eta_imag));

    if (fresnel_only) {
        if (reflect) {
            return F;
        } else {
            return std::abs(1-F);
        }
    }

    if (reflect) {
        return F * D / (4.0 * std::abs(mu_i*mu_o));
    } else {
        double sqrt_denom = (double) dot(wi, wh) + std::real(eta) * (double) dot(wo, wh);
        return std::abs(((1 - F) * D * std::real(eta) * std::real(eta) * (double) dot(wi, wh)
                        * (double) dot(wo, wh)) / (mu_i*mu_o * sqrt_denom * sqrt_denom));
    }
}

double microfacet_G(double mu_o, double mu_i, double phi_s, double phi_d,
                    double alpha_u, double alpha_v,
                    std::complex<double> eta_,
                    bool isotropic_g ) {
    double phi_i = 0.5 * (phi_s - phi_d);
    double phi_o = 0.5 * (phi_s + phi_d);

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi_i = std::cos(phi_i),
           sin_phi_i = std::sin(phi_i),
           cos_phi_o = std::cos(phi_o),
           sin_phi_o = std::sin(phi_o);

    Vector3f wi(-sin_theta_i*cos_phi_i,
                -sin_theta_i*sin_phi_i,
                -mu_i);
    Vector3f wo(sin_theta_o*cos_phi_o,
                sin_theta_o*sin_phi_o,
                mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1.0) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    /* For the isotropic shadowing-masking function, take max{alpha_u, alpha_v}
       to ensure energy conservation. */
    double alpha_u_g = isotropic_g ? std::max(alpha_u, alpha_v) : alpha_u;
    double alpha_v_g = isotropic_g ? alpha_u_g : alpha_v;

    return smith_G1(wi, wh, alpha_u_g, alpha_v_g) *
           smith_G1(wo, wh, alpha_u_g, alpha_v_g);
}

double smith_G1(const Vector3f &v, const Vector3f &m, double alpha_u, double alpha_v) {
    (void)m; // Unused

    /* The following visibility check usually found in smith_G1 implementations is
       treated in the "Fresnel + Remainder" term in this framework. */
    // if (dot(v, m) * Frame3f::cos_theta(v) <= 0)
    //     return 0;

    const double tan_theta = (double) std::abs(Frame3f::tan_theta(v));
    if (tan_theta == 0)
        return 1;

    double alpha = project_roughness(v, alpha_u, alpha_v);

    double a = 1.0 / (alpha * tan_theta);
    if (a < 1.6) {
        /* Use a fast and accurate (<0.35% rel. error) rational
           approximation to the shadowing-masking function */
        const double a_sqr = a * a;
        return (3.535 * a + 2.181 * a_sqr)
             / (1.0 + 2.276 * a + 2.577 * a_sqr);
    }

    return 1.0;
}

VectorX smith_G1_fourier_series(double mu, double alpha_u, double alpha_v,
                                size_t order, size_t n_samples) {
    /* Here we compute the Fourier series of the Smith Shadowing-Masking component,
       for one of the directions phi_i, phi_o via Filon integration.
       Note that we need to sample the function in the domain [0, pi] and perform
       the integration over [0, 2pi] s.t. the "diagonal" versions of these series
       end up being 2pi-periodic in phi_s, phi_d. */
    double cos_theta     = std::abs(mu),
           sin_theta     = std::sqrt(1.0 - cos_theta*cos_theta),
           tan_theta     = sin_theta / cos_theta;

    double phi_step     =  math::Pi<double> / (n_samples - 1),
           cos_phi_prev =  std::cos(phi_step), cos_phi_cur = 1.0,
           sin_phi_prev = -std::sin(phi_step), sin_phi_cur = 0.0,
           two_cos_phi  =  2.0 * cos_phi_prev;

    VectorXc values(n_samples);

    for (size_t i = 0; i < n_samples; ++i) {
        double cos_phi_next = two_cos_phi*cos_phi_cur - cos_phi_prev,
               sin_phi_next = two_cos_phi*sin_phi_cur - sin_phi_prev;

        double cos_phi_2 = cos_phi_cur*cos_phi_cur;
        double sin_phi_2 = sin_phi_cur*sin_phi_cur;

        double alpha = std::sqrt(cos_phi_2 * alpha_u * alpha_u +
                                 sin_phi_2 * alpha_v * alpha_v);

        double result = 1.0;
        double a = 1.0 / (alpha * tan_theta);
        if (a < 1.6) {
            /* Use a fast and accurate (<0.35% rel. error) rational
               approximation to the shadowing-masking function */
            double a_sqr = a * a;
            result = (3.535 * a + 2.181 * a_sqr)
                 / (1.0 + 2.276 * a + 2.577 * a_sqr);
        }
        values[i] = result;

        cos_phi_prev = cos_phi_cur; cos_phi_cur = cos_phi_next;
        sin_phi_prev = sin_phi_cur; sin_phi_cur = sin_phi_next;
    }

    VectorXc coeffs(order);
    coeffs.setZero();
    filon_integrate_exp(values.data(), n_samples, coeffs.data(), order, 0, 2*math::Pi<double>);

    return coeffs.real();
}

double project_roughness(const Vector3f &v, double alpha_u, double alpha_v) {
    double inv_sin_theta_2 = 1.0 / (double) Frame3f::sin_theta_2(v);

    if ((alpha_u == alpha_v) || inv_sin_theta_2 < 0)
        return alpha_u;

    double cos_phi_2 = (double) (v.x() * v.x()) * inv_sin_theta_2;
    double sin_phi_2 = (double) (v.y() * v.y()) * inv_sin_theta_2;

    return std::sqrt(cos_phi_2 * alpha_u * alpha_u +
                     sin_phi_2 * alpha_v * alpha_v);
}

static int expcos_coefficient_count(double B, double relerr) {
    double prod = 1, invB = 1 / B;
    if (B == 0)
        return 1;

    for (int i = 0; ; ++i) {
        prod /= 1 + i * invB;

        if (prod < relerr)
            return 2*i+1;
    }
}

static double mod_bessel_ratio(double z, double k) {
    const double eps = std::numeric_limits<double>::epsilon(),
                 inv_two_b = 2 / z;

    double i  = (double) k,
           D  = 1 / (inv_two_b * i++),
           Cd = D,
           C  = Cd;

    while (std::abs(Cd) > eps * std::abs(C)) {
        double coeff = inv_two_b * i++;
        D = 1 / (D + coeff);
        Cd *= coeff*D - 1;
        C += Cd;
    }

    return C;
}

static void bessel_functions(double c, double z, int n, VectorX &bessel) {
    bessel.resize(n);

    /* Determine the last ratio and work downwards */
    bessel[n-1] = mod_bessel_ratio(z, n - 1);
    for (int i = n-2; i > 0; --i)
        bessel[i] = z / (2*i + z*bessel[i+1]);

    /* Evaluate the exponentially scaled I0 and correct scaling */
    bessel[0] = enoki::i0e(z) * std::exp(z + c);

    /* Apply the ratios upwards */
    double prod = bessel[0];
    for (int i = 1; i < n; ++i) {
        prod *= bessel[i];
        bessel[i] = prod;
    }
}

static double max_B_heuristic(size_t m, double relerr) {
    if (relerr >= 1e-1)
        return 0.1662*std::pow((double) m, 2.05039);
    else if (relerr >= 1e-2)
        return 0.0818*std::pow((double) m, 2.04982);
    else if (relerr >= 1e-3)
        return 0.0538*std::pow((double) m, 2.05001);
    else if (relerr >= 1e-4)
        return 0.0406*std::pow((double) m, 2.04686);
    else if (relerr >= 1e-5)
        return 0.0337*std::pow((double) m, 2.03865);
    else if (relerr >= 1e-6)
        return 0.0299*std::pow((double) m, 2.02628);
    else {
        Log(Warn, "max_B(): unknown relative error bound!");
        return math::Infinity<double>;
    }
}

void microfacet_reflection_exp_coeffs(double mu_o, double mu_i, double alpha_u, double alpha_v, double phi_s, VectorX &c) {
    c.resize(3);

    double denom = 1.0 / (alpha_u * alpha_v * (mu_i - mu_o)),
           alpha_sqr_sum = (alpha_u * alpha_u + alpha_v * alpha_v),
           alpha_sqr_dif = (alpha_u * alpha_u - alpha_v * alpha_v),
           root = safe_sqrt((1 - mu_i * mu_i) * (1 - mu_o * mu_o)),
           tmp = (mu_i * mu_i + mu_o * mu_o - 2);

    double c0 = 0.5 * denom * denom * alpha_sqr_sum * tmp,
           c1 = denom * denom * alpha_sqr_sum * root,
           c2 = denom * denom * -alpha_sqr_dif * root,
           c3 = 0.5 * denom * denom * -alpha_sqr_dif * (mu_o * mu_o - 1),
           c4 = 0.5 * denom * denom * -alpha_sqr_dif * (mu_i * mu_i - 1);

    std::complex<double> z(c1 + (c3 + c4) * std::cos(phi_s),
                                (c3 - c4) * std::sin(phi_s));

    double A = c0 + c2 * std::cos(phi_s),
           B = std::abs(z),
           C = std::arg(z);

    c[0] = A;
    c[1] = B;
    c[2] = C;
}

void microfacet_refraction_exp_coeffs(double mu_o, double mu_i, double alpha_u, double alpha_v, double phi_s, double eta, VectorX &c) {
    c.resize(3);

    double denom = 1.0 / (alpha_u * alpha_v * (mu_i - eta * mu_o)),
           alpha_sqr_sum = (alpha_u * alpha_u + alpha_v * alpha_v),
           alpha_sqr_dif = (alpha_u * alpha_u - alpha_v * alpha_v),
           root = safe_sqrt((1 - mu_i * mu_i) * (1 - mu_o * mu_o));

    double c0 = 0.5 * denom * denom * alpha_sqr_sum * (mu_i * mu_i - 1 + eta * eta * (mu_o * mu_o - 1)),
           c1 = denom * denom * alpha_sqr_sum * eta * root,
           c2 = denom * denom * -alpha_sqr_dif * eta * root,
           c3 = 0.5 * denom * denom * -alpha_sqr_dif * eta * eta * (mu_o * mu_o - 1),
           c4 = 0.5 * denom * denom * -alpha_sqr_dif * (mu_i * mu_i - 1);

    std::complex<double> z(c1 + (c3 + c4) * std::cos(phi_s),
                                (c3 - c4) * std::sin(phi_s));

    double A = c0 + c2 * std::cos(phi_s),
           B = std::abs(z),
           C = std::arg(z);

    c[0] = A;
    c[1] = B;
    c[2] = C;
}

VectorXc exp_cos_fourier_series(double A, double B, double C, double relerr) {
    VectorXc result;
    int md = expcos_coefficient_count(B, relerr);
    int mdh = md / 2;

    // Create Fourier Series for exp(A + B * cos(phi_d + 0))
    VectorX bessel;
    bessel_functions(A, B, mdh + 1, bessel);

    int truncated_size = bessel.size();
    for (int i = 1; i < bessel.size(); ++i) {
         if (2*std::abs(bessel[i]) < bessel[0] * relerr) {
            truncated_size = i;
            break;
        }
    }
    VectorX bessel_trunc = bessel.head(truncated_size);
    mdh = bessel_trunc.size() - 1;
    md = 2*mdh+1;

    result.resize(md);

    result.head(mdh) = bessel_trunc.tail(mdh).reverse().cast<std::complex<double>>();
    result.tail(mdh + 1) = bessel_trunc.cast<std::complex<double>>();

    if (std::abs(C) > 0) {
        // Shift Fourier Series by phase C
        std::complex<double> exp_phase_inc = std::exp(1i * C);
        std::complex<double> exp_phase = std::exp(-1i * (double) mdh * C);
        for (int i = 0; i < md; ++i) {
            result[i] *= exp_phase;
            exp_phase *= exp_phase_inc;
        }
    }

    return result;
}

inline void sample_function(const std::function<double(double)> &f,
                            double *values, size_t size,
                            double a, double b) {
    double delta = (b - a) / (size - 1);
    for (size_t i = 0; i < size; ++i) {
        double x = a + i*delta;
        values[i] = f(x);
    }
}

inline MatrixX coefficient_conversion_matrix(int m, double phi_a, double phi_b) {
    /* Precompute some sines and cosines */
    VectorX cos_phi_a(m), sin_phi_a(m), cos_phi_b(m), sin_phi_b(m);
    for (int i = 0; i < m; ++i) {
        std::tie(sin_phi_a[i], cos_phi_a[i]) = enoki::sincos(i*phi_a);
        std::tie(sin_phi_b[i], cos_phi_b[i]) = enoki::sincos(i*phi_b);
    }

    MatrixX A(m, m);
    for (int i = 0; i < m; ++i) {
        for (int j = 0; j <= i; ++j) {
            if (i != j) {
                A(i, j) = A(j, i) = (i * cos_phi_b[j] * sin_phi_b[i] +
                                     j * cos_phi_a[i] * sin_phi_a[j] -
                                     i * cos_phi_a[j] * sin_phi_a[i] -
                                     j * cos_phi_b[i] * sin_phi_b[j]) / (i*i - j*j);
            } else if (i != 0) {
                A(i, i) = (std::sin(2*i * phi_b) -
                           std::sin(2*i * phi_a) +
                           2*i * (phi_b - phi_a)) / (4*i);
            } else {
                A(i, i) = phi_b - phi_a;
            }
        }
    }
    return A;
}

VectorX fresnel_fourier_series(double mu_o, double mu_i,
                                double alpha_u, double alpha_v,
                                std::complex<double> eta_,
                                int md, double phi_max,
                                bool svd_reg, bool fresnel_only) {
    int m = md / 2 + 1; // Number of Cosine series coefficients
    VectorX result;

    bool reflect = -mu_i * mu_o > 0;

    double sin_mu_2 = safe_sqrt((1.0 - mu_i * mu_i) * (1.0 - mu_o * mu_o)),
           phi_critical = 0.0;

    bool conductor = (std::imag(eta_) != 0);
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    if (reflect) {
        if (!conductor) {
            double tmp = (2.0*std::real(eta)*std::real(eta) - mu_i*mu_o - 1.0) / sin_mu_2;
            phi_critical = safe_acos(tmp);
        }
    } else if (!reflect) {
        if (conductor) {
            Throw("microfacet_no_exp_fourier_series(): Encountered refraction case for a conductor!");
        }
        double eta_denser = (std::real(eta) > 1 ? std::real(eta) : 1 / std::real(eta));
        double tmp = (1 - eta_denser * mu_i * mu_o) / (eta_denser * sin_mu_2);
        phi_critical = safe_acos(tmp);
    }

    bool phi_critical_inside_interval;
    if (reflect && mu_i > 0) {
        /* For reflection from bottom, phi_critical = 0 results in a high-frequency feature that should be captured. */
        phi_critical_inside_interval = phi_critical < phi_max - math::Epsilon<double>;
    } else {
        phi_critical_inside_interval = phi_critical > math::Epsilon<double> && phi_critical < phi_max - math::Epsilon<double>;
    }

    if (!conductor && phi_critical_inside_interval) {
        /* Uh oh, some high frequency content (critical angle) leaked in the
           generally low frequency part. Increase the number of coefficients so
           that we can capture it. Fortunately, this happens very rarely. */
        m = std::max(m, 100);
        md = 2*m-1;
    }

    VectorX coeffs;
    if (svd_reg) {
        coeffs.resize(m);
    } else {
        coeffs.resize(2*m); // Allocate two times the space for QR regularization below
    }
    coeffs.setZero();

    const int samples = 200;
    auto integrand = std::bind(&microfacet_fresnel, mu_o, mu_i, 0.0, std::placeholders::_1, alpha_u, alpha_v, eta_, fresnel_only);

    if (reflect) {
        if (phi_critical_inside_interval) {
            filon_integrate_cosine(integrand, samples, coeffs.data(), m, 0, phi_critical);
            filon_integrate_cosine(integrand, samples, coeffs.data(), m, phi_critical, phi_max);
        } else {
            filon_integrate_cosine(integrand, samples, coeffs.data(), m, 0, phi_max);
        }
    } else {
        filon_integrate_cosine(integrand, samples, coeffs.data(), m, 0, std::min(phi_critical, phi_max));
    }

    if (phi_max < math::Pi<double> - math::Epsilon<double>) {
        /* The fit only occurs on a subset [0, phi_max], where the Fourier
           basis functions are not orthogonal anymore! The following then
           does a change of basis to proper Fourier coefficients. */

        auto start = high_resolution_clock::now();

        if (svd_reg) {
            MatrixX A = coefficient_conversion_matrix(m, 0, phi_max);

            auto svd = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
            const MatrixX &U = svd.matrixU();
            const MatrixX &V = svd.matrixV();
            const VectorX &sigma = svd.singularValues();

            if (sigma[0] == 0) {
                result.resize(1);
                result(0) = 0;
                return result;
            }

            VectorX temp = VectorX::Zero(m);
            coeffs[0] *= math::Pi<double>;
            coeffs.tail(m-1) *= 0.5 * math::Pi<double>;
            for (int i = 0; i < m; ++i) {
                if (sigma[i] < 1e-9 * sigma[0])
                    break;
                temp += V.col(i) * U.col(i).dot(coeffs) / sigma[i];
            }
            result = temp;
        } else {
            double eps = 1e-5;
            MatrixX A(2*m, m);

            /* We want the fit to be as close-as-possible on the interval [0, phi_max].
             We then add soft regularization constraints for the series to be small outside. */
            A.block(0, 0, m, m) = coefficient_conversion_matrix(m, 0, phi_max);
            A.block(m, 0, m, m) = coefficient_conversion_matrix(m, phi_max, math::Pi<double>) * eps;

            // Scale coefficients
            coeffs *= 0.5*math::Pi<double>;
            coeffs[0] *= 2;

            // QR factorization
            result = A.colPivHouseholderQr().solve(coeffs);
        }

        auto end = high_resolution_clock::now();
        duration<double> diff = (end-start);

        while (true) {
            double old = microfacet_timer;
            double update = old + diff.count();
            if (microfacet_timer.compare_exchange_strong(old, update))
                break;
        }

    } else {
        result = coeffs.head(m);   // No regularization necessary, just get rid of the zero padding from before again.
    }

    VectorX result_exp(md);
    result_exp.head(m - 1) = 0.5*result.tail(m - 1).reverse();
    result_exp[m-1] = result[0];
    result_exp.tail(m - 1) = 0.5*result.tail(m - 1);

    return result_exp;
}

void microfacet_fourier_series(double mu_o, double mu_i,
                               double alpha_u, double alpha_v,
                               std::complex<double> eta_,
                               int ms, int md, double relerr, MatrixX &result,
                               int component, int n_samples_phi_s, bool svd_reg) {
    bool reflect = -mu_i * mu_o > 0;

    bool conductor = (std::imag(eta_) != 0.0);
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    if (!reflect) {
        if (conductor) {
            /* No refraction in conductors */
            result.resize(1, 1);
            result(0, 0) = 0;
            return;
        }
    }

    if (component == 2) {
        /* Only compute shadowing-masking component */
        VectorX smith_G1_coeffs_i = smith_G1_fourier_series(mu_i, alpha_u, alpha_v, 9);
        VectorX smith_G1_coeffs_o = smith_G1_fourier_series(mu_o, alpha_u, alpha_v, 9);

        MatrixX G1_coeffs(9, 9);
        G1_coeffs.setZero();
        G1_coeffs.diagonal(0) = smith_G1_coeffs_o;

        MatrixX G2_coeffs;
        convolve_fourier_series_antidiagonal(smith_G1_coeffs_i, G1_coeffs, G2_coeffs);

        result = G2_coeffs;
        return;
    }

    /* At grazing angles, microfacet BSDFs can contain arbitrarily high frequencies.
       This value sets an upper bound on exponential cosine series parameter 'B' to
       prevent ringing in such cases. */
    double max_B = max_B_heuristic(md/2+1, relerr);

    /* Here we compute the Fourier coefficients of the Microfacet "remainder term", i.e.
       Fresnel and some normalizations. This is isotropic and thus invariant over phi_s,
       so we only compute it once here.
       This part is later convolved with the exponential component, so it makes sense to
       only compute it accurately for the interval [0, phi_max] in which the exponential
       component takes large enough values.
       We find such a conservative interval (useful for all phi_s) with a golden section search here. */
    VectorX fresnel_coeffs;
    if (component == 0 || component == 3) {
        auto gss = [] (const std::function<double(double)>& f, double a, double b, double tol=1e-3) {
            double gr = 0.5*(std::sqrt(5.0) + 1.0);
            double inv_gr = 1.0 / gr;

            while (true) {
                double x1 = b - (b - a) * inv_gr,
                       x2 = a + (b - a) * inv_gr;

                if (std::abs(b - a) < tol)
                    break;
                else if (f(x1) < f(x2))
                    b = x2;
                else
                    a = x1;
            }

            return 0.5*(a + b);
        };

        auto get_phi = [&] (double phi_s) {
            VectorX c;
            if (reflect) {
                microfacet_reflection_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, c);
            } else {
                microfacet_refraction_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, std::real(eta), c);
            }
            double A = c[0];
            double B = c[1];
            double C = c[2];

            if (B > max_B && (std::abs(mu_i) < 0.2 || std::abs(mu_o) < 0.2)) {
                A = A + B - max_B + std::log(enoki::i0e(B) / enoki::i0e(max_B));
                B = max_B;
            }

            double phi_max = safe_acos(1.0 + std::log(relerr) / B);
            return -phi_max - C;
        };

        double phi_s = gss(get_phi, -math::Pi<double>, math::Pi<double>);
        double phi_max = std::min(-get_phi(phi_s), math::Pi<double>);

        fresnel_coeffs = fresnel_fourier_series(mu_o, mu_i,
                                                alpha_u, alpha_v, eta_,
                                                23, phi_max, svd_reg, component==3);
    }

    /* Sample phi_s dimension with regular samples and for each, create a Fourier series in phi_d. */

#ifdef USE_FFTW
    n_samples_phi_s = ms;   // Need as many samples as Fourier orders requested for FFTW integration
#else
    n_samples_phi_s = std::max(2, n_samples_phi_s); // Need at least 2 samples in order to do Filon integration.
#endif

    bool isotropic = false;
    if (alpha_u == alpha_v) {
        /* Isotropic Microfacet model. Only compute a 1D Fourier series and
           zero pad coefficient matrix to have dimension ms x md later. */
        n_samples_phi_s = 1;
        isotropic = true;
    }

    int md_max = 0;
    std::vector<VectorXc> coeffs;
    for (int s = 0; s < n_samples_phi_s; ++s) {
        #ifdef USE_FFTW
            int n_steps = n_samples_phi_s;
        #else
            int n_steps = n_samples_phi_s - 1;
        #endif

        double phi_s = 2 * math::Pi<double> * s / n_steps;

        if (n_samples_phi_s == 1) phi_s = 0;    // Handle division by zero for isotropic cases where ms == 1

        VectorX c;
        if (reflect) {
            microfacet_reflection_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, c);
        } else {
            microfacet_refraction_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, std::real(eta), c);
        }
        double A = c[0];
        double B = c[1];
        double C = c[2];

        /* Minor optimization: don't even bother computing the Fourier series
           if the contribution to the scattering model is miniscule */
        if (enoki::i0e(B) * std::exp(A + B) < 1e-10 && component == 0) {
            VectorXc final_coeffs(1);
            final_coeffs.setZero();
            md_max = std::max(md_max, (int) final_coeffs.size());
            coeffs.push_back(final_coeffs);
            continue;
        }

        if (B > max_B && (std::abs(mu_i) < 0.2 || std::abs(mu_o) < 0.2)) {
            A = A + B - max_B + std::log(enoki::i0e(B) / enoki::i0e(max_B));
            B = max_B;
        }

        /* Compute Fourier coefficients of the exponential term */
        VectorXc exp_coeffs = exp_cos_fourier_series(A, B, C, relerr);

        /* Perform discrete convolution of the exponential & Fresnel series, if requested */
        VectorXc final_coeffs;
        if (component == 0) {
            /* full model */
            convolve_fourier_series(fresnel_coeffs, exp_coeffs, final_coeffs);
        } else if (component == 1) {
            /* exponential part only */
            final_coeffs = exp_coeffs;
        } else if (component == 3) {
            /* Fresnel + remainder part */
            final_coeffs = fresnel_coeffs.cast<std::complex<double>>();
        }

        /* Already truncate very low coefficients before considering integration over phi_s */
        int md_tmp = final_coeffs.size(),
            mdh_tmp = md_tmp / 2;
        double zero_coeff = std::abs(final_coeffs[mdh_tmp]);
        double ref = zero_coeff * relerr;

        if (zero_coeff < relerr) {
            final_coeffs.resize(1);
            final_coeffs[0] = 0;
        } else {
            int md_trunc = md_tmp;
            for (int d = mdh_tmp; d >= 0; --d) {
                int od = d + mdh_tmp;
                if (std::abs(final_coeffs[od]) >= ref) {
                    md_trunc = 2*d+1;
                    break;
                }
            }
            int mdh_trunc = md_trunc / 2;

            VectorXc truncated = final_coeffs.segment(mdh_tmp - mdh_trunc, md_trunc);
            final_coeffs.resize(truncated.size());
            final_coeffs = truncated;
        }

        md_max = std::max(md_max, (int) final_coeffs.size());
        coeffs.push_back(final_coeffs);
    }

    /* Collect data in matrix */
    int md_max_half = md_max / 2;
    MatrixXc temp(n_samples_phi_s, md_max);
    temp.setZero();
    for (int i = 0; i < n_samples_phi_s; ++i) {
        int md = coeffs[i].size();
        int mdh = md / 2;
        temp.block(i, md_max_half - mdh, 1, md) = coeffs[i].transpose();
    }

    MatrixX result_tmp(ms, md_max);
    result_tmp.setZero();

    /* Integrate the sampled Fourier series over phi_s. */

    if (ms == 1) {
        /* We only have a 1D Fourier series, thus no integration over phi_s needed. */
        result_tmp.row(0) = temp.row(0).real();
    } else {
        #ifdef USE_FFTW
            /* Use FFT to compute Fourier Series of phi_s dimension */
            std::complex<double> *data       = (std::complex<double> *) fftw_malloc(sizeof(std::complex<double>) * ms),
                                 *spectrum   = (std::complex<double> *) fftw_malloc(sizeof(std::complex<double>) * ms);

            fftw_plan_with_nthreads(1);
            fftw_plan plan = fftw_plan_dft_1d(ms, (fftw_complex *) data, (fftw_complex *) spectrum, FFTW_FORWARD, FFTW_ESTIMATE);

            for (int d = 0; d < md_max; ++d) {
                for (int s = 0; s < ms; ++s) {
                    data[s] = temp(s, d);
                }
                fftw_execute(plan);
                for (int s = 0; s < ms; ++s) {
                    temp(s, d) = spectrum[s] / (double) ms;
                }
            }

            fftw_destroy_plan(plan);
            fftw_free(spectrum);
            fftw_free(data);

            /* Reorder Fourier coefficients. And we know that the coefficients must be real. */
            int msh = ms / 2;
            result_tmp.row(msh) = temp.row(0).real();
            result_tmp.block(0, 0, msh, md_max) = temp.block(msh + 1, 0, msh, md_max).real();
            result_tmp.block(msh + 1, 0, msh, md_max) = temp.block(1, 0, msh, md_max).real();
        #else
            if (isotropic || component == 3) {
                /* No integration over phi_s necessary */
                int msh = ms / 2;
                result_tmp.block(msh, 0, 1, md_max) = temp.real();
            } else {
                /* Compute Fourier series over phi_s with Filon integration.
                   We can save some time here by exploiting symmetry. Only integrate
                   one half of the data and then mirror it. */
                MatrixXc result_tmp_complex(ms, md_max);
                result_tmp_complex.setZero();
                for (int d = 0; d <= md_max_half; ++d) {
                    int od = d + md_max_half;
                    filon_integrate_exp(temp.col(od).data(), n_samples_phi_s, result_tmp_complex.col(od).data(), ms);
                }
                result_tmp_complex.block(0, 0, ms, md_max_half) = result_tmp_complex.block(0, md_max_half+1, ms, md_max_half);
                result_tmp_complex.block(0, 0, ms, md_max_half) = result_tmp_complex.block(0, 0, ms, md_max_half).colwise().reverse().eval();
                result_tmp_complex.block(0, 0, ms, md_max_half) = result_tmp_complex.block(0, 0, ms, md_max_half).rowwise().reverse().eval();

                /* We know that the coefficients must be real. */
                result_tmp = result_tmp_complex.real();
            }

        #endif
    }

    for (int s = 0; s < ms; ++s) {
      for (int d = 0; d < md_max; ++d) {
        if (std::abs(result_tmp(s, d)) < 1e-9) result_tmp(s, d) = 0;
      }
    }

    /* Truncate Fourier series */
    int md_trunc = md_max,
        ms_trunc = ms;

    int msh = ms / 2,
        mdh = md_max_half;

    double zero_coeff = std::abs(result_tmp(msh, mdh));
    double ref = zero_coeff * relerr * 0.01;

    // Check in phi_s dimension
    for (int s = 0; s <= msh; ++s) {
        bool keep = false;
        for (int d = -mdh; d <= mdh; ++d) {
            double value = std::abs(result_tmp(s + msh, d + mdh));
            if (value > ref) keep = true;
        }
        if (keep) {
            ms_trunc = 2*s+1;
        } else {
            break;
        }
    }

    // Check in phi_d dimension
    for (int d = 0; d <= mdh; ++d) {
        bool keep = false;
        for (int s = -msh; s <= msh; ++s) {
            double value = std::abs(result_tmp(s + msh, d + mdh));
            if (value > ref) keep = true;
        }
        if (keep) {
            md_trunc = 2*d+1;
        } else {
            break;
        }
    }

    int msh_trunc = ms_trunc / 2,
        mdh_trunc = md_trunc / 2;
    result.resize(ms_trunc, md_trunc);

    result = result_tmp.block(msh - msh_trunc, mdh - mdh_trunc, ms_trunc, md_trunc);

    if (component == 0) {
        /* Requested full model: add Smith Shadowing-masking term via 2 diagonal convolutions */
        VectorX smith_G1_coeffs_i = smith_G1_fourier_series(mu_i, alpha_u, alpha_v, 19);
        VectorX smith_G1_coeffs_o = smith_G1_fourier_series(mu_o, alpha_u, alpha_v, 19);

        MatrixX tmp;
        convolve_fourier_series_diagonal(smith_G1_coeffs_o, result, tmp);
        convolve_fourier_series_antidiagonal(smith_G1_coeffs_i, tmp, result);
    }
}

bool microfacet_inside_lowfreq_interval(double mu_o, double mu_i, double phi_s,
                                        double alpha_u, double alpha_v,
                                        std::complex<double> eta,
                                        double phi_d, double relerr) {
    bool reflect = -mu_i * mu_o > 0;

    VectorX c;
    if (reflect) {
        microfacet_reflection_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, c);
    } else {
        microfacet_refraction_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, std::real(eta), c);
    }
    double B = c[1];
    double C = c[2];

    /* Ideally, we would also remove high frequencies here,
       but then this would have to depend on our chosen md as well. */
    // if (B > max_B) {
    //     A = A + B - max_B + std::log(enoki::i0e(B) / enoki::i0e(max_B));
    //     B = max_B;
    // }

    double phi_max = safe_acos(1.0 + std::log(relerr) / B),
           phi_a = -phi_max - C,
           phi_b = +phi_max - C;

    if (phi_d > phi_a && phi_d < phi_b)
        return true;

    return false;
}

NAMESPACE_END(mitsuba)


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

/**
 * @file odeint.cpp ODE numerical integration example.
 */

#include <boost/numeric/odeint.hpp>

#include <matplot/matplot.h>

#include "smooth/bundle.hpp"
#include "smooth/compat/odeint.hpp"
#include "smooth/so3.hpp"
#include "smooth/tn.hpp"

#include "plot_tools.hpp"

using matplot::plot;
using std::views::transform;

/**
 * @brief Numerically solve the following ODE on \f$ \mathbb{SO}(3) \times \mathbb{R}^3 \f$:
 */
int main(int argc, char const * argv[])
{
  using state_t = smooth::Bundle<smooth::SO3d, smooth::SO3d>;
  using deriv_t = typename state_t::Tangent;

  std::srand(2);

  // equilibrium point
  const smooth::SO3d Xc = smooth::SO3d::Identity();

  const smooth::SO3d X1 = smooth::SO3d::Random();
  const smooth::SO3d X2 = smooth::SO3d::Random();
  const smooth::SO3d::Tangent d = 0.1 * smooth::SO3d::Tangent::Random();

  auto ode = [&](const state_t & state, deriv_t & deriv, double) {
    deriv.head<3>() = 0.1 * (state.part<0>() - Xc);
    deriv.tail<3>() = 0.1 * (state.part<1>() - Xc);
  };

  auto stepper = boost::numeric::odeint::
    runge_kutta4<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra>();

  std::vector<double> tvec;
  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> v1, v2;

  state_t state1(X1, X1 + d), state2(X2, X2 + d);

  boost::numeric::odeint::integrate_const(
    stepper, ode, state1, 0., 10., 0.01, [&](const state_t & s, double t) {
      tvec.push_back(t);
      v1.push_back(s.part<1>() - s.part<0>());
    });

  boost::numeric::odeint::integrate_const(
    stepper, ode, state2, 0., 10., 0.01, [&](const state_t & s, double t) {
      v2.push_back(s.part<1>() - s.part<0>());
    });

  matplot::figure();
  matplot::hold(matplot::on);
  plot(tvec, r2v(v1 | transform([](auto s) { return s(0); })), "r") ->line_width(2);
  plot(tvec, r2v(v2 | transform([](auto s) { return s(0); })), ":r") ->line_width(2);
  plot(tvec, r2v(v1 | transform([](auto s) { return s(1); })), "g") ->line_width(2);
  plot(tvec, r2v(v2 | transform([](auto s) { return s(1); })), ":g") ->line_width(2);
  plot(tvec, r2v(v1 | transform([](auto s) { return s(2); })), "b") ->line_width(2);
  plot(tvec, r2v(v2 | transform([](auto s) { return s(2); })), ":b") ->line_width(2);
  matplot::title("Difference");

  matplot::show();

  return 0;
}


#include <Eigen/Dense>
#include <cmath>
#include <random>
#include "thermostats.h"
#include "constants.h"

std::random_device g_device;
std::mt19937 g_mt_rand(g_device());
std::uniform_real_distribution<double> distribution(0.0, 1.0);

Thermostat::Thermostat(double aim_temp) {
    m_aim_temp = aim_temp;
}

VelocityRescale::VelocityRescale(int steps, double aim_temp) {
    m_steps = steps;
    m_aim_temp = aim_temp;
}

Berendsen::Berendsen(double timescale, double aim_temp) {
    m_timescale = timescale;
    m_aim_temp = aim_temp;
}

Andersen::Andersen(double freq, double timestep, double aim_temp) {
    // The user provides us with a timestep in ps
    // and a collision frequency in ps^-1

    m_cutoff = freq * timestep;
    m_aim_temp = aim_temp;
}

void Berendsen::apply(Eigen::ArrayXXd &velocities, const Eigen::ArrayXd &masses) {
    auto velocities_squared = (velocities * velocities).rowwise().sum();
    auto effective_temp = (0.5 * masses * velocities_squared).sum() / constants::boltzHar;
    auto delta_T = effective_temp - m_aim_temp / m_timescale;
}

void VelocityRescale::apply(Eigen::ArrayXXd &velocities, const Eigen::ArrayXd &masses) {
    auto velocities_squared = (velocities * velocities).rowwise().sum();
    auto translational_energy = (0.5 * masses * velocities_squared).sum();
    double thermal_energy = constants::boltzHar * m_aim_temp;
    double velocity_factor = sqrt(thermal_energy / translational_energy);
    velocities *= velocity_factor;
}

void Andersen::apply(Eigen::ArrayXXd &velocities, const Eigen::ArrayXd &masses) {
    //Implements an Andersen thermostat. This keeps the system at
    //constant temperature by the following method:
    //    1) For each atom, calculate a "collision chance" in the last
    //       timestep. If this chance > collision frequency * timestep,
    //       then progress to 2
    //    2) Simulate a "collision" with an external heat bath. This
    //       randomly selects the velocity from a Maxwell-Boltzmann distribution

    for (int atom = 0; atom < velocities.rows(); ++atom) {
        double random_num = distribution(g_mt_rand);

        if (random_num > m_cutoff) {
            // The lucky atom is hit!
            double mean = 0.0;
            double variance = std::sqrt(constants::boltz * m_aim_temp / masses(atom));
            std::normal_distribution<double> gaussian(mean, variance);
            for (int dim=0; dim < velocities.cols(); ++dim) {
                velocities(atom, dim) = gaussian(g_mt_rand);
            }
        }
    }
}


// Copyright 2020 Erik Teichmann <kontakt.teichmann@gmail.com>

#ifndef INCLUDE_SAM_ANALYSIS_SAVITZKY_GOLAY_HPP_
#define INCLUDE_SAM_ANALYSIS_SAVITZKY_GOLAY_HPP_

#include <Eigen/Dense>

#include <stdexcept>
#include <vector>

namespace sam {

template<typename state_type = std::vector<double>>
std::vector<std::vector<double>> SavitzkyGolayFilter(double stepsize,
                                                     state_type state,
                                                     unsigned int n_points,
                                                     unsigned int derivatives);

// Implementation

Eigen::MatrixXd CalculateConvolutionCoefficients(unsigned int n_points,
                                                 unsigned int derivatives) {
  unsigned int n_left = n_points/2;
  Eigen::MatrixXd J(n_points, derivatives);
  for (unsigned int i = 0; i < n_points; ++i) {
    double z = static_cast<int>(i) - static_cast<int>(n_left);
    for (unsigned int j = 0; j < derivatives; ++j) {
      J(i, j) = pow(z, j);
    }
  }
  return (J.transpose()*J).inverse()*J.transpose();
}

int factorial(unsigned int n) {
  int factorial = 1.;
  for (unsigned int i = 1; i < n+1; ++i) factorial *= i;
  return factorial;
}

template<typename state_type>
std::vector<std::vector<double>> SavitzkyGolayFilter(double stepsize,
                                                     state_type state,
                                                     unsigned int n_points,
                                                     unsigned int derivatives) {
  if (n_points%2 == 0) {
    throw std::invalid_argument("Number of points for Savitzky-Golay Filter "
        "has to be uneven.");
  }
  if (n_points < derivatives) {
    throw std::invalid_argument("Number of points has to be bigger than "
        "number of derivatives in Savitzky-Golay Filter.");
  }
  Eigen::MatrixXd coefficients = CalculateConvolutionCoefficients(n_points,
      derivatives);
  std::vector<std::vector<double>> a(derivatives);
  for (size_t point = n_points/2; point < state.size() - n_points/2; ++point) {
    for (size_t deriv = 0; deriv < derivatives; ++deriv) {
      double sum = 0.;
      for (int i = -static_cast<int>(n_points)/2;
           i <= static_cast<int>(n_points)/2; ++i) {
        sum += coefficients(deriv, i + n_points/2)*state[point + i];
      }
      sum *= factorial(deriv)/pow(stepsize, deriv);
      a[deriv].push_back(sum);
    }
  }
  return a;
}

}  // namespace sam

#endif  // INCLUDE_SAM_ANALYSIS_SAVITZKY_GOLAY_HPP_