/**
 * \file Math.hpp
 * \brief Header for GeographicLib::Math class
 *
 * Copyright (c) Charles Karney (2008-2019) <charles@karney.com> and licensed
 * under the MIT/X11 License.  For more information, see
 * https://geographiclib.sourceforge.io/
 **********************************************************************/

// Constants.hpp includes Math.hpp.  Place this include outside Math.hpp's
// include guard to enforce this ordering.
#include <GeographicLib/Constants.hpp>

#if !defined(GEOGRAPHICLIB_MATH_HPP)
#define GEOGRAPHICLIB_MATH_HPP 1

/**
 * Are C++11 math functions available?
 **********************************************************************/
#if !defined(GEOGRAPHICLIB_CXX11_MATH)
// Recent versions of g++ -std=c++11 (4.7 and later?) set __cplusplus to 201103
// and support the new C++11 mathematical functions, std::atanh, etc.  However
// the Android toolchain, which uses g++ -std=c++11 (4.8 as of 2014-03-11,
// according to Pullan Lu), does not support std::atanh.  Android toolchains
// might define __ANDROID__ or ANDROID; so need to check both.  With OSX the
// version is GNUC version 4.2 and __cplusplus is set to 201103, so remove the
// version check on GNUC.
#  if defined(__GNUC__) && __cplusplus >= 201103 && \
  !(defined(__ANDROID__) || defined(ANDROID) || defined(__CYGWIN__))
#    define GEOGRAPHICLIB_CXX11_MATH 1
// Visual C++ 12 supports these functions
#  elif defined(_MSC_VER) && _MSC_VER >= 1800
#    define GEOGRAPHICLIB_CXX11_MATH 1
#  else
#    define GEOGRAPHICLIB_CXX11_MATH 0
#  endif
#endif

#if !defined(GEOGRAPHICLIB_WORDS_BIGENDIAN)
#  define GEOGRAPHICLIB_WORDS_BIGENDIAN 0
#endif

#if !defined(GEOGRAPHICLIB_HAVE_LONG_DOUBLE)
#  define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 0
#endif

#if !defined(GEOGRAPHICLIB_PRECISION)
/**
 * The precision of floating point numbers used in %GeographicLib.  1 means
 * float (single precision); 2 (the default) means double; 3 means long double;
 * 4 is reserved for quadruple precision.  Nearly all the testing has been
 * carried out with doubles and that's the recommended configuration.  In order
 * for long double to be used, GEOGRAPHICLIB_HAVE_LONG_DOUBLE needs to be
 * defined.  Note that with Microsoft Visual Studio, long double is the same as
 * double.
 **********************************************************************/
#  define GEOGRAPHICLIB_PRECISION 2
#endif

#include <cmath>
#include <algorithm>
#include <limits>

#if GEOGRAPHICLIB_PRECISION == 4
#include <boost/version.hpp>
#include <boost/multiprecision/float128.hpp>
#include <boost/math/special_functions.hpp>
#elif GEOGRAPHICLIB_PRECISION == 5
#include <mpreal.h>
#endif

#if GEOGRAPHICLIB_PRECISION > 3
// volatile keyword makes no sense for multiprec types
#define GEOGRAPHICLIB_VOLATILE
// Signal a convergence failure with multiprec types by throwing an exception
// at loop exit.
#define GEOGRAPHICLIB_PANIC \
  (throw GeographicLib::GeographicErr("Convergence failure"), false)
#else
#define GEOGRAPHICLIB_VOLATILE volatile
// Ignore convergence failures with standard floating points types by allowing
// loop to exit cleanly.
#define GEOGRAPHICLIB_PANIC false
#endif

namespace GeographicLib {

  /**
   * \brief Mathematical functions needed by %GeographicLib
   *
   * Define mathematical functions in order to localize system dependencies and
   * to provide generic versions of the functions.  In addition define a real
   * type to be used by %GeographicLib.
   *
   * Example of use:
   * \include example-Math.cpp
   **********************************************************************/
  class GEOGRAPHICLIB_EXPORT Math {
  private:
    void dummy();               // Static check for GEOGRAPHICLIB_PRECISION
    Math();                     // Disable constructor
  public:

#if GEOGRAPHICLIB_HAVE_LONG_DOUBLE
    /**
     * The extended precision type for real numbers, used for some testing.
     * This is long double on computers with this type; otherwise it is double.
     **********************************************************************/
    typedef long double extended;
#else
    typedef double extended;
#endif

#if GEOGRAPHICLIB_PRECISION == 2
    /**
     * The real type for %GeographicLib. Nearly all the testing has been done
     * with \e real = double.  However, the algorithms should also work with
     * float and long double (where available).  (CAUTION: reasonable
     * accuracy typically cannot be obtained using floats.)
     **********************************************************************/
    typedef double real;
#elif GEOGRAPHICLIB_PRECISION == 1
    typedef float real;
#elif GEOGRAPHICLIB_PRECISION == 3
    typedef extended real;
#elif GEOGRAPHICLIB_PRECISION == 4
    typedef boost::multiprecision::float128 real;
#elif GEOGRAPHICLIB_PRECISION == 5
    typedef mpfr::mpreal real;
#else
    typedef double real;
#endif

    /**
     * @return the number of bits of precision in a real number.
     **********************************************************************/
    static int digits();

    /**
     * Set the binary precision of a real number.
     *
     * @param[in] ndigits the number of bits of precision.
     * @return the resulting number of bits of precision.
     *
     * This only has an effect when GEOGRAPHICLIB_PRECISION = 5.  See also
     * Utility::set_digits for caveats about when this routine should be
     * called.
     **********************************************************************/
    static int set_digits(int ndigits);

    /**
     * @return the number of decimal digits of precision in a real number.
     **********************************************************************/
    static int digits10();

    /**
     * Number of additional decimal digits of precision for real relative to
     * double (0 for float).
     **********************************************************************/
    static int extra_digits();

    /**
     * true if the machine is big-endian.
     **********************************************************************/
    static const bool bigendian = GEOGRAPHICLIB_WORDS_BIGENDIAN;

    /**
     * @tparam T the type of the returned value.
     * @return π.
     **********************************************************************/
    template<typename T> static T pi() {
      using std::atan2;
      static const T pi = atan2(T(0), T(-1));
      return pi;
    }
    /**
     * A synonym for pi<real>().
     **********************************************************************/
    static real pi() { return pi<real>(); }

    /**
     * @tparam T the type of the returned value.
     * @return the number of radians in a degree.
     **********************************************************************/
    template<typename T> static T degree() {
      static const T degree = pi<T>() / 180;
      return degree;
    }
    /**
     * A synonym for degree<real>().
     **********************************************************************/
    static real degree() { return degree<real>(); }

    /**
     * Square a number.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return x2.
     **********************************************************************/
    template<typename T> static T sq(T x)
    { return x * x; }

    /**
     * The hypotenuse function avoiding underflow and overflow.
     *
     * @tparam T the type of the arguments and the returned value.
     * @param[in] x
     * @param[in] y
     * @return sqrt(x2 + y2).
     **********************************************************************/
    template<typename T> static T hypot(T x, T y);

    /**
     * exp(\e x) − 1 accurate near \e x = 0.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return exp(\e x) − 1.
     **********************************************************************/
    template<typename T> static T expm1(T x);

    /**
     * log(1 + \e x) accurate near \e x = 0.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return log(1 + \e x).
     **********************************************************************/
    template<typename T> static T log1p(T x);

    /**
     * The inverse hyperbolic sine function.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return asinh(\e x).
     **********************************************************************/
    template<typename T> static T asinh(T x);

    /**
     * The inverse hyperbolic tangent function.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return atanh(\e x).
     **********************************************************************/
    template<typename T> static T atanh(T x);

    /**
     * Copy the sign.
     *
     * @tparam T the type of the argument.
     * @param[in] x gives the magitude of the result.
     * @param[in] y gives the sign of the result.
     * @return value with the magnitude of \e x and with the sign of \e y.
     *
     * This routine correctly handles the case \e y = −0, returning
     * &minus|x|.
     **********************************************************************/
    template<typename T> static T copysign(T x, T y);

    /**
     * The cube root function.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return the real cube root of \e x.
     **********************************************************************/
    template<typename T> static T cbrt(T x);

    /**
     * The remainder function.
     *
     * @tparam T the type of the arguments and the returned value.
     * @param[in] x
     * @param[in] y
     * @return the remainder of \e x/\e y in the range [−\e y/2, \e y/2].
     **********************************************************************/
    template<typename T> static T remainder(T x, T y);

    /**
     * The remquo function.
     *
     * @tparam T the type of the arguments and the returned value.
     * @param[in] x
     * @param[in] y
     * @param[out] n the low 3 bits of the quotient
     * @return the remainder of \e x/\e y in the range [−\e y/2, \e y/2].
     **********************************************************************/
    template<typename T> static T remquo(T x, T y, int* n);

    /**
     * The round function.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return \e x round to the nearest integer (ties round away from 0).
     **********************************************************************/
    template<typename T> static T round(T x);

    /**
     * The lround function.
     *
     * @tparam T the type of the argument.
     * @param[in] x
     * @return \e x round to the nearest integer as a long int (ties round away
     *   from 0).
     *
     * If the result does not fit in a long int, the return value is undefined.
     **********************************************************************/
    template<typename T> static long lround(T x);

    /**
     * Fused multiply and add.
     *
     * @tparam T the type of the arguments and the returned value.
     * @param[in] x
     * @param[in] y
     * @param[in] z
     * @return xy + z, correctly rounded (on those platforms with
     *   support for the fma instruction).
     *
     * On platforms without the fma instruction, no attempt is
     * made to improve on the result of a rounded multiplication followed by a
     * rounded addition.
     **********************************************************************/
    template<typename T> static T fma(T x, T y, T z);

    /**
     * Normalize a two-vector.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in,out] x on output set to x/hypot(x, y).
     * @param[in,out] y on output set to y/hypot(x, y).
     **********************************************************************/
    template<typename T> static void norm(T& x, T& y)
    { T h = hypot(x, y); x /= h; y /= h; }

    /**
     * The error-free sum of two numbers.
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] u
     * @param[in] v
     * @param[out] t the exact error given by (\e u + \e v) - \e s.
     * @return \e s = round(\e u + \e v).
     *
     * See D. E. Knuth, TAOCP, Vol 2, 4.2.2, Theorem B.  (Note that \e t can be
     * the same as one of the first two arguments.)
     **********************************************************************/
    template<typename T> static T sum(T u, T v, T& t);

