///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 Nikhar Agrawal
//  Copyright 2013 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2013 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef _BOOST_POLYGAMMA_DETAIL_2013_07_30_HPP_
  #define _BOOST_POLYGAMMA_DETAIL_2013_07_30_HPP_

#include <cmath>
  #include <limits>
  #include <boost/cstdint.hpp>
  #include <boost/math/policies/policy.hpp>
  #include <boost/math/special_functions/bernoulli.hpp>
  #include <boost/math/special_functions/trunc.hpp>
  #include <boost/math/special_functions/zeta.hpp>
  #include <boost/math/special_functions/digamma.hpp>
  #include <boost/math/special_functions/sin_pi.hpp>
  #include <boost/math/special_functions/cos_pi.hpp>
  #include <boost/math/special_functions/pow.hpp>
  #include <boost/mpl/if.hpp>
  #include <boost/mpl/int.hpp>
  #include <boost/static_assert.hpp>
  #include <boost/type_traits/is_convertible.hpp>

#ifdef _MSC_VER
#pragma once
#pragma warning(push)
#pragma warning(disable:4702) // Unreachable code (release mode only warning)
#endif

namespace boost { namespace math { namespace detail{

  template<class T, class Policy>
  T polygamma_atinfinityplus(const int n, const T& x, const Policy& pol, const char* function) // for large values of x such as for x> 400
  {
     // See http://functions.wolfram.com/GammaBetaErf/PolyGamma2/06/02/0001/
     BOOST_MATH_STD_USING
     //
     // sum       == current value of accumulated sum.
     // term      == value of current term to be added to sum.
     // part_term == value of current term excluding the Bernoulli number part
     //
     if(n + x == x)
     {
        // x is crazy large, just concentrate on the first part of the expression and use logs:
        if(n == 1) return 1 / x;
        T nlx = n * log(x);
        if((nlx < tools::log_max_value<T>()) && (n < (int)max_factorial<T>::value))
           return ((n & 1) ? 1 : -1) * boost::math::factorial<T>(n - 1) * pow(x, -n);
        else
         return ((n & 1) ? 1 : -1) * exp(boost::math::lgamma(T(n), pol) - n * log(x));
     }
     T term, sum, part_term;
     T x_squared = x * x;
     //
     // Start by setting part_term to:
     //
     // (n-1)! / x^(n+1)
     //
     // which is common to both the first term of the series (with k = 1)
     // and to the leading part.
     // We can then get to the leading term by:
     //
     // part_term * (n + 2 * x) / 2
     //
     // and to the first term in the series
     // (excluding the Bernoulli number) by:
     //
     // part_term n * (n + 1) / (2x)
     //
     // If either the factorial would overflow,
     // or the power term underflows, this just gets set to 0 and then we
     // know that we have to use logs for the initial terms:
     //
     part_term = ((n > (int)boost::math::max_factorial<T>::value) && (T(n) * n > tools::log_max_value<T>()))
        ? T(0) : static_cast<T>(boost::math::factorial<T>(n - 1, pol) * pow(x, -n - 1));
     if(part_term == 0)
     {
        // Either n is very large, or the power term underflows,
        // set the initial values of part_term, term and sum via logs:
        part_term = static_cast<T>(boost::math::lgamma(n, pol) - (n + 1) * log(x));
        sum = exp(part_term + log(n + 2 * x) - boost::math::constants::ln_two<T>());
        part_term += log(T(n) * (n + 1)) - boost::math::constants::ln_two<T>() - log(x);
        part_term = exp(part_term);
     }
     else
     {
        sum = part_term * (n + 2 * x) / 2;
        part_term *= (T(n) * (n + 1)) / 2;
        part_term /= x;
     }
     //
     // If the leading term is 0, so is the result:
     //
     if(sum == 0)
        return sum;

     for(unsigned k = 1;;)
     {
        term = part_term * boost::math::bernoulli_b2n<T>(k, pol);
        sum += term;
        //
        // Normal termination condition:
        //
        if(fabs(term / sum) < tools::epsilon<T>())
           break;
        //
        // Increment our counter, and move part_term on to the next value:
        //
        ++k;
        part_term *= T(n + 2 * k - 2) * (n - 1 + 2 * k);
        part_term /= (2 * k - 1) * 2 * k;
        part_term /= x_squared;
        //
        // Emergency get out termination condition:
        //
        if(k > policies::get_max_series_iterations<Policy>())
        {
           return policies::raise_evaluation_error(function, "Series did not converge, closest value was %1%", sum, pol);
        }
     }

     if((n - 1) & 1)
        sum = -sum;

     return sum;
  }

  template<class T, class Policy>
  T polygamma_attransitionplus(const int n, const T& x, const Policy& pol, const char* function)
  {
    // See: http://functions.wolfram.com/GammaBetaErf/PolyGamma2/16/01/01/0017/

    // Use N = (0.4 * digits) + (4 * n) for target value for x:
    BOOST_MATH_STD_USING
    const int d4d  = static_cast<int>(0.4F * policies::digits_base10<T, Policy>());
    const int N = d4d + (4 * n);
    const int m    = n;
    const int iter = N - itrunc(x);

    if(iter > (int)policies::get_max_series_iterations<Policy>())
       return policies::raise_evaluation_error<T>(function, ("Exceeded maximum series evaluations evaluating at n = " + boost::lexical_cast<std::string>(n) + " and x = %1%").c_str(), x, pol);

    const int minus_m_minus_one = -m - 1;

    T z(x);
    T sum0(0);
    T z_plus_k_pow_minus_m_minus_one(0);

    // Forward recursion to larger x, need to check for overflow first though:
    if(log(z + iter) * minus_m_minus_one > -tools::log_max_value<T>())
    {
       for(int k = 1; k <= iter; ++k)
       {
          z_plus_k_pow_minus_m_minus_one = pow(z, minus_m_minus_one);
          sum0 += z_plus_k_pow_minus_m_minus_one;
          z += 1;
       }
       sum0 *= boost::math::factorial<T>(n);
    }
    else
    {
       for(int k = 1; k <= iter; ++k)
       {
          T log_term = log(z) * minus_m_minus_one + boost::math::lgamma(T(n + 1), pol);
          sum0 += exp(log_term);
          z += 1;
       }
    }
    if((n - 1) & 1)
       sum0 = -sum0;

    return sum0 + polygamma_atinfinityplus(n, z, pol, function);
  }

  template <class T, class Policy>
  T polygamma_nearzero(int n, T x, const Policy& pol, const char* function)
  {
     BOOST_MATH_STD_USING
     //
     // If we take this expansion for polygamma: http://functions.wolfram.com/06.15.06.0003.02
     // and substitute in this expression for polygamma(n, 1): http://functions.wolfram.com/06.15.03.0009.01
     // we get an alternating series for polygamma when x is small in terms of zeta functions of
     // integer arguments (which are easy to evaluate, at least when the integer is even).
     //
     // In order to avoid spurious overflow, save the n! term for later, and rescale at the end:
     //
     T scale = boost::math::factorial<T>(n, pol);
     //
     // "factorial_part" contains everything except the zeta function
     // evaluations in each term:
     //
     T factorial_part = 1;
     //
     // "prefix" is what we'll be adding the accumulated sum to, it will
     // be n! / z^(n+1), but since we're scaling by n! it's just
     // 1 / z^(n+1) for now:
     //
     T prefix = pow(x, n + 1);
     if(prefix == 0)
        return boost::math::policies::raise_overflow_error<T>(function, 0, pol);
     prefix = 1 / prefix;
     //
     // First term in the series is necessarily < zeta(2) < 2, so
     // ignore the sum if it will have no effect on the result anyway:
     //
     if(prefix > 2 / policies::get_epsilon<T, Policy>())
        return ((n & 1) ? 1 : -1) *
         (tools::max_value<T>() / prefix < scale ? policies::raise_overflow_error<T>(function, 0, pol) : prefix * scale);
     //
     // As this is an alternating series we could accelerate it using
     // "Convergence Acceleration of Alternating Series",
     // Henri Cohen, Fernando Rodriguez Villegas, and Don Zagier, Experimental Mathematics, 1999.
     // In practice however, it appears not to make any difference to the number of terms
     // required except in some edge cases which are filtered out anyway before we get here.
     //
     T sum = prefix;
     for(unsigned k = 0;;)
     {
        // Get the k'th term:
        T term = factorial_part * boost::math::zeta(T(k + n + 1), pol);
        sum += term;
        // Termination condition:
        if(fabs(term) < fabs(sum * boost::math::policies::get_epsilon<T, Policy>()))
           break;
        //
        // Move on k and factorial_part:
        //
        ++k;
        factorial_part *= (-x * (n + k)) / k;
        //
        // Last chance exit:
        //
        if(k > policies::get_max_series_iterations<Policy>())
           return policies::raise_evaluation_error<T>(function, "Series did not converge, best value is %1%", sum, pol);
     }
     //
     // We need to multiply by the scale, at each stage checking for oveflow:
     //
     if(boost::math::tools::max_value<T>() / scale < sum)
        return boost::math::policies::raise_overflow_error<T>(function, 0, pol);
     sum *= scale;
     return n & 1 ? sum : T(-sum);
  }

  //
  // Helper function which figures out which slot our coefficient is in
  // given an angle multiplier for the cosine term of power:
  //
  template <class Table>
  typename Table::value_type::reference dereference_table(Table& table, unsigned row, unsigned power)
  {
     return table[row][power / 2];
  }