    /**
     * Evaluate a polynomial.
     *
     * @tparam T the type of the arguments and returned value.
     * @param[in] N the order of the polynomial.
     * @param[in] p the coefficient array (of size \e N + 1).
     * @param[in] x the variable.
     * @return the value of the polynomial.
     *
     * Evaluate y = ∑n=0..N
     * pn xN−n.
     * Return 0 if \e N < 0.  Return p0, if \e N = 0 (even
     * if \e x is infinite or a nan).  The evaluation uses Horner's method.
     **********************************************************************/
    template<typename T> static T polyval(int N, const T p[], T x)
    // This used to employ Math::fma; but that's too slow and it seemed not to
    // improve the accuracy noticeably.  This might change when there's direct
    // hardware support for fma.
    { T y = N < 0 ? 0 : *p++; while (--N >= 0) y = y * x + *p++; return y; }

    /**
     * Normalize an angle.
     *
     * @tparam T the type of the argument and returned value.
     * @param[in] x the angle in degrees.
     * @return the angle reduced to the range (−180°, 180°].
     *
     * The range of \e x is unrestricted.
     **********************************************************************/
    template<typename T> static T AngNormalize(T x) {
      x = remainder(x, T(360)); return x != -180 ? x : 180;
    }

    /**
     * Normalize a latitude.
     *
     * @tparam T the type of the argument and returned value.
     * @param[in] x the angle in degrees.
     * @return x if it is in the range [−90°, 90°], otherwise
     *   return NaN.
     **********************************************************************/
    template<typename T> static T LatFix(T x)
    { using std::abs; return abs(x) > 90 ? NaN<T>() : x; }

    /**
     * The exact difference of two angles reduced to
     * (−180°, 180°].
     *
     * @tparam T the type of the arguments and returned value.
     * @param[in] x the first angle in degrees.
     * @param[in] y the second angle in degrees.
     * @param[out] e the error term in degrees.
     * @return \e d, the truncated value of \e y − \e x.
     *
     * This computes \e z = \e y − \e x exactly, reduced to
     * (−180°, 180°]; and then sets \e z = \e d + \e e where \e d
     * is the nearest representable number to \e z and \e e is the truncation
     * error.  If \e d = −180, then \e e > 0; If \e d = 180, then \e e
     * ≤ 0.
     **********************************************************************/
    template<typename T> static T AngDiff(T x, T y, T& e) {
      T t, d = AngNormalize(sum(remainder(-x, T(360)),
                                remainder( y, T(360)), t));
      // Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
      // abs(t) <= eps (eps = 2^-45 for doubles).  The only case where the
      // addition of t takes the result outside the range (-180,180] is d = 180
      // and t > 0.  The case, d = -180 + eps, t = -eps, can't happen, since
      // sum would have returned the exact result in such a case (i.e., given t
      // = 0).
      return sum(d == 180 && t > 0 ? -180 : d, t, e);
    }

    /**
     * Difference of two angles reduced to [−180°, 180°]
     *
     * @tparam T the type of the arguments and returned value.
     * @param[in] x the first angle in degrees.
     * @param[in] y the second angle in degrees.
     * @return \e y − \e x, reduced to the range [−180°,
     *   180°].
     *
     * The result is equivalent to computing the difference exactly, reducing
     * it to (−180°, 180°] and rounding the result.  Note that
     * this prescription allows −180° to be returned (e.g., if \e x
     * is tiny and negative and \e y = 180°).
     **********************************************************************/
    template<typename T> static T AngDiff(T x, T y)
    { T e; return AngDiff(x, y, e); }

    /**
     * Coarsen a value close to zero.
     *
     * @tparam T the type of the argument and returned value.
     * @param[in] x
     * @return the coarsened value.
     *
     * The makes the smallest gap in \e x = 1/16 − nextafter(1/16, 0) =
     * 1/257 for reals = 0.7 pm on the earth if \e x is an angle in
     * degrees.  (This is about 1000 times more resolution than we get with
     * angles around 90°.)  We use this to avoid having to deal with near
     * singular cases when \e x is non-zero but tiny (e.g.,
     * 10−200).  This converts −0 to +0; however tiny
     * negative numbers get converted to −0.
     **********************************************************************/
    template<typename T> static T AngRound(T x);

    /**
     * Evaluate the sine and cosine function with the argument in degrees
     *
     * @tparam T the type of the arguments.
     * @param[in] x in degrees.
     * @param[out] sinx sin(x).
     * @param[out] cosx cos(x).
     *
     * The results obey exactly the elementary properties of the trigonometric
     * functions, e.g., sin 9° = cos 81° = − sin 123456789°.
     * If x = −0, then \e sinx = −0; this is the only case where
     * −0 is returned.
     **********************************************************************/
    template<typename T> static void sincosd(T x, T& sinx, T& cosx);

    /**
     * Evaluate the sine function with the argument in degrees
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x in degrees.
     * @return sin(x).
     **********************************************************************/
    template<typename T> static T sind(T x);

    /**
     * Evaluate the cosine function with the argument in degrees
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x in degrees.
     * @return cos(x).
     **********************************************************************/
    template<typename T> static T cosd(T x);

    /**
     * Evaluate the tangent function with the argument in degrees
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x in degrees.
     * @return tan(x).
     *
     * If \e x = ±90°, then a suitably large (but finite) value is
     * returned.
     **********************************************************************/
    template<typename T> static T tand(T x);

    /**
     * Evaluate the atan2 function with the result in degrees
     *
     * @tparam T the type of the arguments and the returned value.
     * @param[in] y
     * @param[in] x
     * @return atan2(y, x) in degrees.
     *
     * The result is in the range (−180° 180°].  N.B.,
     * atan2d(±0, −1) = +180°; atan2d(−ε,
     * −1) = −180°, for ε positive and tiny;
     * atan2d(±0, +1) = ±0°.
     **********************************************************************/
    template<typename T> static T atan2d(T y, T x);

    /**
     * Evaluate the atan function with the result in degrees
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return atan(x) in degrees.
     **********************************************************************/
   template<typename T> static T atand(T x);

    /**
     * Evaluate e atanh(e x)
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @param[in] es the signed eccentricity =  sign(e2)
     *    sqrt(|e2|)
     * @return e atanh(e x)
     *
     * If e2 is negative (e is imaginary), the
     * expression is evaluated in terms of atan.
     **********************************************************************/
    template<typename T> static T eatanhe(T x, T es);

    /**
     * tanχ in terms of tanφ
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] tau τ = tanφ
     * @param[in] es the signed eccentricity = sign(e2)
     *   sqrt(|e2|)
     * @return τ′ = tanχ
     *
     * See Eqs. (7--9) of
     * C. F. F. Karney,
     *
     * Transverse Mercator with an accuracy of a few nanometers,
     * J. Geodesy 85(8), 475--485 (Aug. 2011)
     * (preprint
     * arXiv:1002.1417).
     **********************************************************************/
    template<typename T> static T taupf(T tau, T es);

    /**
     * tanφ in terms of tanχ
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] taup τ′ = tanχ
     * @param[in] es the signed eccentricity = sign(e2)
     *   sqrt(|e2|)
     * @return τ = tanφ
     *
     * See Eqs. (19--21) of
     * C. F. F. Karney,
     *
     * Transverse Mercator with an accuracy of a few nanometers,
     * J. Geodesy 85(8), 475--485 (Aug. 2011)
     * (preprint
     * arXiv:1002.1417).
     **********************************************************************/
    template<typename T> static T tauf(T taup, T es);

    /**
     * Test for finiteness.
     *
     * @tparam T the type of the argument.
     * @param[in] x
     * @return true if number is finite, false if NaN or infinite.
     **********************************************************************/
    template<typename T> static bool isfinite(T x);

    /**
     * The NaN (not a number)
     *
     * @tparam T the type of the returned value.
     * @return NaN if available, otherwise return the max real of type T.
     **********************************************************************/
    template<typename T> static T NaN();

    /**
     * A synonym for NaN<real>().
     **********************************************************************/
    static real NaN() { return NaN<real>(); }

    /**
     * Test for NaN.
     *
     * @tparam T the type of the argument.
     * @param[in] x
     * @return true if argument is a NaN.
     **********************************************************************/
    template<typename T> static bool isnan(T x);

    /**
     * Infinity
     *
     * @tparam T the type of the returned value.
     * @return infinity if available, otherwise return the max real.
     **********************************************************************/
    template<typename T> static T infinity();

    /**
     * A synonym for infinity<real>().
     **********************************************************************/
    static real infinity() { return infinity<real>(); }

    /**
     * Swap the bytes of a quantity
     *
     * @tparam T the type of the argument and the returned value.
     * @param[in] x
     * @return x with its bytes swapped.
     **********************************************************************/
    template<typename T> static T swab(T x) {
      union {
        T r;
        unsigned char c[sizeof(T)];
      } b;
      b.r = x;
      for (int i = sizeof(T)/2; i--; )
        std::swap(b.c[i], b.c[sizeof(T) - 1 - i]);
      return b.r;
    }

  };

} // namespace GeographicLib

#endif  // GEOGRAPHICLIB_MATH_HPP


// File: insert.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

template <typename Matrix>
void fill(Matrix& m)
{
    // Matrices are not initialized by default
    m= 0.0;

    // Create inserter for matrix m
    mat::inserter<Matrix> ins(m);

    // Insert value in m[0][0]
    ins[0][0] << 2.0;
    ins[1][2] << 0.5;
    ins[2][1] << 3.0;

    // Destructor of ins sets final state of m
}

template <typename Matrix>
void modify(Matrix& m)
{
    // Type of m's elements
    typedef typename Collection<Matrix>::value_type value_type;

    // Create inserter for matrix m
    // Existing values are not overwritten but inserted
    mat::inserter<Matrix, update_plus<value_type> > ins(m, 3);

    // Increment value in m[0][0]
    ins[0][0] << 1.0;

    // Elements that doesn't exist (in sparse matrices) are inserted
    ins[1][1] << 2.5;
    ins[2][1] << 1.0;
    ins[2][2] << 4.0;

    // Destructor of ins sets final state of m
}

int main(int, char**)
{
    // Matrices of different types
    compressed2D<double>              A(3, 3);
    dense2D<double>                   B(3, 3);
    morton_dense<float, morton_mask>  C(3, 3);

    // Fill the matrices generically
    fill(A); fill(B); fill(C);
    std::cout << "A is \n" << A << "\nB is \n" << B << "\nC is \n" << C;

    // Modify the matrices generically
    modify(A); modify(B); modify(C);
    std::cout << "\n\nAfter modification:\nA is \n" << A
	      << "\nB is \n" << B << "\nC is \n" << C;

    return 0;
}