  template <class T, class Policy>
  T poly_cot_pi(int n, T x, T xc, const Policy& pol, const char* function)
  {
     BOOST_MATH_STD_USING
     // Return n'th derivative of cot(pi*x) at x, these are simply
     // tabulated for up to n = 9, beyond that it is possible to
     // calculate coefficients as follows:
     //
     // The general form of each derivative is:
     //
     // pi^n * SUM{k=0, n} C[k,n] * cos^k(pi * x) * csc^(n+1)(pi * x)
     //
     // With constant C[0,1] = -1 and all other C[k,n] = 0;
     // Then for each k < n+1:
     // C[k-1, n+1]  -= k * C[k, n];
     // C[k+1, n+1]  += (k-n-1) * C[k, n];
     //
     // Note that there are many different ways of representing this derivative thanks to
     // the many trigomonetric identies available.  In particular, the sum of powers of
     // cosines could be replaced by a sum of cosine multiple angles, and indeed if you
     // plug the derivative into Mathematica this is the form it will give.  The two
     // forms are related via the Chebeshev polynomials of the first kind and
     // T_n(cos(x)) = cos(n x).  The polynomial form has the great advantage that
     // all the cosine terms are zero at half integer arguments - right where this
     // function has it's minumum - thus avoiding cancellation error in this region.
     //
     // And finally, since every other term in the polynomials is zero, we can save
     // space by only storing the non-zero terms.  This greatly complexifies
     // subscripting the tables in the calculation, but halves the storage space
     // (and complexity for that matter).
     //
     T s = fabs(x) < fabs(xc) ? boost::math::sin_pi(x, pol) : boost::math::sin_pi(xc, pol);
     T c = boost::math::cos_pi(x, pol);
     switch(n)
     {
     case 1:
        return -constants::pi<T, Policy>() / (s * s);
     case 2:
     {
        return 2 * constants::pi<T, Policy>() * constants::pi<T, Policy>() * c / boost::math::pow<3>(s, pol);
     }
     case 3:
     {
        int P[] = { -2, -4 };
        return boost::math::pow<3>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<4>(s, pol);
     }
     case 4:
     {
        int P[] = { 16, 8 };
        return boost::math::pow<4>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<5>(s, pol);
     }
     case 5:
     {
        int P[] = { -16, -88, -16 };
        return boost::math::pow<5>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<6>(s, pol);
     }
     case 6:
     {
        int P[] = { 272, 416, 32 };
        return boost::math::pow<6>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<7>(s, pol);
     }
     case 7:
     {
        int P[] = { -272, -2880, -1824, -64 };
        return boost::math::pow<7>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<8>(s, pol);
     }
     case 8:
     {
        int P[] = { 7936, 24576, 7680, 128 };
        return boost::math::pow<8>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<9>(s, pol);
     }
     case 9:
     {
        int P[] = { -7936, -137216, -185856, -31616, -256 };
        return boost::math::pow<9>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<10>(s, pol);
     }
     case 10:
     {
        int P[] = { 353792, 1841152, 1304832, 128512, 512 };
        return boost::math::pow<10>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<11>(s, pol);
     }
     case 11:
     {
        int P[] = { -353792, -9061376, -21253376, -8728576, -518656, -1024};
        return boost::math::pow<11>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<12>(s, pol);
     }
     case 12:
     {
        int P[] = { 22368256, 175627264, 222398464, 56520704, 2084864, 2048 };
        return boost::math::pow<12>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<13>(s, pol);
     }
#ifndef BOOST_NO_LONG_LONG
     case 13:
     {
        long long P[] = { -22368256LL, -795300864LL, -2868264960LL, -2174832640LL, -357888000LL, -8361984LL, -4096 };
        return boost::math::pow<13>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<14>(s, pol);
     }
     case 14:
     {
        long long P[] = { 1903757312LL, 21016670208LL, 41731645440LL, 20261765120LL, 2230947840LL, 33497088LL, 8192 };
        return boost::math::pow<14>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<15>(s, pol);
     }
     case 15:
     {
        long long P[] = { -1903757312LL, -89702612992LL, -460858269696LL, -559148810240LL, -182172651520LL, -13754155008LL, -134094848LL, -16384 };
        return boost::math::pow<15>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<16>(s, pol);
     }
     case 16:
     {
        long long P[] = { 209865342976LL, 3099269660672LL, 8885192097792LL, 7048869314560LL, 1594922762240LL, 84134068224LL, 536608768LL, 32768 };
        return boost::math::pow<16>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<17>(s, pol);
     }
     case 17:
     {
        long long P[] = { -209865342976LL, -12655654469632LL, -87815735738368LL, -155964390375424LL, -84842998005760LL, -13684856848384LL, -511780323328LL, -2146926592LL, -65536 };
        return boost::math::pow<17>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<18>(s, pol);
     }
     case 18:
     {
        long long P[] = { 29088885112832LL, 553753414467584LL, 2165206642589696LL, 2550316668551168LL, 985278548541440LL, 115620218667008LL, 3100738912256LL, 8588754944LL, 131072 };
        return boost::math::pow<18>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<19>(s, pol);
     }
     case 19:
     {
        long long P[] = { -29088885112832LL, -2184860175433728LL, -19686087844429824LL, -48165109676113920LL, -39471306959486976LL, -11124607890751488LL, -965271355195392LL, -18733264797696LL, -34357248000LL, -262144 };
        return boost::math::pow<19>(constants::pi<T, Policy>(), pol) * tools::evaluate_even_polynomial(P, c) / boost::math::pow<20>(s, pol);
     }
     case 20:
     {
        long long P[] = { 4951498053124096LL, 118071834535526400LL, 603968063567560704LL, 990081991141490688LL, 584901762421358592LL, 122829335169859584LL, 7984436548730880LL, 112949304754176LL, 137433710592LL, 524288 };
        return boost::math::pow<20>(constants::pi<T, Policy>(), pol) * c * tools::evaluate_even_polynomial(P, c) / boost::math::pow<21>(s, pol);
     }
#endif
     }

     //
     // We'll have to compute the coefficients up to n,
     // complexity is O(n^2) which we don't worry about for now
     // as the values are computed once and then cached.
     // However, if the final evaluation would have too many
     // terms just bail out right away:
     //
     if((unsigned)n / 2u > policies::get_max_series_iterations<Policy>())
        return policies::raise_evaluation_error<T>(function, "The value of n is so large that we're unable to compute the result in reasonable time, best guess is %1%", 0, pol);
#ifdef BOOST_HAS_THREADS
     static boost::detail::lightweight_mutex m;
     boost::detail::lightweight_mutex::scoped_lock l(m);
#endif
     static int digits = tools::digits<T>();
     static std::vector<std::vector<T> > table(1, std::vector<T>(1, T(-1)));

     int current_digits = tools::digits<T>();

     if(digits != current_digits)
     {
        // Oh my... our precision has changed!
        table = std::vector<std::vector<T> >(1, std::vector<T>(1, T(-1)));
        digits = current_digits;
     }

     int index = n - 1;

     if(index >= (int)table.size())
     {
        for(int i = (int)table.size() - 1; i < index; ++i)
        {
           int offset = i & 1; // 1 if the first cos power is 0, otherwise 0.
           int sin_order = i + 2;  // order of the sin term
           int max_cos_order = sin_order - 1;  // largest order of the polynomial of cos terms
           int max_columns = (max_cos_order - offset) / 2;  // How many entries there are in the current row.
           int next_offset = offset ? 0 : 1;
           int next_max_columns = (max_cos_order + 1 - next_offset) / 2;  // How many entries there will be in the next row
           table.push_back(std::vector<T>(next_max_columns + 1, T(0)));

           for(int column = 0; column <= max_columns; ++column)
           {
              int cos_order = 2 * column + offset;  // order of the cosine term in entry "column"
              BOOST_ASSERT(column < (int)table[i].size());
              BOOST_ASSERT((cos_order + 1) / 2 < (int)table[i + 1].size());
              table[i + 1][(cos_order + 1) / 2] += ((cos_order - sin_order) * table[i][column]) / (sin_order - 1);
              if(cos_order)
                table[i + 1][(cos_order - 1) / 2] += (-cos_order * table[i][column]) / (sin_order - 1);
           }
        }

     }
     T sum = boost::math::tools::evaluate_even_polynomial(&table[index][0], c, table[index].size());
     if(index & 1)
        sum *= c;  // First coeffient is order 1, and really an odd polynomial.
     if(sum == 0)
        return sum;
     //
     // The remaining terms are computed using logs since the powers and factorials
     // get real large real quick:
     //
     T power_terms = n * log(boost::math::constants::pi<T>());
     if(s == 0)
        return sum * boost::math::policies::raise_overflow_error<T>(function, 0, pol);
     power_terms -= log(fabs(s)) * (n + 1);
     power_terms += boost::math::lgamma(T(n));
     power_terms += log(fabs(sum));

     if(power_terms > boost::math::tools::log_max_value<T>())
        return sum * boost::math::policies::raise_overflow_error<T>(function, 0, pol);

     return exp(power_terms) * ((s < 0) && ((n + 1) & 1) ? -1 : 1) * boost::math::sign(sum);
  }

  template <class T, class Policy>
  struct polygamma_initializer
  {
     struct init
     {
        init()
        {
           // Forces initialization of our table of coefficients and mutex:
           boost::math::polygamma(30, T(-2.5f), Policy());
        }
        void force_instantiate()const{}
     };
     static const init initializer;
     static void force_instantiate()
     {
        initializer.force_instantiate();
     }
  };

  template <class T, class Policy>
  const typename polygamma_initializer<T, Policy>::init polygamma_initializer<T, Policy>::initializer;

  template<class T, class Policy>
  inline T polygamma_imp(const int n, T x, const Policy &pol)
  {
    BOOST_MATH_STD_USING
    static const char* function = "boost::math::polygamma<%1%>(int, %1%)";
    polygamma_initializer<T, Policy>::initializer.force_instantiate();
    if(n < 0)
       return policies::raise_domain_error<T>(function, "Order must be >= 0, but got %1%", static_cast<T>(n), pol);
    if(x < 0)
    {
       if(floor(x) == x)
       {
          //
          // Result is infinity if x is odd, and a pole error if x is even.
          //
          if(lltrunc(x) & 1)
             return policies::raise_overflow_error<T>(function, 0, pol);
          else
             return policies::raise_pole_error<T>(function, "Evaluation at negative integer %1%", x, pol);
       }
       T z = 1 - x;
       T result = polygamma_imp(n, z, pol) + constants::pi<T, Policy>() * poly_cot_pi(n, z, x, pol, function);
       return n & 1 ? T(-result) : result;
    }
    //
    // Limit for use of small-x-series is chosen
    // so that the series doesn't go too divergent
    // in the first few terms.  Ordinarily this
    // would mean setting the limit to ~ 1 / n,
    // but we can tolerate a small amount of divergence:
    //
    T small_x_limit = (std::min)(T(T(5) / n), T(0.25f));
    if(x < small_x_limit)
    {
      return polygamma_nearzero(n, x, pol, function);
    }
    else if(x > 0.4F * policies::digits_base10<T, Policy>() + 4.0f * n)
    {
      return polygamma_atinfinityplus(n, x, pol, function);
    }
    else if(x == 1)
    {
       return (n & 1 ? 1 : -1) * boost::math::factorial<T>(n, pol) * boost::math::zeta(T(n + 1), pol);
    }
    else if(x == 0.5f)
    {
       T result = (n & 1 ? 1 : -1) * boost::math::factorial<T>(n, pol) * boost::math::zeta(T(n + 1), pol);
       if(fabs(result) >= ldexp(tools::max_value<T>(), -n - 1))
          return boost::math::sign(result) * policies::raise_overflow_error<T>(function, 0, pol);
       result *= ldexp(T(1), n + 1) - 1;
       return result;
    }
    else
    {
      return polygamma_attransitionplus(n, x, pol, function);
    }
  }

} } } // namespace boost::math::detail

#ifdef _MSC_VER
#pragma warning(pop)
#endif

#endif // _BOOST_POLYGAMMA_DETAIL_2013_07_30_HPP_


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

/*
    Copyright (c) 2020, Klaus Spanderen
    All rights reserved.

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

    1. Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

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

    3. Neither the names of the copyright holders nor the names of the QuantLib
    Group and 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.
*/

/*! \file mphestonengine.hpp
    \brief multi precision Heston vanilla engine
*/

#ifndef quantlib_mp_heston_vanilla_engine_hpp
#define quantlib_mp_heston_vanilla_engine_hpp

#include <ql/pricingengines/genericmodelengine.hpp>
#include <ql/models/equity/hestonmodel.hpp>
#include <ql/instruments/vanillaoption.hpp>

#include <boost/multiprecision/cpp_dec_float.hpp>

namespace QuantLib {

    class MPHestonVanillaEngine
        : public GenericModelEngine<HestonModel,
                                    VanillaOption::arguments,
                                    VanillaOption::results> {
      public:
        typedef boost::multiprecision::number<
            boost::multiprecision::cpp_dec_float<150> > MP_Real;

        MPHestonVanillaEngine(const ext::shared_ptr<HestonModel>& model,Size n);

        void calculate() const;

      private:
        const Size n_;
        const ext::shared_ptr<HestonModel> model_;

        std::vector<MP_Real> x_, w_;
    };
}

#endif


/**
 * @file dense_chained_multiplication.cpp
 * @brief Benchmarks the chained multiplication of 6 random square matrices of increasing size
 *
 * @author Matthew Powelson
 * @date April 1, 2020
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2020, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * 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
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <benchmark/benchmark.h>

#include <arrayfire.h>
#include <Eigen/Eigen>
#include <torch/torch.h>

auto BM_PYTORCH_CM = [](benchmark::State& state, int size, torch::Device device) {
  torch::TensorOptions options =
      torch::TensorOptions().dtype(torch::kFloat64).layout(torch::kStrided).device(device).requires_grad(true);

  torch::manual_seed(0);
  torch::Tensor tensor0 = torch::rand({ size, size }, options);
  torch::manual_seed(1);
  torch::Tensor tensor1 = torch::rand({ size, size }, options);
  torch::manual_seed(2);
  torch::Tensor tensor2 = torch::rand({ size, size }, options);
  torch::manual_seed(3);
  torch::Tensor tensor3 = torch::rand({ size, size }, options);
  torch::manual_seed(4);
  torch::Tensor tensor4 = torch::rand({ size, size }, options);
  torch::manual_seed(5);
  torch::Tensor tensor5 = torch::rand({ size, size }, options);

  torch::Tensor result = torch::rand({ size, size }, options);

  for (auto _ : state)
  {
    benchmark::DoNotOptimize(
        result = torch::mm(torch::mm(torch::mm(torch::mm(torch::mm(tensor0, tensor1), tensor2), tensor3), tensor4),
                           tensor5));
  }
};

auto BM_EIGEN_CM = [](benchmark::State& state, int size) {
  srand(0);
  Eigen::MatrixXd matrix1 = Eigen::MatrixXd::Random(size, size);
  srand(1);
  Eigen::MatrixXd matrix2 = Eigen::MatrixXd::Random(size, size);
  srand(2);
  Eigen::MatrixXd matrix3 = Eigen::MatrixXd::Random(size, size);
  srand(3);
  Eigen::MatrixXd matrix4 = Eigen::MatrixXd::Random(size, size);
  srand(4);
  Eigen::MatrixXd matrix5 = Eigen::MatrixXd::Random(size, size);
  srand(5);
  Eigen::MatrixXd matrix6 = Eigen::MatrixXd::Random(size, size);

  Eigen::MatrixXd result = Eigen::MatrixXd::Random(size, size);
  for (auto _ : state)
  {
    benchmark::DoNotOptimize(result = matrix1 * matrix2 * matrix3 * matrix4 * matrix5 * matrix6);
  }
};

auto BM_ARRAYFIRE_CM = [](benchmark::State& state, int size, auto device) {
  af::setBackend(device);

  af::setSeed(0);
  af::array array0 = af::randu(size, size);
  af::setSeed(1);
  af::array array1 = af::randu(size, size);
  af::setSeed(2);
  af::array array2 = af::randu(size, size);
  af::setSeed(3);
  af::array array3 = af::randu(size, size);
  af::setSeed(4);
  af::array array4 = af::randu(size, size);
  af::setSeed(5);
  af::array array5 = af::randu(size, size);

  af::array result = af::randu(size, size);
  for (auto _ : state)
  {
    benchmark::DoNotOptimize(result = af::matmul(af::matmul(array0, array1, array2, array3), array4, array5));
  }
};

int main(int argc, char** argv)
{
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_PYTORCH_CPU_CM_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_PYTORCH_CM, test_input, torch::kCPU)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_PYTORCH_GPU_CM_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_PYTORCH_CM, test_input, torch::kCUDA)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_EIGEN_CM_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_EIGEN_CM, test_input)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_ARRAYFIRE_CPU_CM_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_ARRAYFIRE_CM, test_input, AF_BACKEND_CPU)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_ARRAYFIRE_CUDA_CM_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_ARRAYFIRE_CM, test_input, AF_BACKEND_CUDA)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  for (auto& test_input : { 2, 3, 4, 8, 16, 32, 64, 128, 256, 512, 1028, 2056})
  {
    std::string name = "BM_ARRAYFIRE_OPENCL_CM_Size_" + std::to_string(test_input);
    benchmark::RegisterBenchmark(name.c_str(), BM_ARRAYFIRE_CM, test_input, AF_BACKEND_OPENCL)
        ->UseRealTime()
        ->Unit(benchmark::TimeUnit::kMicrosecond);
  }
  benchmark::Initialize(&argc, argv);
  benchmark::RunSpecifiedBenchmarks();
}


#pragma once
#include <Eigen/Core>
#include <iostream>

namespace srrg2_core {
  //! @brief ldg implements similiarity class eigen illnesses style
  template <typename Scalar_, int Dim_>
  class Similiarity_ {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    static constexpr int Dim       = Dim_;
    static constexpr int MatrixDim = Dim + 1;
    using Index                    = Eigen::Index;
    using Scalar                   = Scalar_;
    using ThisType                 = Similiarity_<Scalar, Dim>;
    using MatrixType               = Eigen::Matrix<Scalar, MatrixDim, MatrixDim>;
    using VectorType               = Eigen::Matrix<Scalar, Dim, 1>;
    using ConstMatrixType          = const MatrixType;
    using LinearType               = Eigen::Matrix<Scalar, Dim, Dim>;
    using ConstLinearType          = const LinearType;
    using TranslationType          = Eigen::Matrix<Scalar, Dim, 1>;
    using ConstTranslationType     = const TranslationType;
    // ldg blocks in super fancy eigen way
    using LinearBlock           = Eigen::Block<MatrixType, Dim, Dim>;
    using ConstLinearBlock      = const Eigen::Block<ConstMatrixType, Dim, Dim>;
    using TranslationBlock      = Eigen::Block<MatrixType, Dim, 1>;
    using ConstTranslationBlock = const Eigen::Block<ConstMatrixType, Dim, 1>;

    //! @brief ldg constructing similiarity to identity
    Similiarity_() {
      _matrix.setIdentity();
    }

    //! @brief ldg copy construction, allows direct initialization
    Similiarity_(const Similiarity_& other) {
      _matrix.setIdentity();
      this->linear()         = other.linear();
      this->translation()    = other.translation();
      this->inverseScaling() = other.inverseScaling();
    }

    static ThisType Identity() {
      return ThisType();
    }

    //! @brief ldg accessor matrix type
    ConstMatrixType& matrix() const {
      return _matrix;
    }

    // ! @brief ldg setter matrix type, eigen style
    MatrixType& matrix() {
      return _matrix;
    }

    //! @brief ldg accessor to rotational part
    ConstLinearBlock linear() const {
      return ConstLinearBlock(_matrix, 0, 0);
    }

    //! @brief ldg setter to rotational part
    LinearBlock linear() {
      return LinearBlock(_matrix, 0, 0);
    }

    //! @brief ldg accessor to rotational part
    ConstLinearBlock rotation() const {
      return linear();
    }

    //! @brief ldg setter to rotational part
    LinearBlock rotation() {
      return linear();
    }

    //! @brief ldg accessor to translational part
    TranslationBlock translation() {
      return TranslationBlock(_matrix, 0, Dim);
    }

    //! @brief ldg setter to translational part
    ConstTranslationBlock translation() const {
      return ConstTranslationBlock(_matrix, 0, Dim);
    }

    //! @brief ldg accessor to inverse scaling value, scalar
    const Scalar& inverseScaling() const {
      return _matrix.coeffRef(Dim, Dim);
    }

    //! @brief ldg setter to inverse scaling value, scalar
    Scalar& inverseScaling() {
      return _matrix.coeffRef(Dim, Dim);
    }

    //! @brief ldg set matrix to indentity
    void setIdentity() {
      _matrix.setIdentity();
    }

    //! @brief ldg returns a inverse of a similiarity
    ThisType inverse() const {
      ThisType inverse_sim;
      inverse_sim.setIdentity();
      inverse_sim.linear() = this->linear().transpose();
      inverse_sim.translation() =
        -Scalar(1) / this->inverseScaling() * (inverse_sim.linear() * this->translation());
      inverse_sim.inverseScaling() = Scalar(1) / this->inverseScaling();
      return inverse_sim;
    }

    //! @brief ldg multiply operator between similiarities
    ThisType operator*(const ThisType& other) const {
      ThisType result;
      result.setIdentity();
      ConstLinearType& R      = other.linear();
      ConstTranslationType& t = other.translation();
      const Scalar& is        = other.inverseScaling();
      result.linear()         = this->linear() * R;
      result.translation()    = this->linear() * t + is * this->translation();
      result.inverseScaling() = is * this->inverseScaling();
      return result;
    }

    //! @brief ldg multiply operator similiarity * vector
    VectorType operator*(const VectorType& other) const {
      VectorType result;
      result = Scalar(1) / this->inverseScaling() * (this->linear() * other + this->translation());
      return result;
    }

    //! @brief ldg equal operator, copy similiarities
    ThisType& operator=(const ThisType& other) {
      this->linear()         = other.linear();
      this->translation()    = other.translation();
      this->inverseScaling() = other.inverseScaling();
      return *this;
    }

    //! @brief ldg multiply operator between similiarities, write result on current sim
    ThisType& operator*=(const ThisType& other) {
      *this = *this * other;
      return *this;
    }

    Scalar& operator()(Index row, Index col) {
      return _matrix(row, col);
    }

    Scalar operator()(Index row, Index col) const {
      return _matrix(row, col);
    }

    Index rows() const {
      return _matrix.rows();
    }

    Index cols() const {
      return _matrix.cols();
    }

  protected:
    MatrixType _matrix;
  };

} // namespace srrg2_core


#include <string>
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <cassert>
#include <vector>
#include <map>
#include <algorithm>

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

#include <rstbx/indexing_api/indexing_api.h>
#include <rstbx/dps_core/direction.h>
#include <cctbx/math/mod.h>

namespace af = scitbx::af;
namespace constants = scitbx::constants;
using rstbx::Direction;
using rstbx::Directional_FFT;
using rstbx::kvalcmp;

af::shared<int>
rstbx::indexing_api::cpp_absence_test(af::shared<cctbx::miller::index<> > hkl,
                     const int& mod, cctbx::miller::index<> vecrep){
  af::shared<int> cum;
  for (std::size_t i=0; i<mod; ++i) {cum.push_back(0);}

  typedef af::shared<cctbx::miller::index<> >::const_iterator For;
  For b = hkl.begin();
  For e = hkl.end();
  for (;b!=e;++b){
    int pattern_sum = (*b)[0]*vecrep[0] + (*b)[1]*vecrep[1] + (*b)[2]*vecrep[2];
    cum[cctbx::math::mod_positive(pattern_sum, mod)]+=1;
  }

  return cum;
}

rstbx::indexing_api::dps_extended::dps_extended():rstbx::dps_core(){}

void
rstbx::indexing_api::dps_extended::setData(const pointlist& raw){
  rawdata=pointlistmm();
  rawdata.reserve(raw.size());
  for (std::size_t i = 0; i< raw.size(); ++i) {
      rawdata.push_back(raw[i]);
  }
}

rstbx::Direction
rstbx::indexing_api::dps_extended::refine_direction(const rstbx::Direction& candidate,
                               const double& current_grid,
                               const double& target_grid) const {
  double incr = current_grid/4.0; //recursively smaller grid with neighbor overlap
  if (incr < target_grid) { return candidate; }
  rstbx::Direction direction0(candidate.dvec);
  rstbx::Direction direction1(candidate.psi + incr, candidate.phi);
  scitbx::vec3<double> rotaxisi = direction0.dvec.cross(direction1.dvec).normalize();
  scitbx::vec3<double> rotaxisj = rotaxisi.cross(direction1.dvec).normalize();

  rstbx::Direction best_refined = candidate;
  for (int i = -2; i < 3; ++i) {
    scitbx::vec3<double> rotate1 = direction0.dvec.unit_rotate_around_origin(rotaxisi,-i*incr);
    for (int j = -2; j < 3; ++j) {
      scitbx::vec3<double> rotate2 = rotate1.unit_rotate_around_origin(rotaxisj,-j*incr);
      SCITBX_ASSERT(std::abs(1.0 - rotate2.length())<0.00001); //rotate2 is unit vector
      rstbx::Direction new_candidate(rotate2);
      rstbx::fftptr dfft( fft_factory(new_candidate) );

      if ( dfft->kval() > best_refined.kval ) {
        new_candidate.extract_directional_properties(dfft);
        best_refined=new_candidate;
      }
    }
  }
  return refine_direction(best_refined,incr,target_grid);
}

double
rstbx::indexing_api::dps_extended::high()const{
  //Go through all film coordinates and get high resolution limit
  // This is NOT a safe function if rawdata.size()==0 due to
  // dereferencing s.end().  This bug causes a painful infinite loop
  // in some runtime environments.  See the protecting test in classify_spots().
  // The real fix will be to split this
  // class into components, with the high() function being a member
  // of the class containing rawdata.

  SCITBX_ASSERT (xyzdata.size() > 0);
  af::shared<double> s(xyzdata.size());
  for (std::size_t i = 0; i<xyzdata.size(); ++i) {
    s[i] =         1./xyzdata[i].length();
  }
  double dmax = *(std::max_element<af::shared<double>::const_iterator>(s.begin(),s.end()));
  //dmax is maximum squared distance (mm^2) from the beam center
  return dmax;
}

af::shared< scitbx::vec3<double> >
rstbx::indexing_api::raw_spot_positions_mm_to_reciprocal_space_xyz(
  pointlist raw_spot_input,
  dxtbx::model::Detector const& detector, double const& inverse_wave,
  scitbx::vec3<double> const& S0_vector, scitbx::vec3<double> const& axis, af::shared<int> panelID
){

  // it is not clear how we will plug in to this function if and when
  // we need to use a derived detector class with a different implemented
  // behavior of get_lab_coord().

  af::shared< scitbx::vec3<double> > reciprocal_space_vectors;
  // with a single panel only, all the convenience functions work for us
  for (int n = 0; n != raw_spot_input.size(); ++n) {
    int pid = panelID[n];

    // tile surface to laboratory transformation
    scitbx::vec2<double> raw_spot(raw_spot_input[n][0],raw_spot_input[n][1]);
    scitbx::vec3<double> lab_direct = detector[pid].get_lab_coord(raw_spot);

    // laboratory direct to reciprocal space xyz transformation
    scitbx::vec3<double> lab_recip = (lab_direct.normalize() * inverse_wave) - S0_vector;

      // raw_spot_input[n][2] MUST be given in degrees.
    reciprocal_space_vectors.push_back( lab_recip.rotate_around_origin(
      axis, -raw_spot_input[n][2] * scitbx::constants::pi_180) );
  }
  return reciprocal_space_vectors;

}

af::shared< scitbx::vec3<double> >
rstbx::indexing_api::raw_spot_positions_mm_to_reciprocal_space_xyz(
  pointlist raw_spot_input,
  dxtbx::model::Detector const& detector, double const& inverse_wave,
  scitbx::vec3<double> const& S0_vector, af::shared<int> panelID
){

  // it is not clear how we will plug in to this function if and when
  // we need to use a derived detector class with a different implemented
  // behavior of get_lab_coord().

  af::shared< scitbx::vec3<double> > reciprocal_space_vectors;
  // with a single panel only, all the convenience functions work for us
  for (int n = 0; n != raw_spot_input.size(); ++n) {
    int pid = panelID[n];

    // tile surface to laboratory transformation
    scitbx::vec2<double> raw_spot(raw_spot_input[n][0],raw_spot_input[n][1]);
    scitbx::vec3<double> lab_direct = detector[pid].get_lab_coord(raw_spot);

    // laboratory direct to reciprocal space xyz transformation
    scitbx::vec3<double> lab_recip = (lab_direct.normalize() * inverse_wave) - S0_vector;

      // raw_spot_input[n][2] MUST be given in degrees.
    reciprocal_space_vectors.push_back(lab_recip);
  }
  return reciprocal_space_vectors;

}


#include <stdint.h>
#include <iostream>
#include <math.h>
#include <tgmath.h>
#include <string.h>
#include <fstream>
#include <stdlib.h>
using namespace std;

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923
#endif

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"
#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"

//Un des deux ou aucun des deux
#define DRAWSTARS 1 //Si 1 etoiles sur le fond celeste, sinon rien
#define DRAWGRID 0  //Si 1 grille sur le fond celeste

//Un des deux ou aucun des deux
#define ADISK_NORMAL 1 //Si 1, texture + temperature, sinon rien
#define ADISK_GRID 0   //Si 1, grille, sinon rien

#include "fct.h"
#include "raytracing.h"
#include "par_for.h"

#define CHANNEL_NUM 3

#define MAXITER 500000

static const int Neq = 6; //Nombre de variables pour l'integration numerique

struct Rendu rdr;
struct Scene scn;
struct Blackhole bh;
struct Disk disk;

int adisk_width, adisk_height, adisk_bpp;
uint8_t *adisk = stbi_load("adisk_upscaled.png", &adisk_width, &adisk_height, &adisk_bpp, 3);

static const int SpectrumSampleSize = 76; //nombre d'échantillons
float deltaWaveLength = 4.;               //(En nm, espacement des échantillons)
float wavelengthSamples[SpectrumSampleSize], wavelengthSamples5[SpectrumSampleSize], sensitivitySamplesR[SpectrumSampleSize], sensitivitySamplesG[SpectrumSampleSize], sensitivitySamplesB[SpectrumSampleSize];

void readSensitivityData(const char *filename, float *wavelengthSamples, float *wavelengthSamples5, float *sensitivitySamplesR, float *sensitivitySamplesG, float *sensitivitySamplesB)
{
    int cnt = 0;
    ifstream source;       // build a read-Stream
    source.open(filename); // open data

    for (std::string line; std::getline(source, line);) //read stream line by line
    {
        std::istringstream in(line); //make a stream for the line itself

        in >> wavelengthSamples[cnt] >> sensitivitySamplesR[cnt] >> sensitivitySamplesG[cnt] >> sensitivitySamplesB[cnt]; //now read the whitespace-separated floats

        wavelengthSamples5[cnt] = pow(wavelengthSamples[cnt], 5);
        cnt++;
    }
}

void getBodyColor(float *rgbR, float *rgbG, float *rgbB, float temperature, float brightness)
{
    float I;

    for (int l = 0; l < SpectrumSampleSize; ++l)
    {

        I = (6.e14 / wavelengthSamples5[l]) / exp(1.43913e7 / (wavelengthSamples[l] * temperature) - 1.) * brightness; //6e14 pour renormaliser les composante des pixels

        I *= deltaWaveLength;
        *rgbR += sensitivitySamplesR[l] * I;
        *rgbG += sensitivitySamplesG[l] * I;
        *rgbB += sensitivitySamplesB[l] * I;
    }
}

//Affiche une grille sur le disque
void getDiskColorGrid(float phi, float r, float *rgbR, float *rgbG, float *rgbB)
{
    phi = (phi + M_PI) / (2. * M_PI);

    bool a = int((100 * phi)) % 2; //100 alternances de couleur par tour

    bool b = ((r - disk.R_min) / (disk.R_max - disk.R_min) > .5); //Séparer le disque en 2 radialement

    if (a ^ b) //Ou exclusif
    {
        *rgbR = 255.;
        *rgbG = 0.;
    }
    else
    {
        *rgbR = 0.;
        *rgbG = 255.;
    }
}

//Passage aux coordonnees de Boyer Lindquist
//Pour l'instant juste passage au coordonnées sphériques (plus simple)
//Utilisé uniquement pour trouver la pos initiale de la camera, pas important
void cartesianToBl(float x, float y, float z, float *r, float *theta, float *phi)
{

    float r2 = x * x + y * y + z * z;
    *r = sqrt(r2);

    *phi = atan2(y, x);
    *theta = acos(z / (*r));
}

//Passage des coordonnées de Boyer Lindquist au coordonnées cartésienne (exacte)
void blToCartesian(float r, float theta, float phi, float *x, float *y, float *z)
{

    float sintheta = sin(theta);
    float costheta = cos(theta);
    float cosphi = cos(phi);
    float sinphi = sin(phi);
    float temp = sintheta * sqrt(r * r + bh.a2);

    *x = temp * cosphi;
    *y = temp * sinphi;
    *z = r * costheta;
}

/* Fonction utilisé pour l'intégration */
void geodesic(float L, float kappa, float *y, float *dydx)
{
    float r, theta, pr, ptheta;

    r = y[0];
    theta = y[1];
    pr = y[4];
    ptheta = y[5];

    float r2 = r * r;
    float twor = 2.0 * r;

    float sintheta, costheta;
    sintheta = sin(theta);
    costheta = cos(theta);

    float costheta2 = costheta * costheta;
    float sintheta2 = sintheta * sintheta;

    float sigma = r2 + bh.a2 * costheta2;
    float delta = r2 - twor + bh.a2;
    float sd = sigma * delta;
    float siginv = 1.0 / sigma;
    float bot = 1.0 / sd;

    /* Prevent problems with the axis */
    if (sintheta < 1e-8)
    {
        sintheta = 1e-8;
        sintheta2 = 1e-16;
    }

    dydx[0] = -pr * delta * siginv;
    dydx[1] = -ptheta * siginv;
    dydx[2] = -(twor * bh.a + (sigma - twor) * L / sintheta2) * bot;
    dydx[3] = -(1.0 + (twor * (r2 + bh.a2) - twor * bh.a * L) * bot);
    dydx[4] = -(((r - 1.0) * (-kappa) + twor * (r2 + bh.a2) - 2.0 * bh.a * L) * bot - 2.0 * pr * pr * (r - 1.0) * siginv);
    dydx[5] = -sintheta * costheta * (L * L / (sintheta2 * sintheta2) - bh.a2) * siginv;
}