//  Copyright Thijs van den Berg, 2008.
//  Copyright John Maddock 2008.
//  Copyright Paul A. Bristow 2008.

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

// This module implements the Laplace distribution.
// Weisstein, Eric W. "Laplace Distribution." From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/LaplaceDistribution.html
// http://en.wikipedia.org/wiki/Laplace_distribution
//
// Abramowitz and Stegun 1972, p 930
// http://www.math.sfu.ca/~cbm/aands/page_930.htm

#ifndef BOOST_STATS_LAPLACE_HPP
#define BOOST_STATS_LAPLACE_HPP

#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/constants/constants.hpp>
#include <limits>

namespace boost{ namespace math{

template <class RealType = double, class Policy = policies::policy<> >
class laplace_distribution
{
public:
   // ----------------------------------
   // public Types
   // ----------------------------------
   typedef RealType value_type;
   typedef Policy policy_type;

   // ----------------------------------
   // Constructor(s)
   // ----------------------------------
   laplace_distribution(RealType location = 0, RealType scale = 1)
      : m_location(location), m_scale(scale)
   {
      RealType result;
      check_parameters("boost::math::laplace_distribution<%1%>::laplace_distribution()", &result);
   }

   // ----------------------------------
   // Public functions
   // ----------------------------------

   RealType location() const
   {
      return m_location;
   }

   RealType scale() const
   {
      return m_scale;
   }

   bool check_parameters(const char* function, RealType* result) const
   {
         if(false == detail::check_scale(function, m_scale, result, Policy())) return false;
         if(false == detail::check_location(function, m_location, result, Policy())) return false;
         return true;
   }

private:
   RealType m_location;
   RealType m_scale;

}; // class laplace_distribution

//
// Convenient type synonym
//
typedef laplace_distribution<double> laplace;

//
// Non member functions
//
template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const laplace_distribution<RealType, Policy>&)
{
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const laplace_distribution<RealType, Policy>&)
{
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(),  max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const laplace_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   // Checking function argument
   RealType result;
   const char* function = "boost::math::pdf(const laplace_distribution<%1%>&, %1%))";
   if (false == dist.check_parameters(function, &result)) return result;
   if (false == detail::check_x(function, x, &result, Policy())) return result;

   // Special pdf values
   if((boost::math::isinf)(x))
      return 0; // pdf + and - infinity is zero.

   // General case
   RealType scale( dist.scale() );
   RealType location( dist.location() );

   RealType exponent = x - location;
   if (exponent>0) exponent = -exponent;
   exponent /= scale;

   result = exp(exponent);
   result /= 2 * scale;

   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const laplace_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   // Checking function argument
   RealType result;
   const char* function = "boost::math::cdf(const laplace_distribution<%1%>&, %1%)";
   if (false == dist.check_parameters(function, &result)) return result;
   if (false == detail::check_x(function, x, &result, Policy())) return result;

   // Special cdf values
   if((boost::math::isinf)(x))
   {
     if(x < 0) return 0; // -infinity
     return 1; // + infinity
   }

   // General cdf  values
   RealType scale( dist.scale() );
   RealType location( dist.location() );

   if (x < location)
      result = exp( (x-location)/scale )/2;
   else
      result = 1 - exp( (location-x)/scale )/2;

   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const laplace_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   // Checking function argument
   RealType result;
   const char* function = "boost::math::quantile(const laplace_distribution<%1%>&, %1%)";
   if (false == dist.check_parameters(function, &result)) return result;
   if(false == detail::check_probability(function, p, &result, Policy())) return result;

   // extreme values
   if(p == 0) return -std::numeric_limits<RealType>::infinity();
   if(p == 1) return std::numeric_limits<RealType>::infinity();

   // Calculate Quantile
   RealType scale( dist.scale() );
   RealType location( dist.location() );

   if (p - 0.5 < 0.0)
      result = location + scale*log( static_cast<RealType>(p*2) );
   else
      result = location - scale*log( static_cast<RealType>(-p*2 + 2) );

   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<laplace_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   RealType scale = c.dist.scale();
   RealType location = c.dist.location();
   RealType x = c.param;

   // Checking function argument
   RealType result;
   const char* function = "boost::math::cdf(const complemented2_type<laplace_distribution<%1%>, %1%>&)";
   if(false == detail::check_x(function, x, &result, Policy()))return result;

   // Calculate cdf

   // Special cdf value
   if((boost::math::isinf)(x))
   {
     if(x < 0) return 1; // cdf complement -infinity is unity.
     return 0; // cdf complement +infinity is zero
   }

   // Cdf interval value
   if (-x < location)
      result = exp( (-x-location)/scale )/2;
   else
      result = 1 - exp( (location+x)/scale )/2;

   return result;
} // cdf complement

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<laplace_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   // Calculate quantile
   RealType scale = c.dist.scale();
   RealType location = c.dist.location();
   RealType q = c.param;

   // Checking function argument
   RealType result;
   const char* function = "quantile(const complemented2_type<laplace_distribution<%1%>, %1%>&)";
   if(false == detail::check_probability(function, q, &result, Policy())) return result;

   // extreme values
   if(q == 0) return std::numeric_limits<RealType>::infinity();
   if(q == 1) return -std::numeric_limits<RealType>::infinity();

   if (0.5 - q < 0.0)
      result = location + scale*log( static_cast<RealType>(-q*2 + 2) );
   else
      result = location - scale*log( static_cast<RealType>(q*2) );

   return result;
} // quantile

template <class RealType, class Policy>
inline RealType mean(const laplace_distribution<RealType, Policy>& dist)
{
   return dist.location();
}

template <class RealType, class Policy>
inline RealType standard_deviation(const laplace_distribution<RealType, Policy>& dist)
{
   return constants::root_two<RealType>() * dist.scale();
}

template <class RealType, class Policy>
inline RealType mode(const laplace_distribution<RealType, Policy>& dist)
{
   return dist.location();
}

template <class RealType, class Policy>
inline RealType median(const laplace_distribution<RealType, Policy>& dist)
{
   return dist.location();
}

template <class RealType, class Policy>
inline RealType skewness(const laplace_distribution<RealType, Policy>& /*dist*/)
{
   return 0;
}