/* Conditions initiales pour un rayon */
void initial(float r0, float theta0, float *L, float *kappa, float *y0, float *ydot0, float x, float y)
{
    y0[0] = r0;
    y0[1] = theta0;
    y0[2] = 0;
    y0[3] = 0;
    y0[4] = cos(y) * cos(x);
    y0[5] = sin(y) / r0;

    float sintheta, costheta;
    sintheta = sin(theta0);
    costheta = cos(theta0);
    float costheta2 = costheta * costheta;
    float sintheta2 = sintheta * sintheta;

    float rdot0 = y0[4];
    float thetadot0 = y0[5];

    float r2 = r0 * r0;
    float sigma = r2 + bh.a2 * costheta2;
    float delta = r2 - 2.0 * r0 + bh.a2;
    float s1 = sigma - 2.0 * r0;

    y0[4] = rdot0 * sigma / delta;
    y0[5] = thetadot0 * sigma;

    ydot0[0] = rdot0;
    ydot0[1] = thetadot0;
    ydot0[2] = cos(y) * sin(x) / (r0 * sin(theta0));

    float phidot0 = ydot0[2];
    float energy2 = s1 * (rdot0 * rdot0 / delta + thetadot0 * thetadot0) + delta * sintheta2 * phidot0 * phidot0;

    float energy = sqrt(energy2);

    /* Energie de 1 */
    y0[4] = y0[4] / energy;
    y0[5] = y0[5] / energy;

    /* Angular Momentum with E = 1 */
    *L = ((sigma * delta * phidot0 - 2.0 * bh.a * r0 * energy) * sintheta2 / s1) / energy;

    *kappa = y0[5] * y0[5] + bh.a2 * sintheta2 + (*L) * (*L) / sintheta2;

    /* Hack - make sure everything is normalized correctly by a call to geodesic */

    geodesic(*L, *kappa, y0, ydot0);
}

//Obtenir la direction du rayon à partir des coord de BL (y) et de leurs dérivées (dydx)
//Retourne un vecteur normé
void getDirection(float *y, float *dydx, float *xp, float *yp, float *zp)
{

    float costheta = cos(y[1]);
    float sintheta = sin(y[1]);
    float cosphi = cos(y[2]);
    float sinphi = sin(y[2]);
    float r2 = y[0] * y[0];
    float R2 = r2 + bh.a2;
    float R = sqrt(r2 + bh.a2);

    *xp = R * dydx[1] * cosphi * costheta - R * dydx[2] * sinphi * sintheta + sintheta * cosphi * y[0] * dydx[0] / R;
    *yp = R * dydx[1] * sinphi * costheta + R * dydx[2] * cosphi * sintheta + sintheta * sinphi * y[0] * dydx[0] / R;
    *zp = -y[0] * sintheta * dydx[1] + dydx[0] * costheta;
    normalise(xp, yp, zp);
}

//Simulation complete d'un rayon (trajectoire+collision)
void sim(float r0, float theta0, float phi0, float xpixel, float ypixel, float *xp, float *yp, float *zp, float *pixel_transpr, float *pixel_transpg, float *pixel_transpb, float *canalAlpha, bool *ReachedInfinity, int *nbCollision)
{
    int N = 0; //Nombre d'itérations
    int k = 0; //nombre de collision

    float oldtheta; //pour detecter le passage à travers le plan z=0 (pour tracer le disque)

    bool zSignChange, diskDistance, diskCollision;

    //Euler

    //float y[Neq];
    //float dydx[Neq];

    //RK4
    float y[Neq];
    float ak[Neq];
    float dydx1[Neq], dydx2[Neq], dydx3[Neq], dydx4[Neq];
    float ytemp[Neq];

    float L, kappa;

    initial(r0, theta0, &L, &kappa, y, ak, xpixel, ypixel);

    float currentStep = rdr.step(y[0]);

    int l = 0; //Juste pour effectuer des boucles sans redeclarer de var

    while ((N < MAXITER) && (rdr.R_min < y[0]) && (y[0] < rdr.R_inf))
    {

        N += 1;

        oldtheta = y[1];

        currentStep = rdr.step(y[0]);

        //Euler
        /*for (int l = 0; l < Neq; l++)
	    {
		    float hdydx = currentStep * dydx[l];
		    y[l] = y[l] + hdydx;
	    }

	    geodesic(L,kappa,y, dydx);*/

        //RK4
        geodesic(L, kappa, y, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx1[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx2[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx3[l] = ak[l];
            ytemp[l] = y[l] + currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx4[l] = ak[l];
            dydx1[l] = currentStep / 6. * (dydx1[l] + 2. * dydx2[l] + 2. * dydx3[l] + dydx4[l]); //Recuperer une bonne approx de la derivée dans dydx1
            y[l] = y[l] + dydx1[l];
        }

        zSignChange = (oldtheta > M_PI_2) != (y[1] > M_PI_2);      //on traverse le plan z=0
        diskDistance = (y[0] < disk.R_max) && (y[0] > disk.R_min); //On l'a traversé la ou est le disque
        diskCollision = zSignChange && diskDistance;

        if (diskCollision)
        {
            float xpos, ypos, zpos;
            blToCartesian(y[0], y[1], y[2], &xpos, &ypos, &zpos); //Obtenir la position en coord cartesiennes
            getDirection(y, dydx1, xp, yp, zp);                   //Obtenir la direction (en coord cartesiennes)

            //Trouver le point de colision (en allant tout droit entre les deux point au dessus et en dessous du disque (valable si le pas est assez petit))
            float lambda = -zpos / (*zp);
            float coll_x = xpos + lambda * (*xp);
            float coll_y = ypos + lambda * (*yp);
            float coll_z = zpos + lambda * (*zp);
            float r = norm(coll_x, coll_y, coll_z);

            diskCollision = (r < disk.R_max) && (r > disk.R_min); //#reverification plus précise
            if (diskCollision)
            {
                if (k < rdr.maxtransparency)
                {

                    float phi = atan2(coll_y / r, coll_x / r); //Coordonné du point d'impact (en sphérique)

#if ADISK_NORMAL == 1

                    float diskspeed_x, diskspeed_y, diskspeed_z;

                    //rotation prograde du disque

                    sphericalToCartesian(M_PI / 2., phi + M_PI / 2., &diskspeed_x, &diskspeed_y, &diskspeed_z); //Direction de la vitesse en ce point, sens trigo,mvt circulaire

                    float beta = disk.RotationSpeed(r);                                                         //Obtenir la norme de la vitesse des poussières en ce point
                    float costhetadoppler = -((*xp) * diskspeed_x + (*yp) * diskspeed_y + (*zp) * diskspeed_z); //Obtenir le cosinus de l'angle entre le rayon et la vitesse des particules (Les vecteurs sont normés)
                    //- à cause du sens des rayons

                    //Temperature du disque à cet endroit
                    float Temp = disk.Temp(r);

                    //Décalage en fréquence égal à décalage en temperature, valable aussi pour l'intensité

                    Temp = Temp * (1 + beta * costhetadoppler) / sqrt(1 - beta * beta); //Effet doppler relativiste

                    //calcul du redshift gravitationnel
                    float gtt = 1. - 1. / r;
                    float gtphi = bh.a / r;
                    float gphiphi = -(r * r + bh.a2 + bh.a2 / r);
                    float omega = beta / r;                                                //Vitesse angulaire
                    Temp = Temp * sqrt(gtt + 2 * gtphi * omega + gphiphi * omega * omega); //Redshift gravitationnel

                    //Recuperer la texture du disque à cet endroit, qu'on utilise uniquemenet pour obtenir la transparence du disque
                    int cx = int((r - disk.R_min) * (adisk_height - 1) / (disk.R_max - disk.R_min));
                    int cy = int((adisk_width - 1) * mod(disk.texture_rep * phi - M_PI, 2. * M_PI) / (2. * M_PI));

                    int loc = (cx * adisk_width + cy) * CHANNEL_NUM;

                    pixel_transpr[k] = 0.;
                    pixel_transpg[k] = 0.;
                    pixel_transpb[k] = 0.;

                    getBodyColor(&pixel_transpr[k], &pixel_transpg[k], &pixel_transpb[k], Temp, 1.); //Obtenir la couleur

                    canalAlpha[k] = (float)adisk[loc + 1] / 255.; //Choix: on utilise la composante verte (le +1) pour reconstituer la transparence

#endif

#if ADISK_GRID == 1

                    getDiskColorGrid(phi, r, &pixel_transpr[k], &pixel_transpg[k], &pixel_transpb[k]); //Motif de grille sur le disque et calcul de la transparence
                    canalAlpha[0] = 1.;
#endif

                    k++;
                }
            }
        }
    }

    *ReachedInfinity = (y[0] >= rdr.R_inf);
    *nbCollision = k;

    if (*ReachedInfinity)
    {
        //Pour visualiser quels rayons ont atteints R_inf
        //*nbCollision=1;
        //pixel_transpr[0]=255;
        //canalAlpha[0]=.5;

        getDirection(y, dydx1, xp, yp, zp);
        normalise(xp, yp, zp); //Ne pas effectuer inutilement cette opération
    }
}

//Simulation d'un rayon (trajectoire uniquement)
void sim_opt(float r0, float theta0, float phi0, float xpixel, float ypixel, float *xp, float *yp, float *zp, bool *ReachedInfinity)
{

    int N = 0;

    //Euler
    //float y[Neq];
    //float dydx[Neq];

    //RK4
    float y[Neq], ak[Neq];
    float dydx1[Neq], dydx2[Neq], dydx3[Neq], dydx4[Neq];
    float ytemp[Neq];

    float L, kappa;

    initial(r0, theta0, &L, &kappa, y, ak, xpixel, ypixel); //CI

    float currentStep = rdr.step(y[0]);

    int l;

    while ((N < MAXITER) && (rdr.R_min < y[0]) && (y[0] < rdr.R_inf))
    {

        N += 1;

        currentStep = rdr.step(y[0]);

        //Euler
        /*for (int l = 0; l < Neq; l++)
	    {
		    float hdydx = currentStep * dydx[l];
		    y[l] = y[l] + hdydx;
	    }
	    geodesic(L,kappa,y, dydx);*/

        //RK4

        geodesic(L, kappa, y, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx1[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx2[l] = ak[l];
            ytemp[l] = y[l] + .5 * currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx3[l] = ak[l];
            ytemp[l] = y[l] + currentStep * ak[l];
        }
        geodesic(L, kappa, ytemp, ak);
        for (l = 0; l < Neq; ++l)
        {
            dydx4[l] = ak[l];
            dydx1[l] = currentStep / 6. * (dydx1[l] + 2. * dydx2[l] + 2. * dydx3[l] + dydx4[l]); //Recuperer une bonne aprrox de la derivée
            y[l] = y[l] + dydx1[l];
        }
    }

    *ReachedInfinity = (y[0] >= rdr.R_inf);

    if (*ReachedInfinity)
    { //Ne pas effectuer inutilement cette opération
        getDirection(y, dydx1, xp, yp, zp);
        normalise(xp, yp, zp);
    }
}

//Simulation d'un faisceu de rayons (uniquement dans le cas ou on dessine des étoiles)
void sim_bundle(int i, int j, float x, float y, float z, float *pixelr, float *pixelg, float *pixelb)
{ //arguments: i et j position du pixel, x,y,z position initiale du rayon dans l'espace

    float r0, theta0, phi0;
    cartesianToBl(x, y, z, &r0, &theta0, &phi0); //Approximativement (compliqué de passer de cartesien à BL), mais pas important

    float xpcentre0, ypcentre0, zpcentre0;
    float xpcentref, ypcentref, zpcentref;
    float xp0, yp0, zp0;
    float xpf, ypf, zpf;

    bool ReachedInfinity = true;
    int nbCollision;

    float maxdist2 = 0.;
    float maxdist2_0 = 0.;
    //float mindist2=1000.;
    //float mindist2_0=1000.;

    /*float Distances2[4];
    float Distances2_0[4];
    float dilatation=1.;*/

    float pixel_transpr[rdr.maxtransparency], pixel_transpg[rdr.maxtransparency], pixel_transpb[rdr.maxtransparency];
    float canalAlpha[rdr.maxtransparency];

    xpcentref = xpcentre0;
    ypcentref = ypcentre0;
    zpcentref = zpcentre0;

    float range = 0.075 * 20. / (rdr.width - 1.0); //FOV
    //float range = 0.1 * 20. / (rdr.width - 1.0);

    float xpixel = -(i - (rdr.width + 1.0) / 2) * range;
    float ypixel = -(j - (rdr.height + 1.0) / 2) * range;

    sim(r0, theta0, phi0, xpixel, ypixel, &xpcentref, &ypcentref, &zpcentref, pixel_transpr, pixel_transpg, pixel_transpb, canalAlpha, &ReachedInfinity, &nbCollision); //Simulation du rayon principal

#if DRAWSTARS == 1

    int Nray = 0;

    while (Nray < 4 && ReachedInfinity)
    {

        if (Nray == 0)
        {
            xpixel = -(i + rdr.delta - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j - (rdr.height + 1.0) / 2) * range;
        }
        if (Nray == 1)
        {
            xpixel = -(i - rdr.delta - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j - (rdr.height + 1.0) / 2) * range;
        }
        if (Nray == 2)
        {
            xpixel = -(i - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j + rdr.delta - (rdr.height + 1.0) / 2) * range;
        }
        if (Nray == 3)
        {
            xpixel = -(i - (rdr.width + 1.0) / 2) * range;
            ypixel = -(j - rdr.delta - (rdr.height + 1.0) / 2) * range;
        }

        Nray++;

        xpf = xp0;
        ypf = yp0;
        zpf = zp0;

        sim_opt(r0, theta0, phi0, xpixel, ypixel, &xpf, &ypf, &zpf, &ReachedInfinity); //Simulation des rayons secondaires

        if (ReachedInfinity)
        {
            //Calcule de la taille de la zone d'impact des differents rayons (zone en terme de direction)
            //On trouve le rayon maxdist du cercle centré sur la direction du rayon principal, qui contient les autres rayons (tjrs en terme de direction)
            //Calcul en coord cartesienne pour éviter les discontinuités liées au modulo 2PI

            //float dx0=xpcentre0-xp0;
            //float dy0=ypcentre0-yp0;
            //float dz0=zpcentre0-zp0;

            //float dist2_0=sqrnorm(dx0,dy0,dz0);

            float dx = xpcentref - xpf;
            float dy = ypcentref - ypf;
            float dz = zpcentref - zpf;

            float dist2 = sqrnorm(dx, dy, dz);

            //if (maxdist2_0<dist2_0){ maxdist2_0=dist2_0; }
            if (maxdist2 < dist2)
            {
                maxdist2 = dist2;
            }
        }
    }

    if (ReachedInfinity)
    { //Si tous les rayons se sont échappés, on regarde quels étoiles sont dans le cercle formé par le faisceau sur le ciel

        float maxdist = sqrt(maxdist2);
        float sqrdistanceToStar; //sqrnorm(scn.starx[i]-xpcentref,scn.stary[i]-ypcentref,scn.starz[i]-zpcentref);

        float c1, c2, c3;

        for (int i = 0; i < Nstar; ++i)
        {

            c1 = abs(scn.starx[i] - xpcentref);

            if (c1 < maxdist)
            {
                c2 = abs(scn.stary[i] - ypcentref);
                if (c2 < maxdist)
                {
                    c3 = abs(scn.starz[i] - zpcentref);
                    if (c3 < maxdist)
                    {
                        sqrdistanceToStar = sqrnorm(c1, c2, c3);
                        if (sqrdistanceToStar < maxdist2)
                        {
                            getBodyColor(pixelr, pixelg, pixelb, scn.starTemp[i], scn.starBrightness[i] * 12.); //(Récupere la couleur et effectue l'addition composante par composante)
                        }
                    }
                }
            }
        }

        float b = 1e-7 / maxdist2; //Angle solide initial environ constant égal à 1e-7
        *pixelr *= b;
        *pixelg *= b;
        *pixelb *= b;
    }
#endif
#if DRAWGRID == 0
    getFinalColor(pixelr, pixelg, pixelb, pixel_transpr, pixel_transpg, pixel_transpb, canalAlpha, nbCollision); // Calcul de transparence
#else
    getFinalColorGrid(xpcentref, ypcentref, zpcentref, pixelr, pixelg, pixelb, pixel_transpr, pixel_transpg, pixel_transpb, nbCollision, ReachedInfinity); // Calcul de la transparence, et calcul de la couleur de la grille
#endif
}

void render(const char *name)
{
    float *image = new float[rdr.width * rdr.height * CHANNEL_NUM]; //Tableau qui va contenir les données

    int Chunkcomputed = 0;
    pl::async_par_for(0, rdr.TotalChunknumber, [&](unsigned h) {
        //for (int h=0;h<rdr.TotalChunknumber;++h){ //A utiliser si jamais pl async ne marche pas
        printf("file %s - Chunk %04d/%04d started - %.2f pourcents  \n", name, h, rdr.TotalChunknumber, 100. * (float)Chunkcomputed / ((float)rdr.TotalChunknumber));

        int j0 = h/rdr.NChunkWidth;
        int i0 = h % rdr.NChunkWidth;

        i0*=rdr.ChunkSizeWidth;
        j0*=rdr.ChunkSizeHeight;
        int i1=i0 + rdr.ChunkSizeWidth;
        int j1=j0 + rdr.ChunkSizeHeight;

        for (int j = j0; j < j1; ++j)
        {

            for (int i = i0; i < i1; ++i)
            {

                float pixelr, pixelg, pixelb;

                pixelr = 0.;
                pixelg = 0.;
                pixelb = 0.;

                sim_bundle(i, j, scn.camera.x, scn.camera.y, scn.camera.z, &pixelr, &pixelg, &pixelb);

                int loc = (rdr.width * j + i) * CHANNEL_NUM;

                image[loc] = pixelr;
                image[loc + 1] = pixelg;
                image[loc + 2] = pixelb;

            }
        }
        Chunkcomputed++;

    });
    //}

    //Postprocess possible ici
    int index = 0;

    uint8_t *image_byte = new uint8_t[rdr.width * rdr.height * CHANNEL_NUM]; //Image finale

    std::cout << "conversion" << endl;

    for (int j = 0; j < rdr.height; ++j)
    {

        for (int i = 0; i < rdr.width; ++i)
        {

            image_byte[index] = char(image[index]);
            index++;

            image_byte[index] = char(image[index]);
            index++;

            image_byte[index] = char(image[index]);
            index++;
        }
    }

    stbi_write_png(name, rdr.width, rdr.height, CHANNEL_NUM, image_byte, rdr.width * CHANNEL_NUM);
}

int image(string fichier)//int image()
{
    ifstream monFlux;
    monFlux.open(fichier.c_str()) ;

    if (monFlux)
    {
        monFlux >> bh.a >> disk.R_max >> scn.camera.x >> scn.camera.y >> scn.camera.z >> rdr.R_inf >> rdr.width >> rdr.height;
        cout<<"Les paramètres ont été initialisés"<<endl;
        if (adisk == NULL)
        {
            cout<<"L'image de départ n'a pas été trouvée"<< endl;
            exit(0);
        }

        /*rdr.height = 1080 / 8;
        rdr.width = 1920 / 8;
        rdr.R_inf = 21.5; //distance à partir de laquelle on considere etre a l'infini */

        rdr.ChunkSizeHeight = 15; //Blocs de 15 par 15 pixels traités en parallele
        rdr.ChunkSizeWidth = 15; //Blocs de 15 par 15 pixels traités en parallele

        //Euler
        /*rdr.stepmax=0.0035;
    rdr.stepmin=0.002; //0.001 min sinon erreurs arrondi ?*/

        //RK4
        rdr.stepmax = 0.02 * 15;
        rdr.stepmin = 0.007 * 15;

        rdr.delta = .5; //Ecart angulaire (en pixel) entre les rayons d'un même faisceau

        //bh.a = 0.5;   //spin (adimensionné,entre 0 et 1)
        bh.precalc(); //A executer avant inner orbit

        disk.R_min = bh.inner_orbit(); //Trouver l'orbite la plus proche du trou noir encore stable (prograde)

        //disk.R_max = 16.;
        disk.betamax = 0.65; //Vitesse du disque au plus proche du trou noir (c'est la qu'est la vitesse max pour un profil de vitesse en r^-1/2)
        disk.TMax = 17500.;  //temperature du disque au plus proche du trou noir

        //scn.FOV=40.;

        disk.texture_rep = 8.; //Répéter la texture du dique (en longeur pour ne pas qu'elle soit pixelisée)

        rdr.precalc(bh.a2); //quelques calculs pour avoir les carrés de certaines qtité et le fov en radian etc..
        //scn.precalc(rdr.width);
        disk.precalc();

        scn.generateSky();

        //Rendre une image fixe:
        /*scn.camera.x = 20.;
        scn.camera.y = 0.;
        scn.camera.z = 1.2;*/

        /*scn.camera.theta=0.04;
    scn.camera.phi=0.01;*/

        readSensitivityData("sensitivity.txt", wavelengthSamples, wavelengthSamples5, sensitivitySamplesR, sensitivitySamplesG, sensitivitySamplesB);

        /*for (int l = 0; l < SpectrumSampleSize; ++l)
    {
        printf("WL:%f R:%f, G:%f B:%f \n",wavelengthSamples[l],sensitivitySamplesR[l],sensitivitySamplesG[l],sensitivitySamplesB[l]);
    }*/

        //float R,G,B;
        //getBodyColor(&R,&G,&B,6000,1.);
        //printf("R:%f G:%f, B:%f \n",R,G,B);

        cout << "Start" << endl;
        render("resultat.png");
        cout << "End" << endl;

        stbi_image_free(adisk);
    }
    else
    {
        cout << "Erreur le fichier ne s'est pas ouvert" << endl;
    }
    return 0;
}

/*int main()
{
    image("params.txt");
    return 0;
}*/

#include <boost/python.hpp>

BOOST_PYTHON_MODULE(kerr)
{
    using namespace boost::python;
    def("image", image);
}


#include <aslam/backend/Optimizer2.hpp>
// std::partial_sum
#include <numeric>
#include <aslam/backend/ErrorTerm.hpp>
// M.inverse()
#include <Eigen/Dense>
#include <sm/eigen/assert_macros.hpp>
#include <sparse_block_matrix/linear_solver_dense.h>
#include <sparse_block_matrix/linear_solver_cholmod.h>
#ifndef QRSOLVER_DISABLED
#include <sparse_block_matrix/linear_solver_spqr.h>
#include <aslam/backend/SparseQrLinearSystemSolver.hpp>
#endif
#include <aslam/backend/sparse_matrix_functions.hpp>
#include <aslam/backend/BlockCholeskyLinearSystemSolver.hpp>
#include <aslam/backend/SparseCholeskyLinearSystemSolver.hpp>
#include <aslam/backend/DenseQrLinearSystemSolver.hpp>
#include <sm/PropertyTree.hpp>

namespace aslam {
    namespace backend {

        Optimizer2::Optimizer2(const Optimizer2Options& options) :
            _options(options)
        {
            initializeLinearSolver();
            initializeTrustRegionPolicy();
        }

        Optimizer2::Optimizer2(const sm::PropertyTree& config, boost::shared_ptr<LinearSystemSolver> linearSystemSolver, boost::shared_ptr<TrustRegionPolicy> trustRegionPolicy) {
          Optimizer2Options options;
          options.convergenceDeltaJ = config.getDouble("convergenceDeltaJ", options.convergenceDeltaJ);
          options.convergenceDeltaX = config.getDouble("convergenceDeltaX", options.convergenceDeltaX);
          options.maxIterations = config.getInt("maxIterations", options.maxIterations);
          options.doSchurComplement = config.getBool("doSchurComplement", options.doSchurComplement);
          options.verbose = config.getBool("verbose", options.verbose);
          options.linearSolverMaximumFails = config.getInt("linearSolverMaximumFails", options.linearSolverMaximumFails);
          options.nThreads = config.getInt("nThreads", options.nThreads);
          options.linearSystemSolver = linearSystemSolver;
          options.trustRegionPolicy = trustRegionPolicy;
          _options = options;
          initializeLinearSolver();
          initializeTrustRegionPolicy();
          // USING C++11 would allow to do constructor delegation and more elegant code, i.e., directly call the upper constructor
        }

        Optimizer2::~Optimizer2()
        {
        }

        /// \brief Set up to work on the optimization problem.
        void Optimizer2::setProblem(boost::shared_ptr<OptimizationProblemBase> problem)
        {
            _problem = problem;
        }

        void Optimizer2::initializeTrustRegionPolicy()
        {
          if( !_options.trustRegionPolicy ) {
            _options.verbose && std::cout << "No trust region policy set in the options. Defaulting to levenberg_marquardt\n";
            _trustRegionPolicy.reset( new LevenbergMarquardtTrustRegionPolicy() );
          } else {
            _trustRegionPolicy = _options.trustRegionPolicy;
          }

          // \todo remove this check when the sparse qr solver supports an augmented diagonal
          if(_solver->name() == "sparse_qr" && _trustRegionPolicy->name() == "levenberg_marquardt") {
            _options.verbose && std::cout << "The sparse_qr solver is not compatible with levenberg_marquardt. Changing to the dog_leg trust region policy\n";
            _trustRegionPolicy.reset( new DogLegTrustRegionPolicy() );
          }

          _options.verbose && std::cout << "Using the " << _trustRegionPolicy->name() << " trust region policy\n";

        }

        void Optimizer2::initializeLinearSolver()
        {
          if( ! _options.linearSystemSolver ) {
            _options.verbose && std::cout << "No linear system solver set in the options. Defaulting to the sparse_cholesky solver\n";
            _solver.reset(new SparseCholeskyLinearSystemSolver());
          } else {
            _solver = _options.linearSystemSolver;
          }

          _options.verbose && std::cout << "Using the " << _solver->name() << " linear system solver\n";
        }

        /// \brief initialize the optimizer to run on an optimization problem.
        ///        This should be called before calling optimize()
        void Optimizer2::initialize()
        {
          initializeLinearSolver();
          initializeTrustRegionPolicy();

          SM_ASSERT_FALSE(Exception, _problem.get() == NULL, "No optimization problem has been set");
            _options.verbose && std::cout << "Initializing\n";
            Timer init("Optimizer2: Initialize Total");
            _designVariables.clear();
            _designVariables.reserve(_problem->numDesignVariables());
            _errorTerms.clear();
            _errorTerms.reserve(_problem->numErrorTerms());
            Timer initDv("Optimizer2: Initialize---Design Variables");
            // Run through all design variables adding active ones to an active list.
            // std::cout << "dvloop 1\n";
            for (size_t i = 0; i < _problem->numDesignVariables(); ++i) {
                DesignVariable* dv = _problem->designVariable(i);
                if (dv->isActive())
                    _designVariables.push_back(dv);
            }
            SM_ASSERT_FALSE(Exception, _designVariables.empty(), "It is illegal to run the optimizer with all marginalized design variables.");
            // Assign block indices to the design variables.
            // "blocks" will hold the structure of the left-hand-side of Gauss-Newton
            int columnBase = 0;
            // std::cout << "dvloop 2\n";
            for (size_t i = 0; i < _designVariables.size(); ++i) {
                _designVariables[i]->setBlockIndex(i);
                _designVariables[i]->setColumnBase(columnBase);
                columnBase += _designVariables[i]->minimalDimensions();
            }
            initDv.stop();
            Timer initEt("Optimizer2: Initialize---Error Terms");
            // Get all of the error terms that work on these design variables.
            int dim = 0;
            // std::cout << "eloop 1\n";
            for (unsigned i = 0; i < _problem->numErrorTerms(); ++i) {
                ErrorTerm* e = _problem->errorTerm(i);
                _errorTerms.push_back(e);
                e->setRowBase(dim);
                dim += e->dimension();
            }
            initEt.stop();
            SM_ASSERT_FALSE(Exception, _errorTerms.empty(), "It is illegal to run the optimizer with no error terms.");
            Timer initMx("Optimizer2: Initialize---Matrices");
            // Set up the block matrix structure.
            // std::cout << "init structure\n";
            //      initializeLinearSolver();
            _solver->initMatrixStructure(_designVariables, _errorTerms, _trustRegionPolicy->requiresAugmentedDiagonal());
            initMx.stop();
            _options.verbose && std::cout << "Optimization problem initialized with " << _designVariables.size() << " design variables and " << _errorTerms.size() << " error terms\n";
            // \todo Say how big the problem is.
            _options.verbose && std::cout << "The Jacobian matrix is " << dim << " x " << columnBase << std::endl;

            // \todo initialize the trust region stuff.

        }

        /*
        // returns true of stop!
        bool Optimizer2::evaluateStoppingCriterion(int iterations)
        {

        // as we have analytic Jacobians we can assume the precision to be:
        double epsilon = std::numeric_limits<double>::epsilon();

        double x_norm = ...;

        // the gradient: is simply the right hand side of GN:
        double grad_norm = _rhs.norm();
        double abs_J = fabs(_J);

        // the first condition:
        bool crit1 = grad_norm < sqrt(epsilon) * (1 + abs_J);

        bool crit2 = _dx.norm() < sqrt(epsilon) * (1 + x_norm);

        bool crit3 = fabs(_J - _p_J) < epsilon * (1 + abs_J);

        bool crit4 = iterations < _options.maxIterations;

        return (crit1 && crit2 && crit3) || crit4;

        }*/

        SolutionReturnValue Optimizer2::optimize()
        {
            Timer timeGn("Optimizer2: build Hessian", true);
            Timer timeErr("Optimizer2: evaluate error", true);
            Timer timeSchur("Optimizer2: Schur complement", true);
            Timer timeBackSub("Optimizer2: Back substitution", true);
            Timer timeSolve("Optimizer2: Solve linear system", true);
            // Select the design variables and (eventually) the error terms involved in the optimization.
            initialize();
            SolutionReturnValue srv;
            _p_J = 0.0;

            //std::cout << "Evaluate error for the first time\n";
            // This sets _J
            timeErr.start();
            evaluateError(true);
            timeErr.stop();
            _p_J = _J;
            srv.JStart = _p_J;
            // *** while not done
            _options.verbose && std::cout << "[" << srv.iterations << ".0]: J: " << _J << std::endl;
            // Set up the estimation problem.
            double deltaX = _options.convergenceDeltaX + 1.0;
            double deltaJ = _options.convergenceDeltaJ + 1.0;
            bool previousIterationFailed = false;
            bool linearSolverFailure = false;

            SM_ASSERT_TRUE(Exception, _solver.get() != NULL, "The solver is null");
            _trustRegionPolicy->setSolver(_solver);
            _trustRegionPolicy->optimizationStarting(_J);

            // Loop until convergence
            while (srv.iterations <  _options.maxIterations &&
                   srv.failedIterations < _options.maxIterations &&
                   ((deltaX > _options.convergenceDeltaX &&
                     fabs(deltaJ) > _options.convergenceDeltaJ) ||
                    linearSolverFailure)) {

                timeSolve.start();
                bool solutionSuccess = _trustRegionPolicy->solveSystem(_J, previousIterationFailed, _options.nThreads, _dx);
                timeSolve.stop();

                if (!solutionSuccess) {
                    _options.verbose && std::cout << "[WARNING] System solution failed\n";
                    previousIterationFailed = true;
                    linearSolverFailure = true;
                    srv.failedIterations++;
                } else {
                    /// Apply the state update. _A, _b, _dx, and _H are passed in implicitly.
                    timeBackSub.start();
                    deltaX = applyStateUpdate();
                    timeBackSub.stop();
                    // This sets _J
                    timeErr.start();
                    evaluateError(true);
                    timeErr.stop();
                    deltaJ = _p_J - _J;
                    // This was a regression.
                    if( _trustRegionPolicy->revertOnFailure() )
                    {
                        if(deltaJ < 0.0)
                        {
                            _options.verbose && std::cout << "Last step was a regression. Reverting\n";
                            revertLastStateUpdate();
                            srv.failedIterations++;
                            previousIterationFailed = true;
                        }
                        else
                        {
                            _p_J = _J;
                            previousIterationFailed = false;
                        }
                    }
                    else
                    {
                        _p_J = _J;
                    }
                    srv.iterations++;

                    _options.verbose && std::cout << "[" << srv.iterations << "]: J: " << _J << ", dJ: " << deltaJ << ", deltaX: " << deltaX << ", ";
                    _options.verbose && _trustRegionPolicy->printState(std::cout);
                    _options.verbose && std::cout << std::endl;
                } // if the linear solver failed / else
            }

            srv.JFinal = _p_J;
            srv.dXFinal = deltaX;
            srv.dJFinal = deltaJ;
            srv.linearSolverFailure = linearSolverFailure;
            return srv;
        }

            DesignVariable* Optimizer2::designVariable(size_t i)
            {
                SM_ASSERT_LT_DBG(Exception, i, _designVariables.size(), "index out of bounds");
                return _designVariables[i];
            }

            size_t Optimizer2::numDesignVariables() const
            {
                return _designVariables.size();
            }

            double Optimizer2::applyStateUpdate()
            {
                // Apply the update to the dense state.
                int startIdx = 0;
                for (size_t i = 0; i < numDesignVariables(); i++) {
                    DesignVariable* d = _designVariables[i];
                    const int dbd = d->minimalDimensions();
                    Eigen::VectorXd dxS = _dx.segment(startIdx, dbd);
                    dxS *= d->scaling();
                    d->update(&dxS[0], dbd);
                    startIdx += dbd;
                }
                // Track the maximum delta
                // \todo: should this be some other metric?
                double deltaX = _dx.array().abs().maxCoeff();
                return deltaX;
            }

            void Optimizer2::revertLastStateUpdate()
            {
                for (size_t i = 0; i < _designVariables.size(); i++) {
                    _designVariables[i]->revertUpdate();
                }
            }

            Optimizer2Options& Optimizer2::options()
            {
                return _options;
            }

            double Optimizer2::evaluateError(bool useMEstimator)
            {
                SM_ASSERT_TRUE(Exception, _solver.get() != NULL, "The solver is null");
                _J = _solver->evaluateError(_options.nThreads, useMEstimator);
                return _J;
            }

            /// \brief return the reduced system dx
            const Eigen::VectorXd& Optimizer2::dx() const
            {
                return _dx;
            }

            /// The value of the objective function.
            double Optimizer2::J() const
            {
                return _J;
            }

            void Optimizer2::printTiming() const
            {
                sm::timing::Timing::print(std::cout);
            }

            void Optimizer2::checkProblemSetup()
            {
                // Check that all error terms are hooked up to design variables.
            }

            void Optimizer2::computeDiagonalCovariances(SparseBlockMatrix& outP, double lambda)
            {
                SM_THROW(Exception, "Broken");

                std::vector<std::pair<int, int> > blockIndices;
                for (size_t i = 0; i < _designVariables.size(); ++i) {
                    blockIndices.push_back(std::make_pair(i, i));
                }
                computeCovarianceBlocks(blockIndices, outP, lambda);
            }

    void Optimizer2::computeCovarianceBlocks(const std::vector<std::pair<int, int> > & /* blockIndices */, SparseBlockMatrix& /* outP */, double /* lambda */)
            {
                SM_THROW(Exception, "Broken");

            }

    void Optimizer2::computeCovariances(SparseBlockMatrix& /* outP */, double /* lambda */)
            {
                SM_THROW(Exception, "Broken");

            }

        void Optimizer2::computeHessian(SparseBlockMatrix& outH, double lambda)
            {

              boost::shared_ptr<BlockCholeskyLinearSystemSolver> solver_sp;
              solver_sp.reset(new BlockCholeskyLinearSystemSolver());
              // True here for creating the diagonal conditioning.
              solver_sp->initMatrixStructure(_designVariables, _errorTerms, true);

              _options.verbose && std::cout << "Setting the diagonal conditioner to: " << lambda << ".\n";
              evaluateError(false);
              solver_sp->setConstantConditioner(lambda);
              solver_sp->buildSystem(_options.nThreads, false);
              solver_sp->copyHessian(outH);
            }

      const LinearSystemSolver * Optimizer2::getBaseSolver() const {
          return _solver.get();
      }

        const Matrix * Optimizer2::getJacobian() const {
            return _solver->Jacobian();
        }

        } // namespace backend
    } // namespace aslam


#include "precompiled.h"

#ifndef AUTOMATIC_PRECOMPILATION
#include <boost/property_tree/ptree.hpp>
#include <boost/throw_exception.hpp>
#include <exception>
#include <memory>
#include <algorithm>
#include <boost/lexical_cast.hpp>
#endif

#include "grid.h"
#include "utility.h"
#include "grid_tests.h"
#include "utility.h"
#include "global_config.h"
#include "grid_iterators.h"
#include "configuration_parser.h"

void cut_off(double& a){ // or you need something like CodeRefTHR
	a = abs(a)<1e-15 ? 0 : a;
}

namespace fipster { namespace _grid {

	state_variable_t::state_variable_t( const ptree& pt )
		:	n(betterGet<int>(pt,"n","state variable"))
		,	spacing_node(betterChild(pt,"spacing","state variable "+toS(n)))
		, hits_zero(spacing_node.get("<xmlattr>.hitsZero",false))
	{
		lower_bound = pt.get<double>("lowerBound");
		upper_bound = pt.get<double>("upperBound");
		FIPSTER_ASSERT(lower_bound<upper_bound);
		tolerance = pt.get("tolerance",1e-17);

		spacing_type = spacing_node.get<string>("<xmlattr>.type");
	}

	template<class A>
	void state_variable_t::fill_coords( A& coords, A& stepsizes )
	{
#define coords(i) coords[i]
#define stepsizes(i) stepsizes[i]
		// homogeneous #########################################
		if(spacing_type == "homogeneous"){
			double s;
			s = (upper_bound-lower_bound)/(n-1);
			for(uint i = 0; i<n ; ++i )
				coords(i) = i*s + lower_bound;

			// exponential #########################################
		}else if(spacing_type == "exponential"){
			double a,b,h = spacing_node.get<double>("h");
			a = (upper_bound-lower_bound)/(exp(h)-1.0);
			b = h/(n-1);
			for(uint i=0; i < n; ++i)
				coords(i) = a*(exp(i*b)-1.0)+lower_bound;

			// not implemented #########################################
		}else
			BOOST_THROW_EXCEPTION(invalid_argument("Grid spacing '"+spacing_type+"' not implemented"));

    //shift to include 0
		double shift = 0;
		if(hits_zero){
			vector<double> temp=coords;
			for(auto& t: temp) t=abs(t);
			shift=coords(min_element(begin(temp),end(temp))-begin(temp));
			for(auto& t: coords) t-=shift;
		}

		//produce stepsizes
		for(uint i=0;i<n-1;i++){
			cut_off(coords(i));
			stepsizes(i) = coords(i+1)-coords(i);

		}

		// validate coordinates and stepsizes  #########################################
		double s=abs(coords(n-1)-upper_bound+shift);
		if(s >= tolerance*(upper_bound-lower_bound)
			|| s!=s ) //This checks for NaN and Inf and stuff
			BOOST_THROW_EXCEPTION(underflow_error
														("coords did to not add up (rel.Err: "+toString(s)+
														 ", tolerance: "+toString(tolerance)+")"));

#undef coords
#undef stepsizes
	}

	Grid::Grid( const ptree& pt )
		: state_variables_config_t(pt)
		, id(betterGet<string>(pt,"<xmlattr>.id","grid"))
		, max_subgrid(pt.get_optional<double>("maxSubgrid"))
	{
		//infer the dimension of the grid
		D = sv_vec.size();
		sizes.resize(D);

		maxsize=0;
		for(uint i=0;i<D;i++){
			sizes[i] = sv_vec[i].n;
			maxsize = max(maxsize,sizes[i]);
		}

		//Strides and Number of points
		N.resize(implemented_inner_grids);
		strides.resize(implemented_inner_grids,D);
		Nbound.resize(implemented_inner_grids-1);

		for(uint j=0; j < implemented_inner_grids; ++j)
		{
			//Strides
			strides(j,0) = 1;
			for(uint i=1; i < D; ++i)
				strides(j,i)=strides(j,i-1)*(sizes[i-1]-j*2);

			//Number of points
			N[j]=strides(j,D-1)*(sizes[D-1]-j*2);

			//check if sizes are sufficient to implement up to implemented_inner_grids!
			if(max_subgrid && *max_subgrid>=j && sizes[D-1]<=(int)j*2)
				BOOST_THROW_EXCEPTION(
					runtime_error("grid size in dimension "+toS(j)+" is too small for inner_grid "+toS(j)));
		}

		//Number of boundary points
		for(uint j=0; j < implemented_inner_grids-1; ++j)
			Nbound[j] = N[j] - N[j+1];

		//origin offsets
		for(uint j=0; j < implemented_inner_grids; ++j)
			origin_offset[j] = strides.row(j).sum()*j;

		//reserve space
		coords.resize(D);
		stepsizes.resize(D);

		//fill coordinates and stepsizes
		for(uint i=0;i<D;i++){
			coords[i].resize(sizes[i]);
			stepsizes[i].resize(sizes[i]-1);
			sv_vec[i].fill_coords(coords[i],stepsizes[i]);
		}

		//test iterator logic:
		if(global_config::get().test_grid_iterators)
			test_iterator_logic(*this);
	}

	ostream& operator<<(ostream& out,const Grid& grid)
	{
		auto state_it = grid_iterator<0,true>(grid);
		for(;state_it!=grid.end<0>();++state_it){
			out<<state_it.ind<<"\t";
			for(uint i=0;i<grid.D;i++)
				out<<boost::lexical_cast<string>(state_it.state()[i])<<"\t";
			out<<"\n";
		}
		out.flush();
		return out;
	}

}}


#define BOOST_TEST_MODULE "test_truncate"

#include <boost/test/included/unit_test.hpp>

// the code to test:
#include "truncate.hpp"

BOOST_AUTO_TEST_CASE(_fast_pow) {
    // results for fast_pow(e < -126) are not defined
    BOOST_CHECK_EQUAL(fast_pow(0), 1);
    BOOST_CHECK_EQUAL(fast_pow(1), 2);
    BOOST_CHECK_EQUAL(fast_pow(2), 4);
    BOOST_CHECK_EQUAL(fast_pow(5), 32);
    BOOST_CHECK_EQUAL(fast_pow(-1), 0.5);
    BOOST_CHECK_EQUAL(fast_pow(-2), 0.25);
    BOOST_CHECK_EQUAL(fast_pow(-4), 0.0625);
    BOOST_CHECK_EQUAL(fast_pow(64), std::pow(2.0, 64));
    BOOST_CHECK_EQUAL(fast_pow(-64), std::pow(2.0, -64));
    BOOST_CHECK_EQUAL(std::numeric_limits<float>::min(), (float)(std::pow(2.0, -126)));
    BOOST_CHECK_EQUAL(fast_pow(-125), (float)std::pow(2.0, -125));
    BOOST_CHECK_EQUAL(fast_pow(-126), (float)std::pow(2.0, -126));
    BOOST_CHECK_EQUAL(fast_pow(INT8_MAX), std::pow(2.0, INT8_MAX));
    BOOST_CHECK_EQUAL(fast_pow(INT8_MIN), -1.0 * std::numeric_limits<float>::infinity());
}

BOOST_AUTO_TEST_CASE(_count_zeros) {
    // int32_t count_zeros(int32_t x)
    BOOST_CHECK_EQUAL(count_zeros(0), 32);
    BOOST_CHECK_EQUAL(count_zeros(1), 31);
    BOOST_CHECK_EQUAL(count_zeros(2), 30);
    BOOST_CHECK_EQUAL(count_zeros(3), 30);
    BOOST_CHECK_EQUAL(count_zeros(INT32_MAX), 1);
    BOOST_CHECK_EQUAL(count_zeros(INT32_MIN), 0);
}

BOOST_AUTO_TEST_CASE(_bit_truncate_float) {
    // float bit_truncate_float(float val, float err)
    BOOST_CHECK_SMALL(bit_truncate_float(0.11, 0.01) - 0.11, 0.01);
    BOOST_CHECK(bit_truncate_float(0.11, 0.01) < 0.11);

    BOOST_CHECK_SMALL(bit_truncate_float(0.11, 0.01) - 0.11, 0.01);
    BOOST_CHECK(bit_truncate_float(0.11, 0.01) < 0.11);

    BOOST_CHECK(bit_truncate_float(0.0, 0.0) == 0.0);
    BOOST_CHECK_SMALL(bit_truncate_float(0.11, 0.0) - 0.11, 0.0001);
    BOOST_CHECK_SMALL(bit_truncate_float(1.11, 0.0) - 1.11, 0.0001);
    BOOST_CHECK(bit_truncate_float(0.0, 0.1) == 0.0);

    BOOST_CHECK_SMALL(bit_truncate_float(-0.11, 0.0) + 0.11, 0.0001);
    BOOST_CHECK_SMALL(bit_truncate_float(-1.11, 0.0) + 1.11, 0.0001);

    BOOST_CHECK_EQUAL(bit_truncate_float(std::numeric_limits<float>::max(), 0.01),
                      std::numeric_limits<float>::max());
    BOOST_CHECK_EQUAL(bit_truncate_float(std::numeric_limits<float>::min(), 0.01),
                      std::numeric_limits<float>::min());
}


#ifndef LINEAR_BASIS_HPP
#define LINEAR_BASIS_HPP

#include <armadillo>
#include <math.h>

class LinearBasisFunction : public Basis {

public:

    int _nX;
    int _nK;
    int _nU;
    int _nM;
    int _nKU;

    LinearBasisFunction(int _nX = 7, int _nK = 9, int _nU = 2 , int _nM = 6, int _nKU = 2) : Basis(_nX,_nK,_nU,_nM,_nKU) {
        this->_nX = _nX;
        this->_nK = _nK;
        this->_nU = _nU;
        this->_nM = _nM;
        this->_nKU = _nKU;
    }

    arma::vec fk(const arma::vec & x, const arma::vec & u ) {
        return arma::join_cols( fkx(x), fku(x, u) );
    }

    arma::vec fkx( const arma::vec& x ) {
        return arma::vec({
            x[0],
            x[1],
            x[2],
            x[3],
            x[4],
            x[5],
            1
        });
    }

    arma::vec fku( const arma::vec & x, const arma::vec& u ) {

        return arma::vec({
            u[0],
            u[1]
          });

    }

    arma::mat fkudu( const arma::vec & x, const arma::vec & u) {
        return arma::mat({
            {1,0},
            {0,1}
        });
    }

};

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestTransformIf
#include <boost/test/unit_test.hpp>

#include <boost/compute/lambda.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/experimental/transform_if.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(abs_if_odd)
{
    using compute::lambda::_1;

    // input data
    int data[] = { -2, -3, -4, -5, -6, -7, -8, -9 };
    compute::vector<int> vector(data, data + 8, queue);

    // calculate absolute value only for odd values
    compute::experimental::transform_if(
        vector.begin(),
        vector.end(),
        vector.begin(),
        compute::abs<int>(),
        _1 % 2 != 0,
        queue
    );

    // check transformed values
    CHECK_RANGE_EQUAL(int, 8, vector, (-2, +3, -4, +5, -6, +7, -8, +9));
}

BOOST_AUTO_TEST_SUITE_END()