template <class RealType, class Policy>
inline RealType kurtosis(const laplace_distribution<RealType, Policy>& /*dist*/)
{
   return 6;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const laplace_distribution<RealType, Policy>& /*dist*/)
{
   return 3;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_LAPLACE_HPP


/*
 * OptimalControlProblem.hpp
 *
 *  Created on: July 7, 2019
 *      Author: Quincy Jones
 *
 * Copyright (c) <2019> <Quincy Jones - quincy@implementedrobotics.com/>
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software
 * is furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#ifndef NOMAD_CORE_OPTIMALCONTROL_OPTIMALCONTROLPROBLEM_H_
#define NOMAD_CORE_OPTIMALCONTROL_OPTIMALCONTROLPROBLEM_H_

#include <iostream>
#include <Eigen/Dense>
#include <qpOASES.hpp>

namespace OptimalControl
{

    class OptimalControlProblem
    {

    public:
        // Base Class Optimal Control Problem
        // N = Prediction Steps
        // T = Horizon Length
        // num_states = Number of States of OCP
        // num_inputs = Number of Inputs of OCP
        OptimalControlProblem(const unsigned int N, const double T, const unsigned int num_states, const unsigned int num_inputs, const unsigned int max_iterations = 1000);

        // Solve
        virtual void Solve() = 0;

        // Get System State Trajectory Vector
        // TODO: Compute this depending on U solutions.  X_ = Ax+Bu
        // TODO: Make pure virtual
        virtual Eigen::MatrixXd X() const { return X_; }

        //Get Current Input Sequence Solution
        Eigen::MatrixXd U() const { return U_; }

        // Set Weight Matrices
        virtual void SetWeights(const Eigen::VectorXd &Q, const Eigen::VectorXd &R)
        {
            // TODO: Verify Vector Size Matches correct state and inputs
            Q_ = Q.matrix().asDiagonal().toDenseMatrix();
            R_ = R.matrix().asDiagonal().toDenseMatrix();
        }

        // Set Problem Initial Condition
        void SetInitialCondition(const Eigen::VectorXd &x_0) { x_0_ = x_0; }

        // Set Reference Trajectory
        void SetReference(const Eigen::MatrixXd &X_ref) { X_ref_ = X_ref; }

        // Time step sample time
        double SampleTime() const { return T_s_; }

        // Prediction Horizon Steps
        int N() const { return N_; }

    protected:
        Eigen::VectorXd x_0_;   // Current State/Initial Condition
        Eigen::MatrixXd X_ref_; // Reference Trajectory

        Eigen::MatrixXd X_; // System State Trajectory
        Eigen::MatrixXd U_; // Optimal Control Input Sequence Solution

        Eigen::MatrixXd Q_; // State Weights
        Eigen::MatrixXd R_; // Input Weights

        int num_states_; // Number of System States
        int num_inputs_; // Number of System Inputs

        int N_; // Number of Prediction Steps

        double T_s_; // Sample Time
        double T_;   // Horizon Length

        qpOASES::int_t max_iterations_;    // Max Iterations for Solver
        qpOASES::int_t solver_iterations_; // Total number of Solver iterations for solution
        double solver_time_;    // Total time for Solver to compute a solution

        bool solved_;
        // TODO:
        // Infeasible, BlahBlah
    };
} // namespace OptimalControl

namespace OptimalControl
{
    namespace LinearOptimalControl
    {
        class LinearOptimalControlProblem : public OptimalControlProblem
        {
        public:
            // Base Class Linear Optimal Control Problem
            // N = Prediction Steps
            // T = Horizon Length
            // num_states = Number of States of OCP
            // num_inputs = Number of Inputs of OCP
            // max_iterations = Maximum number of solver iterations
            LinearOptimalControlProblem(const unsigned int N,
                                        const double T,
                                        const unsigned int num_states,
                                        const unsigned int num_inputs,
                                        const unsigned int max_iterations = 1000);

            // Set Model Matrices
            void SetModelMatrices(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
            {
                A_ = A;
                B_ = B;
            }

            // Solve
            virtual void Solve();

        protected:
            Eigen::MatrixXd A_; // System State Transition Matrix
            Eigen::MatrixXd B_; // Control Input Matrix
        };
    } // namespace LinearOptimalControl
} // namespace OptimalControl

#endif // NOMAD_CORE_OPTIMALCONTROL_OPTIMALCONTROLPROBLEM_H_


#define NO_IMPORT_ARRAY

#include <complex>
#include <stdint.h>
#include <stdio.h>
#include <vector>
#include <string>
extern "C" {
  #include <cblas.h>
  // Additional prototypes for Fortran LAPACK routines.
  // sposv: solve Ax = b for A positive definite.
  void sposv_(const char* uplo, int* n, int* nrhs, float* a, int* lda,
              float* b, int* ldb, int* info );
}

#include <boost/python.hpp>

#include <pybindings.h>
#include "so3g_numpy.h"
#include "numpy_assist.h"
#include "Ranges.h"

// TODO: Generalize to double precision too.
// This implements Jon's noise model for ACT. It takes in
// * ft[ndet,nfreq]        the fourier transform of the time-ordered data
// * bins[nbin,{from,to}]  the start and end of each bin
// * iD[nbin,ndet]         the inverse uncorrelated variance for each detector per bin
// * iV[nbin,ndet,nvec]    matrix representing the scaled eivenvectors per bin
void nmat_detvecs_apply(const bp::object & ft, const bp::object & bins, const bp::object & iD, const bp::object & iV, float s, float norm) {
	// Should pass in this too
	BufferWrapper<float>               ft_buf  ("ft",   ft,   false, std::vector<int>{-1,-1});
	BufferWrapper<int32_t>             bins_buf("bins", bins, false, std::vector<int>{-1, 2});
	int ndet = ft_buf->shape[0], nmode = ft_buf->shape[1], nbin = bins_buf->shape[0];
	BufferWrapper<float>               iD_buf  ("iD",   iD,   false, std::vector<int>{nbin,ndet});
	BufferWrapper<float>               iV_buf  ("iV",   iV,   false, std::vector<int>{nbin,ndet,-1});
	int nvec = iV_buf->shape[2];
	if (ft_buf->strides[1] != ft_buf->itemsize || ft_buf->strides[0] != ft_buf->itemsize*nmode)
		throw buffer_exception("ft must be C-contiguous along last axis");
	if (bins_buf->strides[1] != bins_buf->itemsize || bins_buf->strides[0] != bins_buf->itemsize*2)
		throw buffer_exception("bins must be C-contiguous along last axis");
	if (iD_buf->strides[1] != iD_buf->itemsize || iD_buf->strides[0] != iD_buf->itemsize*ndet)
		throw buffer_exception("iD must be C-contiguous along last axis");
	if (iV_buf->strides[2] != iV_buf->itemsize || iV_buf->strides[1] != iV_buf->itemsize*nvec || iV_buf->strides[0] != iV_buf->itemsize*nvec*ndet)
		throw buffer_exception("iV must be C-contiguous along last axis");
	// Internally we work with a real view of ft, with twice as many elements to compensate
	//int nmode = 2*nfreq;
	float   * ft_   = (float*)   ft_buf->buf;
	int32_t * bins_ = (int32_t*) bins_buf->buf;
	float   * iD_   = (float*)   iD_buf->buf;
	float   * iV_   = (float*)   iV_buf->buf;

	// Ok, actually do the work
	for(int bi = 0; bi < nbin; bi++) {
		int b1 = min(2*bins_[2*bi+0],nmode-1);
		int b2 = min(2*bins_[2*bi+1],nmode);
		int nm = b2-b1;
		float * biD = iD_ + bi*ndet;
		float * biV = iV_ + bi*ndet*nvec;

		// what I want to do
		// ft    = ftod[:,b[0]:b[1]]
		// iD    = self.iD[bi]/norm
		// iV    = self.iV[bi]/norm**0.5
		// ft[:] = iD[:,None]*ft + self.s*iV.dot(iV.T.dot(ft))
		// So first do iV.T [nvec,ndet] dot ft [ndet,nm] -> Q [nvec,nm]
		float * Q = new float[nvec*nm];
		cblas_sgemm(CblasRowMajor, CblasTrans, CblasNoTrans, nvec, nm, ndet, 1.0f, biV, nvec, ft_+b1, nmode, 0.0f, Q, nm);
		// Handle the uncorrelated part
		//#pragma omp parallel for
		for(int di = 0; di < ndet; di++)
			for(int i = b1; i < b2; i++)
				ft_[di*nmode+i] *= biD[di]/norm;
		// Do ft += s*iV[ndet,nvec] dot Q [nvec,nm]
		cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, ndet, nm, nvec, s/norm, biV, nvec, Q, nm, 1.0f, ft_+b1, nmode);
		delete [] Q;
	}
}

// Support of maximum-liklihood sample cut handling. This got a bit long, so it should
// probably be moved into its own file.

// Forward declarations of helper functions
int get_dtype(const bp::object &);
int pcut_full_measure_helper(const vector<RangesInt32> &);
template <typename T> void pcut_full_tod2vals_helper(const vector<RangesInt32> &, T *, int, int, T *);
template <typename T> void pcut_full_vals2tod_helper(const vector<RangesInt32> &, T *, int, int, T *);
int pcut_poly_measure_helper(const vector<RangesInt32> &, int, int nmax);
template <typename T> void pcut_poly_tod2vals_helper(const vector<RangesInt32> &, int, int, T *, int, int, T *);
template <typename T> void pcut_poly_vals2tod_helper(const vector<RangesInt32> &, int, int, T *, int, int, T *);
template <typename T> void pcut_clear_helper(const vector<RangesInt32> &, T *, int, int);

// The main cuts processing function. In Maximum-likelihood map-making cuts are handled
// as special degrees of freedom, and are part of the pointing matrix. This function
// provides the operations corresponding to the cuts part of the pointing matrix P and
// P'. In particular, operation == "insert" corresponds to the model-to-data matrix P,
// while operation == "extract" corresponds to the data-to-model matrix P'. Additionally,
// we provide operation == "measure", which doesn't touch any of the arrays, and just returns
// how long the vals argument should be.
//
// Note that "extract" does not in general estimate the coefficients of the model. It just does
// P'd, while the coefficients would (in the absence of any sky signal etc) be (P'P)'P'd.
//
// Arguments:
//  range_matrix: the .ranges member of a python RangesMatrix
//  operation: "measure", "insert" or "extract"
//  model: The type of model used
//   * "full": One degree of freedom per sample
//   * "poly": Model is a legendre polynomial in each range.
//        Order of poly determined by params["resolution"] (samples per order) and params["nmax"] (max order)
//  tod:  numpy array with shape [ndet,nsamp] of dtype float32 or float64
//  vals: numpy array with shape [:] of same dtype as tod. Holds the model degrees of freedom.
//
// TODO: To be able to process cuts in parallel, we need a lookup table for where in vals each
// cut range starts. This will be fast enough to build on the fly. Would pass this as an extra
// argument to the helper functions.

int process_cuts(const bp::object & range_matrix, const std::string & operation, const std::string & model, const bp::dict & params, const bp::object & tod, const bp::object & vals) {
	auto ranges = extract_ranges<int32_t>(range_matrix);
	// Decoding these up here lets us avoid some duplication later
	int resolution, nmax;
	if     (model == "full") {}
	else if(model == "poly") {
		resolution = bp::extract<int>(params.get("resolution"));
		nmax       = bp::extract<int>(params.get("nmax"));
	} else throw general_exception("process_cuts model can only be 'full' or 'poly'");

	if(operation == "measure") {
		if     (model == "full") return pcut_full_measure_helper(ranges);
		else if(model == "poly") return pcut_poly_measure_helper(ranges, resolution, nmax);
	} else {
		int dtype = get_dtype(tod);
		if(dtype == NPY_FLOAT) {
			BufferWrapper<float> tod_buf  ("tod",  tod,  false, std::vector<int>{-1,-1});
			BufferWrapper<float> vals_buf ("vals", vals, false, std::vector<int>{-1});
			int ndet = tod_buf->shape[0], nsamp = tod_buf->shape[1];
			if(operation == "insert") {
				if     (model == "full")
					pcut_full_vals2tod_helper(ranges, (float*)tod_buf->buf, ndet, nsamp, (float*) vals_buf->buf);
				else if(model == "poly")
					pcut_poly_vals2tod_helper(ranges, resolution, nmax, (float*)tod_buf->buf, ndet, nsamp, (float*) vals_buf->buf);
			} else if(operation == "extract") {
				if     (model == "full")
					pcut_full_tod2vals_helper(ranges, (float*)tod_buf->buf, ndet, nsamp, (float*) vals_buf->buf);
				else if(model == "poly")
					pcut_poly_tod2vals_helper(ranges, resolution, nmax, (float*)tod_buf->buf, ndet, nsamp, (float*) vals_buf->buf);
			} else if(operation == "clear") {
				pcut_clear_helper(ranges, (float*)tod_buf->buf, ndet, nsamp);
			} else throw general_exception("process_cuts operation can only be 'measure', 'insert' or 'extract'");
		} else if(dtype == NPY_DOUBLE) {
			BufferWrapper<double> tod_buf  ("tod",  tod,  false, std::vector<int>{-1,-1});
			BufferWrapper<double> vals_buf ("vals", vals, false, std::vector<int>{-1});
			int ndet = tod_buf->shape[0], nsamp = tod_buf->shape[1];
			if(operation == "insert") {
				if     (model == "full")
					pcut_full_vals2tod_helper(ranges, (double*)tod_buf->buf, ndet, nsamp, (double*) vals_buf->buf);
				else if(model == "poly")
					pcut_poly_vals2tod_helper(ranges, resolution, nmax, (double*)tod_buf->buf, ndet, nsamp, (double*) vals_buf->buf);
			} else if(operation == "extract") {
				if     (model == "full")
					pcut_full_tod2vals_helper(ranges, (double*)tod_buf->buf, ndet, nsamp, (double*) vals_buf->buf);
				else if(model == "poly")
					pcut_poly_tod2vals_helper(ranges, resolution, nmax, (double*)tod_buf->buf, ndet, nsamp, (double*) vals_buf->buf);
			} else if(operation == "clear") {
				pcut_clear_helper(ranges, (float*)tod_buf->buf, ndet, nsamp);
			} else throw general_exception("process_cuts operation can only be 'measure', 'insert' or 'extract'");
		} else throw general_exception("process_cuts only supports float32 and float64");
	}
	return 0;
}

// Helpers for the cuts

int get_dtype(const bp::object & arr) {
	PyObject *ob = PyArray_FromAny(arr.ptr(), NULL, 0, 0, 0, NULL);
	if (ob == NULL) throw exception();
	PyArrayObject * a = reinterpret_cast<PyArrayObject*>(ob);
	int res = PyArray_TYPE(a);
	Py_DECREF(a);
	return PyArray_TYPE(a);
}

// This is all from Jon's work for ACT.
int get_npoly(int gapsize, int resolution, int nmax) {
	if(nmax < 1) nmax = 1;
	if     (gapsize <=  1) return min<int>(1, nmax);
	else if(gapsize <=  3) return min<int>(2, nmax);
	else if(gapsize <=  6) return min<int>(3, nmax);
	else if(gapsize <= 20) return min<int>(4, nmax);
	else return min<int>(5 + gapsize/resolution, nmax);
}

// Full cut treatment, with one degree of freedom for each sample in the cut
int pcut_full_measure_helper(const vector<RangesInt32> & rangemat) {
	int n = 0;
	for(int di = 0; di < rangemat.size(); di++)
		for (auto const &r: rangemat[di].segments)
			n += r.second-r.first;
	return n;
}
template <typename T>
void pcut_full_tod2vals_helper(const vector<RangesInt32> & rangemat, T * tod, int ndet, int nsamp, T * vals) {
	int i = 0;
	for(int di = 0; di < rangemat.size(); di++)
		for (auto const &r: rangemat[di].segments)
			for(int j = r.first; j < r.second; j++, i++)
				vals[i] = tod[di*nsamp+j];
}
template <typename T>
void pcut_full_vals2tod_helper(const vector<RangesInt32> & rangemat, T * tod, int ndet, int nsamp, T * vals) {
	int i = 0;
	for(int di = 0; di < rangemat.size(); di++)
		for (auto const &r: rangemat[di].segments)
			for(int j = r.first; j < r.second; j++, i++)
				tod[di*nsamp+j] = vals[i];
}

// Polynomial cut treatment
int pcut_poly_measure_helper(const vector<RangesInt32> & rangemat, int resolution, int nmax) {
	int n = 0;
	for(int di = 0; di < rangemat.size(); di++)
		for (auto const &r: rangemat[di].segments)
			n += get_npoly(r.second-r.first, resolution, nmax);
	return n;
}
template <typename T>
void pcut_poly_tod2vals_helper(const vector<RangesInt32> & rangemat, int resolution, int nmax, T * tod, int ndet, int nsamp, T * vals) {
	int i = 0;
	for(int di = 0; di < rangemat.size(); di++) {
		for (auto const &r: rangemat[di].segments) {
			int np = get_npoly(r.second-r.first, resolution, nmax);
			if(np <= 1) {
				for(int s = r.first; s < r.second; s++)
					vals[i] += tod[di*nsamp+s];
				i++;
			} else {
				for(int p = 0; p < np; p++) vals[i+p] = 0;
				for(int s = r.first; s < r.second; s++) {
					T x = -1 + 2*(s-r.first)/T(r.second-r.first-1);
					T t = tod[di*nsamp+s];
					vals[i] += t;
					if(np > 1) vals[i+1] += t*x;
					if(np > 2) {
						T Pa = x, Pb = 1, Pc = 0;
						for(int p = 2; p < np; p++) {
							Pc = Pb; Pb = Pa; Pa = ((2*p-1)*x*Pb-(p-1)*Pc)/p;
							vals[i+p] += t*Pa;
						}
					}
					i += np;
				}
			}
		}
	}
}
template <typename T>
void pcut_poly_vals2tod_helper(const vector<RangesInt32> & rangemat, int resolution, int nmax, T * tod, int ndet, int nsamp, T * vals) {
	int i = 0;
	for(int di = 0; di < rangemat.size(); di++) {
		for (auto const &r: rangemat[di].segments) {
			int np = get_npoly(r.second-r.first, resolution, nmax);
			if(np <= 1) {
				for(int s = r.first; s < r.second; s++)
					tod[di*nsamp+s] = vals[i];
				i++;
			} else {
				for(int s = r.first; s < r.second; s++) {
					T x = -1 + 2*(s-r.first)/T(r.second-r.first-1);
					T t = vals[i];
					if(np > 1) t += x*vals[i+1];
					if(np > 2) {
						T Pa = x, Pb = 1, Pc = 0;
						for(int p = 2; p < np; p++) {
							Pc = Pb; Pb = Pa; Pa = ((2*p-1)*x*Pb-(p-1)*Pc)/p;
							t += Pa*vals[i+p];
						}
					}
					tod[di*nsamp+s] = t;
					i += np;
				}
			}
		}
	}
}
template <typename T>
void pcut_clear_helper(const vector<RangesInt32> & rangemat, T * tod, int ndet, int nsamp) {
	#pragma omp parallel for
	for(int di = 0; di < rangemat.size(); di++)
		for (auto const &r: rangemat[di].segments)
			for(int s = r.first; s < r.second; s++)
				tod[di*nsamp+s] = 0;
}

// get_gap_fill_poly_single
//
// Single detector processor for get_gap_fill_poly.  Fit polynomials
// to each stretch of data in "gaps".  Requires square matrix a and
// vector b to be preallocated for ncoeff.
//
// The arguments (inplace, extract) behave like this:
//
//   (true,  valid):   The data are replaced with the model, and the
//                     replaced samples are copied into *extract.
//   (false, valid):   The data are left unchanged and the model
//                     values are placed in *extract.
//   (true,  nullptr): The data are replaced with the model, and
//                     the original data samples are discarded.
//   (false, nullptr): The data is left unchanged and the model
//                     is discarded (probably not what you want...).
//
// When extract is non-null, it needs to be the right size... this
// should be checked by get_gap_fill_poly.

void get_gap_fill_poly_single(const RangesInt32 &gaps, float *data,
                             float *a, float *b,
                             int buffer, int ncoeff,
                             bool inplace, float *extract)
{
        // Generate Ranges corresponding to samples near the edges of
        // intervals we want to fill.
        RangesInt32 rsegs = gaps.buffered((int32_t)buffer);
        rsegs.intersect(gaps.complement());

        // We are guaranteed that there is one or two rseg intervals
        // between each of our gaps, and zero or one rseg intervals
        // before the first gap and after the second gap.

        // Index of the rseg we're working with.
        int model_i = 0;

        for (auto const &gap: gaps.segments) {
                // std::cout << "GAP\n";
                int contrib_samps = 0;
                int contrib_segs = 0;
                memset(a, 0, ncoeff*ncoeff*sizeof(*a));
                memset(b, 0, ncoeff*sizeof(*b));
                float x0 = gap.first;

                for (; model_i < rsegs.segments.size(); model_i++) {
                        // Advance until just before this gap.
                        auto const &ival = rsegs.segments[model_i];
                        if (ival.second + 1 < gap.first)
                                continue;
                        // Include this interval in the fit.
                        for (int i=ival.first; i<ival.second; i++) {
                                float xx = 1.;
                                float yxx = data[i];
                                float x = i - x0;
                                for (int j=0; j<ncoeff; j++) {
                                        b[j] += yxx;
                                        yxx *= x;
                                        a[j] += xx;
                                        xx *= x;
                                }
                                // Now down...
                                for (int k=2; k<ncoeff+1; k++) {
                                        a[k*ncoeff - 1] += xx;
                                        xx *= x;
                                }
                        }
                        contrib_samps += ival.second - ival.first;
                        contrib_segs += 1;
                        // If this was the right side interval, bail
                        // now (so we can possibly re-use this
                        // interval for next gap).
                        if (ival.first > gap.first)
                                break;
                }

                // Restrict order based on number of contributing samples.
                int n_keep = std::min(contrib_samps / 10 + 1, ncoeff);
                if (contrib_samps > 0) {
                        // Fill in a's interior.
                        for (int r=1; r<ncoeff; r++)
                                for (int c=0; c<ncoeff-1; c++)
                                        a[r*ncoeff + c] = a[r*ncoeff + c - ncoeff + 1];

                        // Re-organize a if the order has changed...
                        if (n_keep < ncoeff) {
                                for (int r=1; r<n_keep; r++)
                                        for (int c=0; c<n_keep; c++)
                                                a[r*n_keep+c] = a[r*ncoeff+c];
                        }

                        // Solve the system...
                        int one = 1;
                        int err = 0;
                        sposv_("Upper", &n_keep, &one, a, &n_keep, b, &n_keep, &err);
                }
                float *write_to = nullptr;
                float *save_data = nullptr;
                if (inplace) {
			// Copy original data to extract, write model to data.
			save_data = extract;
			write_to = data + gap.first;
                } else {
			// Write results to extract, do not touch data.
			write_to = extract;
                }

                if (save_data != nullptr) {
			for (int i=gap.first; i<gap.second; i++, save_data++)
				*save_data = data[i];
                }
                if (write_to != nullptr) {
			for (int i=gap.first; i<gap.second; i++, write_to++) {
				float xx = 1.;
				*write_to = 0.;
				for (int j=0; j<n_keep; j++) {
					*write_to += xx * b[j];
					xx *= (i - gap.first);
				}
			}
                }
                if (extract != nullptr)
			extract += (gap.second - gap.first);
        }
}

void get_gap_fill_poly(const bp::object ranges,
		       const bp::object tod,
		       int buffer,
		       int order,
		       bool inplace,
		       const bp::object ex)
{
        // As a test, copy data from rangemat into segment.
	auto rangemat = extract_ranges<int32_t>(ranges);
        int ndet = rangemat.size();

        BufferWrapper<float> tod_buf  ("tod",  tod,  false, std::vector<int>{ndet,-1});
        int nsamp = tod_buf->shape[1];

        int ncoeff = order + 1; // Let us not speak of order again.
        float *a = (float*)malloc(ncoeff*(ncoeff+1)*sizeof(*a));
        float *b = a + ncoeff*ncoeff;

        float *ex_data = nullptr;
        std::vector<int> ex_offsets;

        if (ex.ptr() != Py_None) {
		// Compute offsets of each detector into ex.
		int n = 0;
		for (auto const &r: rangemat) {
			ex_offsets.push_back(n);
			for (auto const &r: r.segments)
				n += (r.second - r.first);
		}
		BufferWrapper<float> ex_buf("ex", ex, false, std::vector<int>{n});
		ex_data = (float*)ex_buf->buf;
        }

        for (int di=0; di < rangemat.size(); di++) {
                float* data = (float*)((char*)tod_buf->buf + di*tod_buf->strides[0]);
                float* _ex = ex_data;
                if (_ex != nullptr)
			_ex += ex_offsets[di];
                get_gap_fill_poly_single(rangemat[di], data, a, b, buffer, ncoeff,
					 inplace, _ex);
        }

        free(a);
}

PYBINDINGS("so3g")
{
	bp::def("nmat_detvecs_apply", nmat_detvecs_apply);
	bp::def("process_cuts",  process_cuts);
	bp::def("get_gap_fill_poly",  get_gap_fill_poly,
                "get_gap_fill_poly(ranges, signal, buffer, order, extract)\n"
                "\n"
                "Do polynomial gap-filling.\n"
                "\n"
                "Args:\n"
                "  ranges: RangesMatrix with shape (ndet, nsamp)\n"
                "  signal: data array with shape (ndet, nsamp)\n"
                "  buffer: integer stating max number of samples to use on each end\n"
                "  order: order of polynomial to use (1 means linear)\n"
		"  inplace: whether to overwrite data array with the model\n"
		"  extract: array to write the original data samples (inplace)\n"
		"    or the model (!inplace) into.\n");
}


#include "ahrs.hpp"

/* C/C++ Includes */
#include <stdint.h>
#include <stdlib.h>
#include <math.h>

/* Thor Includes */
#include "Thor/include/thor.h"
#include "Thor/include/spi.h"
#include "Thor/include/gpio.h"
#include "Thor/include/exceptions.h"

/* Boost Includes */
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>

/* FreeRTOS Includes */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

/* Project Includes */
#include "ahrs.hpp"
#include "LSM9DS1.hpp"
#include "config.hpp"
#include "dataTypes.hpp"
#include "threading.hpp"

/* Madgwick Filter */
#include "madgwick.hpp"

/* Kalman Filter */
#include "kalman/SquareRootUnscentedKalmanFilter.hpp"
#include "IMUModel.hpp"

namespace SOAR_AHRS
{
	typedef float T;
	typedef IMU::State<T> State;
	typedef IMU::Control<T> Control;
	typedef IMU::Measurement<T> Measurement;
	typedef IMU::SystemModel<T> SystemModel;
	typedef IMU::MeasurementModel<T> MeasurementModel;

	const int updateRate_mS = (1.0 / SENSOR_UPDATE_FREQ_HZ) * 1000.0;
	const int magMaxUpdateRate_mS = (1.0 / LSM9DS1_M_MAX_BW) * 1000.0;

	const float accelUncertainty = 0.8f;
	const float gyroUncertainty = 1.05f;

	void ahrsTask(void* argument)
	{
		#ifdef DEBUG
		volatile float pitch;
		volatile float roll;
		volatile float yaw;
		volatile float ax;
		volatile float ay;
		volatile float az;
		volatile float gx;
		volatile float gy;
		volatile float gz;
		volatile float mx;
		volatile float my;
		volatile float mz;
		volatile UBaseType_t stackHighWaterMark_AHRS = 0;
		volatile size_t bytesRemaining = xPortGetFreeHeapSize();
		#endif

		AHRSData_t ahrsData;					/* Output struct to push to the motor controller thread */

		/*----------------------------------
		* Initialize the UKF
		*----------------------------------*/
		State x, x_ukf;
		Control u;
		SystemModel sys;
		MeasurementModel om;
		Measurement meas;

		Eigen::Matrix<T, 6, 6> R;
		Eigen::Matrix<T, 6, 6> processNoise;

		Kalman::SquareRootUnscentedKalmanFilter<State> ukf(0.5f, 3.0f, 0.0f);

		x.setZero();
		u.setZero();
		R.setZero();

		T cnst = 5.00e-4f;
		T dnst = 3.33e-4f;
		T enst = 5.00e-4f;

		processNoise <<
			cnst, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
			0.0f, dnst, 0.0f, 0.0f, 0.0f, 0.0f,
			0.0f, 0.0f, enst, 0.0f, 0.0f, 0.0f,
			0.0f, 0.0f, 0.0f, cnst, 0.0f, 0.0f,
			0.0f, 0.0f, 0.0f, 0.0f, dnst, 0.0f,
			0.0f, 0.0f, 0.0f, 0.0f, 0.0f, enst;

		sys.setCovariance(processNoise);

		R(0, 0) = accelUncertainty * accelUncertainty;
		R(1, 1) = accelUncertainty * accelUncertainty;
		R(2, 2) = accelUncertainty * accelUncertainty;
		R(3, 3) = gyroUncertainty * gyroUncertainty;
		R(4, 4) = gyroUncertainty * gyroUncertainty;
		R(5, 5) = gyroUncertainty * gyroUncertainty;

		om.setCovariance(R);

		ukf.init(x);

		/*----------------------------------
		* Initialize the IMU
		*----------------------------------*/
		GPIOClass_sPtr lsm_ss_xg = boost::make_shared<GPIOClass>(GPIOC, PIN_4, ULTRA_SPD, NOALTERNATE);
		GPIOClass_sPtr lsm_ss_m = boost::make_shared<GPIOClass>(GPIOC, PIN_3, ULTRA_SPD, NOALTERNATE);
		SPIClass_sPtr lsm_spi = spi2;

		LSM9DS1 imu(lsm_spi, lsm_ss_xg, lsm_ss_m);

		/* Force halt of the device if the IMU cannot be reached */
		if (imu.begin() == 0)
			BasicErrorHandler("The IMU WHO_AM_I registers did not return valid readings");

		//TODO: Switch this out to print over the serial port

		imu.calibrate(true); /* "true" forces an automatic software subtraction of the calculated bias from all further data */
		imu.calibrateMag(true); /* "true" writes the offest into the mag sensor hardware for automatic subtraction in results */

		int count = 0;

		/*----------------------------------
		* Initialize the Madgwick Filter
		*----------------------------------*/
		float beta = 10.0;
		Eigen::Vector3f accel_raw, gyro_raw, mag_raw;
		Eigen::Vector3f accel_filtered, gyro_filtered, mag_filtered, eulerDeg;

		MadgwickFilter ahrs((AHRS_UPDATE_RATE_MULTIPLIER * SENSOR_UPDATE_FREQ_HZ), beta);

		float dt = (1.0f / SENSOR_UPDATE_FREQ_HZ);
		float tau_accel = 0.01f;
		float alpha_lp_accel = dt / tau_accel;

		/* Tell init task that this thread's initialization is done and ok to run.
		* Wait for init task to resume operation. */
		xTaskSendMessage(INIT_TASK, 1u);
		vTaskSuspend(NULL);
		taskYIELD();

		TickType_t lastTimeWoken = xTaskGetTickCount();
		for (;;)
		{
			#ifdef DEBUG
			stackHighWaterMark_AHRS = uxTaskGetStackHighWaterMark(NULL);
			bytesRemaining = xPortGetFreeHeapSize();
			#endif

			/*----------------------------
			* Sensor Reading
			*---------------------------*/
			/* Update Accel & Gyro Data at whatever frequency set by user. Max bandwidth on
			* chip is 952Hz which will saturate FreeRTOS if sampled that often.*/
			//taskENTER_CRITICAL();
			imu.readAccel();
			imu.readGyro();

			/* Update Mag Data at a max frequency set by LSM9DS1_M_MAX_BW (~75Hz) */
			#if (SENSOR_UPDATE_FREQ_HZ > LSM9DS1_M_MAX_BW)
			if (count > magMaxUpdateRate_mS)
			{
				imu.readMag();
				imu.calcMag();
				count = 0;
			}
			else
				count += updateRate_mS;
			#else
			imu.readMag();
			imu.calcMag();
			#endif
			//taskEXIT_CRITICAL();

			/* Convert raw data from chip into meaningful data */
			imu.calcAccel(); imu.calcGyro();

// 			#ifdef DEBUG
// 			ax = imu.aRaw[0];
// 			ay = imu.aRaw[1];
// 			az = imu.aRaw[2];
//
// 			gx = imu.gRaw[0];
// 			gy = imu.gRaw[1];
// 			gz = imu.gRaw[2];
//
// 			mx = imu.mRaw[0];
// 			my = imu.mRaw[1];
// 			mz = imu.mRaw[2];
// 			#endif

			accel_raw << imu.aRaw[0], imu.aRaw[1], imu.aRaw[2];
			gyro_raw << imu.gRaw[0], imu.gRaw[1], imu.gRaw[2];
			mag_raw <<
				-imu.mRaw[0],  //Align mag x with accel x
				-imu.mRaw[1],  //Align mag y with accel y
				-imu.mRaw[2];  //from LSM9DS1, mag z is opposite direction of accel z

			/*----------------------------
			* UKF Algorithm
			*---------------------------*/
			//Simulate the system
			x = sys.f(x, u);

			//Predict state for current time step
			x_ukf = ukf.predict(sys);

			//Take a measurement given system state
			meas << accel_raw, gyro_raw;

			//Update the state equation given measurement
			x_ukf = ukf.update(om, meas);

			accel_filtered << x_ukf.ax(), x_ukf.ay(), x_ukf.az();
			gyro_filtered << x_ukf.gx(), x_ukf.gy(), x_ukf.gz();
			mag_filtered = mag_raw;

			/*----------------------------
			* AHRS Algorithm
			*---------------------------*/
			/* The Madgwick filter needs to run between 3-5 times as fast IMU measurements
			* to achieve decent convergence to a stable value. This thread only runs when
			* new data has arrived from the IMU, so frequency multiplication is as simple
			* as looping 3-5 times here. */
			for (int i = 0; i < AHRS_UPDATE_RATE_MULTIPLIER; i++)
				ahrs.update(accel_filtered, gyro_filtered, mag_filtered);

			ahrs.getEulerDeg(eulerDeg);
			ahrsData(eulerDeg, accel_filtered, gyro_filtered, mag_filtered);

			#ifdef DEBUG
			pitch = eulerDeg(0);
			roll = eulerDeg(1);
			yaw = eulerDeg(2);

			ax = accel_filtered(0);
			ay = accel_filtered(1);
			az = accel_filtered(2);

			gx = gyro_filtered(0);
			gy = gyro_filtered(1);
			gz = gyro_filtered(2);

			mx = mag_filtered(0);
			my = mag_filtered(1);
			mz = mag_filtered(2);
			#endif

			/* Send data over to the Serial thread*/
			if (xSemaphoreTake(ahrsBufferMutex, 0) == pdPASS)
			{
				xQueueOverwrite(qAHRS, &ahrsData);
				xSemaphoreGive(ahrsBufferMutex);
			}

			vTaskDelayUntil(&lastTimeWoken, pdMS_TO_TICKS(updateRate_mS));
		}
	}
}


//
// Created by dchansen on 10/12/18.
//
#include "ConvolutionMatrix.h"

#include <GadgetronTimer.h>
#include <numeric>
#include "vector_td_utilities.h"
#include <boost/range/irange.hpp>

namespace
{
    using namespace Gadgetron;

    template<int N>
    struct iteration_counter { };

    template<class REAL, unsigned int D, template<class, unsigned int> class K>
    void iterate_body(
        const vector_td<REAL, D> &point,
        const vector_td<size_t, D> &matrix_size,
        std::vector<size_t> &indices,
        std::vector<REAL> &weights,
        vector_td<REAL, D> &image_point,
        size_t index,
        const ConvolutionKernel<REAL, D, K>& kernel,
        iteration_counter<-1>)
    {
        auto delta = abs(image_point - point);
        indices.push_back(index);
        weights.push_back(kernel.get(delta));
    }

    template<class REAL, unsigned int D, template<class, unsigned int> class K, int N>
    void iterate_body(
        const vector_td<REAL, D> &point,
        const vector_td<size_t, D> &matrix_size,
        std::vector<size_t> &indices,
        std::vector<REAL> &weights,
        vector_td<REAL, D> &image_point,
        size_t index,
        const ConvolutionKernel<REAL, D, K>& kernel,
        iteration_counter<N>)
    {
        size_t frame_offset = std::accumulate(&matrix_size[0], &matrix_size[N], 1, std::multiplies<size_t>());

        for (int i = std::ceil(point[N] - kernel.get_radius());
             i <= std::floor(point[N] + kernel.get_radius());
             i++)
        {
            auto wrapped_i = (i + matrix_size[N]) % matrix_size[N];
            size_t index2 = index + frame_offset * wrapped_i;
            image_point[N] = i;
            iterate_body(point, matrix_size, indices, weights, image_point,
                         index2, kernel, iteration_counter<N - 1>());
        }
    }

    template<class REAL, unsigned int D, template<class, unsigned int> class K>
    std::tuple<std::vector<size_t>, std::vector<REAL>> get_indices(
        const vector_td<REAL, D> &point,
        const vector_td<size_t, D> &matrix_size,
        const ConvolutionKernel<REAL, D, K>& kernel)
    {
        std::vector<size_t> indices;
        indices.reserve(size_t(std::pow(std::ceil(kernel.get_width()), D)));

        std::vector<REAL> weights;
        weights.reserve(size_t(std::pow(std::ceil(kernel.get_width()), D)));

        vector_td<REAL, D> image_point;
        size_t index = 0;
        iterate_body(point, matrix_size, indices, weights, image_point, index,
                     kernel, iteration_counter<D - 1>());

        return std::make_tuple(std::move(indices), std::move(weights));
    }
}

template<class REAL, unsigned int D, template<class, unsigned int> class K>
Gadgetron::ConvInternal::ConvolutionMatrix<REAL>
Gadgetron::ConvInternal::make_conv_matrix(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<REAL, D>> trajectory,
    const Gadgetron::vector_td<size_t, D> &matrix_size,
    const ConvolutionKernel<REAL, D, K>& kernel)
{
    ConvolutionMatrix<REAL> matrix(trajectory.get_number_of_elements(),
                                   prod(matrix_size));

    #pragma omp parallel for
    for (int i = 0; i < (int)trajectory.get_number_of_elements(); i++)
    {
        std::tie(matrix.indices[i], matrix.weights[i]) = get_indices(
            trajectory[i], matrix_size, kernel);
    }

    return matrix;
}

template<class REAL>
Gadgetron::ConvInternal::ConvolutionMatrix<REAL>
Gadgetron::ConvInternal::transpose(const Gadgetron::ConvInternal::ConvolutionMatrix<REAL> &matrix) {

    auto work_index = boost::irange(size_t(0),matrix.n_cols);

    ConvolutionMatrix<REAL> transposed(matrix.n_rows, matrix.n_cols);

    std::vector<int> counts(matrix.n_rows, 0);

    for (size_t i : work_index)
    {
        auto &rows = matrix.indices[i];
        for (auto &row : rows)
            counts[row]++;
    }

    for (size_t i = 0; i < counts.size(); i++)
    {
        transposed.indices[i].reserve(counts[i]);
        transposed.weights[i].reserve(counts[i]);
    }

    for (size_t i : work_index )
    {
        auto &rows = matrix.indices[i];
        auto &weights = matrix.weights[i];
        for (size_t n = 0; n < rows.size(); n++)
        {
            transposed.indices[rows[n]].push_back(i);
            transposed.weights[rows[n]].push_back(weights[n]);
        }
    }

    return transposed;
}

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 1, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 1>> trajectory,
    const Gadgetron::vector_td<size_t, 1> &matrix_size,
    const ConvolutionKernel<float, 1, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 2, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 2>> trajectory,
    const Gadgetron::vector_td<size_t, 2> &matrix_size,
    const ConvolutionKernel<float, 2, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 3, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 3>> trajectory,
    const Gadgetron::vector_td<size_t, 3> &matrix_size,
    const ConvolutionKernel<float, 3, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 4, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 4>> trajectory,
    const Gadgetron::vector_td<size_t, 4> &matrix_size,
    const ConvolutionKernel<float, 4, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 1, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 1>> trajectory,
    const Gadgetron::vector_td<size_t, 1> &matrix_size,
    const ConvolutionKernel<double, 1, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 2, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 2>> trajectory,
    const Gadgetron::vector_td<size_t, 2> &matrix_size,
    const ConvolutionKernel<double, 2, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 3, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 3>> trajectory,
    const Gadgetron::vector_td<size_t, 3> &matrix_size,
    const ConvolutionKernel<double, 3, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 4, Gadgetron::KaiserKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 4>> trajectory,
    const Gadgetron::vector_td<size_t, 4> &matrix_size,
    const ConvolutionKernel<double, 4, Gadgetron::KaiserKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 1, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 1>> trajectory,
    const Gadgetron::vector_td<size_t, 1> &matrix_size,
    const ConvolutionKernel<float, 1, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 2, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 2>> trajectory,
    const Gadgetron::vector_td<size_t, 2> &matrix_size,
    const ConvolutionKernel<float, 2, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 3, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 3>> trajectory,
    const Gadgetron::vector_td<size_t, 3> &matrix_size,
    const ConvolutionKernel<float, 3, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::make_conv_matrix<float, 4, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<float, 4>> trajectory,
    const Gadgetron::vector_td<size_t, 4> &matrix_size,
    const ConvolutionKernel<float, 4, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 1, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 1>> trajectory,
    const Gadgetron::vector_td<size_t, 1> &matrix_size,
    const ConvolutionKernel<double, 1, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 2, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 2>> trajectory,
    const Gadgetron::vector_td<size_t, 2> &matrix_size,
    const ConvolutionKernel<double, 2, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 3, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 3>> trajectory,
    const Gadgetron::vector_td<size_t, 3> &matrix_size,
    const ConvolutionKernel<double, 3, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::make_conv_matrix<double, 4, Gadgetron::JincKernel>(
    const Gadgetron::hoNDArray<Gadgetron::vector_td<double, 4>> trajectory,
    const Gadgetron::vector_td<size_t, 4> &matrix_size,
    const ConvolutionKernel<double, 4, Gadgetron::JincKernel>& kernel);

template Gadgetron::ConvInternal::ConvolutionMatrix<float>
Gadgetron::ConvInternal::transpose(
    const Gadgetron::ConvInternal::ConvolutionMatrix<float> &matrix);

template Gadgetron::ConvInternal::ConvolutionMatrix<double>
Gadgetron::ConvInternal::transpose(
        const Gadgetron::ConvInternal::ConvolutionMatrix<double> &matrix);


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "cuNDArray_fileio.h"
#include "cuNDArray_math.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "cuTvOperator.h"
#include "cuTv1dOperator.h"
#include "cuTvPicsOperator.h"
#include "CBSubsetOperator.h"
#include "osSPSSolver.h"
#include "osMOMSolver.h"
#include "osMOMSolverD.h"
#include "osMOMSolverD2.h"
#include "hoCuNCGSolver.h"
#include "osMOMSolverD3.h"
#include "osMOMSolverL1.h"
#include "osMOMSolverF.h"
#include "osAHZCSolver.h"
#include "ADMMSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <GPUTimer.h>
#include "cuSolverUtils.h"
#include "osPDsolver.h"
#include "osLALMSolver.h"
#include "osLALMSolver2.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include "cuEdgeATrousOperator.h"
#include "cuDCTOperator.h"
#include "cuDCTDerivativeOperator.h"
#include "dicomWriter.h"
#include "conebeam_projection.h"
#include "weightingOperator.h"
#include "hoNDArray_math.h"
#include "cuNCGSolver.h"
#include "CT_acquisition.h"
#include "hoCuOSOMSolver.h"

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<cuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
	std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
	std::vector<size_t> is_dims3d = is_dims;
	is_dims3d.pop_back();
	boost::shared_ptr< hoCuConebeamProjectionOperator >
	Ep( new hoCuConebeamProjectionOperator() );
	Ep->setup(ps,binning_pics,imageDimensions);
	Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	Ep->set_domain_dimensions(&is_dims3d);
	Ep->set_use_filtered_backprojection(true);
	boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	Ep->mult_MH(&projections,prior3d.get());

	hoCuNDArray<float> tmp_proj(*ps->get_projections());
	Ep->mult_M(prior3d.get(),&tmp_proj);
	float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	*prior3d *= s;
	boost::shared_ptr<cuNDArray<float> > prior(new cuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	std::cout << "Prior complete" << std::endl;
	return prior;
}

boost::shared_ptr<cuNDArray<float>> calculate_weightImage(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& ho_projections, std::vector<size_t> is_dims, floatd3 imageDimensions){

	cuNDArray<float> projections(ho_projections);
	boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
	std::vector<size_t> is_dims3d = is_dims;
	is_dims3d.pop_back();
	auto Ep = boost::make_shared<cuConebeamProjectionOperator>();
	auto ps2 = boost::make_shared<CBCT_acquisition>(boost::shared_ptr<hoCuNDArray<float>>(),ps->get_geometry());
	Ep->setup(ps2,binning_pics,imageDimensions);
	Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	Ep->set_domain_dimensions(&is_dims3d);
	//Ep->set_use_filtered_backprojection(true);
	Ep->offset_correct(&projections);
	//Ep->mult_MH(&projections,prior3d.get());
	//cgSolver<hoCuNDArray<float>> solv;
	cuNCGSolver<float> solv;
	solv.set_non_negativity_constraint(true);
	solv.set_encoding_operator(Ep);
	solv.set_max_iterations(10);
	auto prior3d = solv.solve(&projections);
	//auto prior3d = boost::make_shared<cuNDArray<float>>(is_dims3d);
	write_nd_array(prior3d.get(),"fdk.real");
	cuNDArray<float> tmp_proj(projections);
	clear(&tmp_proj);
	Ep->mult_M(prior3d.get(),&tmp_proj);
	//float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	//std::cout << "Scaling " << s << std::endl;
/*
	 //Ep->offset_correct(&tmp_proj);
	//tmp_proj *= s;
	write_nd_array(&tmp_proj,"projtmp.real");
	tmp_proj -= projections;
	tmp_proj *= float(-1);
	abs_inplace(&tmp_proj);

	write_nd_array(ps->get_projections().get(),"proj.real");
	write_nd_array(&tmp_proj,"projdiff.real");
	std::cout << "Proj size ";
	auto pdims = *tmp_proj.get_dimensions();
	for (auto p : pdims ) std::cout << p << " ";
	std::cout << std::endl;
	//Ep->set_use_filtered_backprojection(false);
	//Ep->mult_MH(&tmp_proj,prior3d.get());
	//solv.set_non_negativity_constraint(false);
	prior3d = solv.solve(&tmp_proj);
	//abs_inplace(prior.get());
	std::cout << "Prior complete" << std::endl;
	*/
	return prior3d;
}

int main(int argc, char** argv)
{

	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	unsigned int downsamples;
	unsigned int iterations;
	unsigned int subsets;
	float rho,tau;
	float tv_weight,pics_weight, wavelet_weight,huber,sigma,dct_weight;
	float tv_4d;
    bool use_non_negativity;
	int reg_iter;

	po::options_description desc("Allowed options");

	desc.add_options()
    				("help", "produce help message")
    				("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    				("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    				("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
    				("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    				("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    				("SAG","Use exact SAG correction if present")
    				("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    				("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    				("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    				("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    				("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    				("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    				("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight in spatial dimensions")
							("TV4D",po::value<float>(&tv_4d)->default_value(0),"Total variation weight in temporal dimensions")
    				("PICS",po::value<float>(&pics_weight)->default_value(0),"PICS weight")
    				("Wavelet,W",po::value<float>(&wavelet_weight)->default_value(0),"Weight of the wavelet operator")
    				("Huber",po::value<float>(&huber)->default_value(0),"Huber weight")
    				("use_prior","Use an FDK prior")
    				("use_non_negativity",po::value<bool>(&use_non_negativity)->default_value(true),"Prevent image from having negative attenuation")
    				("sigma",po::value<float>(&sigma)->default_value(0.1),"Sigma for billateral filter")
    				("DCT",po::value<float>(&dct_weight)->default_value(0),"DCT regularization")
    				("3D","Only use binning for selecting valid projections")
							("tau",po::value<float>(&tau)->default_value(1e-5),"Tau value for solver")
							("reg_iter",po::value<int>(&reg_iter)->default_value(2))
    				;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
        else if (a.type() == typeid(bool)) command_line_string << it->second.as<bool>();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}
	std::cout << command_line_string.str();

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);
	ps->downsample(downsamples);

	float SDD = ps->get_geometry()->get_SDD();
	float SAD = ps->get_geometry()->get_SAD();

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
		if (vm.count("3D"))
			binning = binning->get_3d_binning();
	} else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	float lengthOfRay_in_mm = norm(imageDimensions);
	unsigned int numSamplesPerPixel = 3;
	float minSpacing = min(voxelSize)/numSamplesPerPixel;

	unsigned int numSamplesPerRay;
	if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
	else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

	float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
	size_t numProjs = ps->get_projections()->get_size(2);
	size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	//osLALMSolver<cuNDArray<float>> solver;
	hoCuOSMOMSolver<float> solver;
	//osMOMSolverL1<cuNDArray<float>> solver;
	//osAHZCSolver<cuNDArray<float>> solver;
	//osMOMSolverF<cuNDArray<float>> solver;
	//ADMMSolver<cuNDArray<float>> solver;
	solver.set_dump(false);

	//hoCuCgDescentSolver<float> solver;

	//osSPSSolver<hoNDArray<float>> solver;
	//hoCuNCGSolver<float> solver;
	//solver.set_domain_dimensions(&is_dims);
	solver.set_max_iterations(iterations);
	solver.set_output_mode(osSPSSolver<cuNDArray<float>>::OUTPUT_VERBOSE);
	solver.set_tau(tau);
	solver.set_non_negativity_constraint(use_non_negativity);
	solver.set_huber(huber);
	solver.set_reg_steps(reg_iter);
	//solver.set_rho(rho);

  if (tv_weight > 0){

  	auto Dx = boost::make_shared<cuPartialDerivativeOperator<float,4>>(0);
  	Dx->set_weight(tv_weight);
  	Dx->set_domain_dimensions(&is_dims);
  	Dx->set_codomain_dimensions(&is_dims);

  	auto Dy = boost::make_shared<cuPartialDerivativeOperator<float,4>>(1);
  	Dy->set_weight(tv_weight);
  	Dy->set_domain_dimensions(&is_dims);
  	Dy->set_codomain_dimensions(&is_dims);

  	auto Dz = boost::make_shared<cuPartialDerivativeOperator<float,4>>(2);
  	Dz->set_weight(tv_weight);
  	Dz->set_domain_dimensions(&is_dims);
  	Dz->set_codomain_dimensions(&is_dims);

  	solver.add_regularization_group({Dx,Dy,Dz});
    if (tv_4d > 0) {
        auto Dt = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(3);
        Dt->set_weight(tv_4d);
        Dt->set_domain_dimensions(&is_dims);
        Dt->set_codomain_dimensions(&is_dims);
        solver.add_regularization_operator(Dt);
    }
/*
	auto projections = *ps->get_projections();
  	auto prior_weight = calculate_weightImage(binning,ps,projections,is_dims,imageDimensions);
  	//sqrt_inplace(prior_weight.get());
  	std::cout << "Prior min " << min(prior_weight.get()) << std::endl;
  	//*prior_weight -= min(prior_weight.get());
  	*prior_weight /= asum(prior_weight.get())/prior_weight->get_number_of_elements();
  	*prior_weight -= max(prior_weight.get());
  	*prior_weight *= float(-1);
  	//clamp_min(prior_weight.get(),float(1e-2));
  	//reciprocal_inplace(prior_weight.get());

  	write_nd_array(prior_weight.get(),"prior.real");
  	//cudaDeviceReset();
  	auto Wt = boost::make_shared<weightingOperator<cuNDArray<float>>>(prior_weight,Dt);
	Wt->set_weight(tv_weight);
  	Wt->set_domain_dimensions(&is_dims);
  	Wt->set_codomain_dimensions(&is_dims);
*/
  	//lver.add_regularization_operator(Dt);

  }
  /*
  if (pics_weight > 0){

  	auto Dx = boost::make_shared<cuPartialDerivativeOperator<float,4>>(0);
  	Dx->set_weight(pics_weight);
  	Dx->set_domain_dimensions(&is_dims);
  	Dx->set_codomain_dimensions(&is_dims);

  	auto Dy = boost::make_shared<cuPartialDerivativeOperator<float,4>>(1);
  	Dy->set_weight(pics_weight);
  	Dy->set_domain_dimensions(&is_dims);
  	Dy->set_codomain_dimensions(&is_dims);

  	auto Dz = boost::make_shared<cuPartialDerivativeOperator<float,4>>(2);
  	Dz->set_weight(pics_weight);
  	Dz->set_domain_dimensions(&is_dims);
  	Dz->set_codomain_dimensions(&is_dims);

  	solver.add_regularization_group({Dx,Dy,Dz},prior);

  }*/

/*
	if (tv_weight > 0){

		auto Dx = boost::make_shared<cuDCTDerivativeOperator<float>>(0);
		Dx->set_weight(tv_weight);
		Dx->set_domain_dimensions(&is_dims);
		Dx->set_codomain_dimensions(&is_dims);

		auto Dy = boost::make_shared<cuDCTDerivativeOperator<float>>(1);
		Dy->set_weight(tv_weight);
		Dy->set_domain_dimensions(&is_dims);
		Dy->set_codomain_dimensions(&is_dims);

		auto Dz = boost::make_shared<cuDCTDerivativeOperator<float>>(2);
		Dz->set_weight(tv_weight);
		Dz->set_domain_dimensions(&is_dims);
		Dz->set_codomain_dimensions(&is_dims);

		solver.add_regularization_group({Dx,Dy,Dz});

	}

*/

	if (dct_weight > 0){
		auto dctOp = boost::make_shared<cuDCTOperator<float>>();
		dctOp->set_domain_dimensions(&is_dims);
		dctOp->set_weight(dct_weight);
		solver.add_regularization_operator(dctOp);
	}

	auto E = boost::make_shared<CBSubsetOperator<cuNDArray> >(subsets);

	//E->setup(ps,binning,imageDimensions);
	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

	solver.set_encoding_operator(E);

	//auto projections = boost::make_shared<cuNDArray<float>>(*ps->get_projections());
	hoCuNDArray<float> projections = *ps->get_projections();
	ps->set_projections(boost::shared_ptr<hoCuNDArray<float>>());
	std::cout << "Projection norm:" << nrm2(&projections) << std::endl;
	//E->set_use_offset_correction(false);

	//boost::shared_ptr<cuNDArray<bool>> mask;
	//mask = E->calculate_mask(projections,0.03f);
	//ps->set_projections(boost::shared_ptr<hoCuNDArray<float>>()); //Clear projections from host memory.
	{
		auto cu_proj = cuNDArray<float>(projections);
		E->offset_correct(&cu_proj);
		cu_proj.to_host(&projections);
	}
	//E->set_mask(mask);
	std::cout << "Projection norm:" << nrm2(&projections) << std::endl;

	//solver.set_damping(1e-6);

	/*
    boost::shared_ptr<hoCuNDArray<float> > prior;

  if (vm.count("use_prior")) {
  	prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
  	solver.set_x0(prior);
  }
	 */
	boost::shared_ptr<hoNDArray<float>> result;
	{
		GPUTimer tim("Solver");
		result = solver.solve(&projections);
	}
//	global_timer.reset();
	std::cout << "Penguin" << nrm2(result.get()) << std::endl;

	std::cout << "Result sum " << asum(result.get()) << std::endl;

	//apply_mask(result.get(),mask.get());

	std::cout << "Result sum " << asum(result.get()) << std::endl;
	//saveNDArray2HDF5(result.get(),outputFile,imageDimensions,vector_td<float,3>(0),command_line_string.str(),iterations);

	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),command_line_string.str(),iterations);
//	write_nd_array(result.get(),"reconstruction.real");
	//write_dicom(result.get(),command_line_string.str(),imageDimensions);

}