#pragma once
#include "IntegerInterval.hpp"
#include "Misc.hpp"
#include <algorithm>
#include <boost/iterator/iterator_facade.hpp>
#include <cassert>
#include <vector>

namespace discreture
{

// An arithmetic progression is simply a set of the form
// {a,a+d,a+2d,a+3d,...,a+kd}. Similar to python range(n,m,step).
template <class IntType>
class ArithmeticProgression
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using value_type = IntType;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    class iterator;
    using const_iterator = iterator;
    using reverse_iterator = iterator;
    using const_reverse_iterator = reverse_iterator;

public:
    ////////////////////////////////////////////////////////////
    /// \brief Single integer constructor. It's usually better to use
    /// integer_interval(n) instead (faster)
    ///
    /// \param n is an integer >= 0
    ///
    /// \return An abstract random-access container whose elements are
    /// {0,1,2,...,n-1}
    ///
    ////////////////////////////////////////////////////////////
    explicit ArithmeticProgression(IntType n) : from_(0), to_(n), step_(1)
    {
        assert(n >= 0);
    }

    //////////////////////////////////////////
    /// \brief Constructor. Automatically detects wheter step size is positive
    /// or negative and adjusts accordingly \param n is an integer \return an
    /// abstract random-access container whose elements are
    /// {n,n+step,n+2*step,...} up to (and not including) t_to.
    //////////////////////////////////////////
    ArithmeticProgression(IntType from, IntType to, IntType step = 1)
        : from_(from), to_(to), step_(step)
    {
        assert(step_ != 0);
        if (step_ > 0 && to_ < from_)
            to_ = from_;
        if (step_ < 0 && to_ > from_)
            to_ = from_;

        auto d = modulo<IntType>(from_ - to_, step_);

        if (step_ > 0)
            to_ += d;
        else
        {
            to_ += d + step_;
            if (d == 0)
                to_ -= step_;
        }
    }

    size_type size() const
    {
        auto t = to_ + modulo<difference_type>(from_ - to_, step_);
        return (t - from_)/step_;
    }

    ////////////////////////////////////////////////////////////
    /// \brief Random access iterator class.
    ////////////////////////////////////////////////////////////
    class iterator
        : public boost::iterator_facade<iterator, const IntType&, boost::random_access_traversal_tag>
    {
    public:
        explicit iterator(size_type t_from = 0, size_type t_step = 1)
            : m_ID(t_from), step_(t_step)
        {}

        size_type step() const { return step_; }

    private:
        void increment() { m_ID += step_; }

        void decrement() { m_ID -= step_; }

        const IntType& dereference() const { return m_ID; }

        void advance(difference_type n) { m_ID += n*step_; }

        bool equal(const iterator& it) const { return m_ID == it.m_ID; }

        difference_type distance_to(const iterator& it) const
        {
            assert(step_ != 0);
            return (static_cast<difference_type>(it.m_ID) -
                    static_cast<difference_type>(m_ID)) /
              step_;
        }

    private:
        IntType m_ID{0};
        size_type step_{1};

        friend class boost::iterator_core_access;
        friend class ArithmeticProgression;
    }; // end class iterator

    iterator begin() const { return iterator(from_, step_); }
    iterator end() const { return iterator(to_, step_); }

    iterator rbegin() const { return iterator(to_ - step_, -step_); }
    iterator rend() const { return iterator(from_ - step_, -step_); }

    IntType operator[](size_type m) const { return from_ + step_*m; }

    template <class Pred>
    IntType partition_point(Pred p)
    {
        return *std::partition_point(begin(), end(), p);
    }

private:
    IntType from_;
    IntType to_;
    IntType step_;
}; // end class ArithmeticProgression

using arithmetic_progression = ArithmeticProgression<int>;
using big_arithmetic_progression = ArithmeticProgression<std::int64_t>;

} // namespace discreture


// Copyright Paul Bristow 2007.
// Copyright John Maddock 2006.

// 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)

// test_uniform.cpp

#include <pch.hpp>

#ifdef _MSC_VER
#  pragma warning(disable: 4127) // conditional expression is constant.
#  pragma warning(disable: 4100) // unreferenced formal parameter.
#endif

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/test/floating_point_comparison.hpp>

#include <boost/math/distributions/uniform.hpp>
    using boost::math::uniform_distribution;
#include <boost/math/tools/test.hpp>
#include "test_out_of_range.hpp"

#include <iostream>
#include <iomanip>
   using std::cout;
   using std::endl;
   using std::setprecision;
#include <limits>
  using std::numeric_limits;

template <class RealType>
void check_uniform(RealType lower, RealType upper, RealType x, RealType p, RealType q, RealType tol)
{
   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::cdf(
         uniform_distribution<RealType>(lower, upper),   // distribution.
         x),  // random variable.
         p,    // probability.
         tol);   // tolerance.
   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::cdf(
         complement(
            uniform_distribution<RealType>(lower, upper), // distribution.
            x)),    // random variable.
         q,    // probability complement.
         tol);  // tolerance.
   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::quantile(
         uniform_distribution<RealType>(lower, upper),  // distribution.
         p),   // probability.
         x,  // random variable.
         tol);  // tolerance.
   BOOST_CHECK_CLOSE_FRACTION(
      ::boost::math::quantile(
         complement(
            uniform_distribution<RealType>(lower, upper),  // distribution.
            q)),     // probability complement.
         x,                                             // random variable.
         tol);  // tolerance.
} // void check_uniform

template <class RealType>
void test_spots(RealType)
{
   // Basic sanity checks
   //
   // These test values were generated for the normal distribution
   // using the online calculator at
   // http://espse.ed.psu.edu/edpsych/faculty/rhale/hale/507Mat/statlets/free/pdist.htm
   //
   // Tolerance is just over 5 decimal digits expressed as a fraction:
   // that's the limit of the test data.
   RealType tolerance = 2e-5f;
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << "." << endl;

   using std::exp;

   // Tests for PDF
   //
   BOOST_CHECK_CLOSE_FRACTION( // x == upper
      pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0)),
      static_cast<RealType>(1),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION( // x == lower
      pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(1)),
      static_cast<RealType>(1),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION( // x > upper
      pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(-1)),
      static_cast<RealType>(0),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION( // x < lower
      pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(2)),
      static_cast<RealType>(0),
      tolerance);

   if(std::numeric_limits<RealType>::has_infinity)
    { // BOOST_CHECK tests for infinity using std::numeric_limits<>::infinity()
      // Note that infinity is not implemented for real_concept, so these tests
      // are only done for types, like built-in float, double.. that have infinity.
    // Note that these assume that  BOOST_MATH_OVERFLOW_ERROR_POLICY is NOT throw_on_error.
    // #define BOOST_MATH_OVERFLOW_ERROR_POLICY == throw_on_error would give a throw here.
    // #define BOOST_MATH_DOMAIN_ERROR_POLICY == throw_on_error IS defined, so the throw path
    // of error handling is tested below with BOOST_MATH_CHECK_THROW tests.

     BOOST_MATH_CHECK_THROW( // x == infinity should NOT be OK.
       pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(std::numeric_limits<RealType>::infinity())),
       std::domain_error);

     BOOST_MATH_CHECK_THROW( // x == minus infinity should be OK too.
       pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(-std::numeric_limits<RealType>::infinity())),
       std::domain_error);
   }
   if(std::numeric_limits<RealType>::has_quiet_NaN)
   { // BOOST_CHECK tests for NaN using std::numeric_limits<>::has_quiet_NaN() - should throw.
     BOOST_MATH_CHECK_THROW(
       pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(std::numeric_limits<RealType>::quiet_NaN())),
       std::domain_error);
     BOOST_MATH_CHECK_THROW(
       pdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(-std::numeric_limits<RealType>::quiet_NaN())),
       std::domain_error);
   } // test for x = NaN using std::numeric_limits<>::quiet_NaN()

   // cdf
   BOOST_CHECK_EQUAL( // x < lower
      cdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(-1)),
      static_cast<RealType>(0) );
   BOOST_CHECK_CLOSE_FRACTION(
      cdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0)),
      static_cast<RealType>(0),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      cdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.5)),
      static_cast<RealType>(0.5),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      cdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.1)),
      static_cast<RealType>(0.1),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      cdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.9)),
      static_cast<RealType>(0.9),
      tolerance);
   BOOST_CHECK_EQUAL( // x > upper
      cdf(uniform_distribution<RealType>(0, 1), static_cast<RealType>(2)),
      static_cast<RealType>(1));

  // cdf complement
   BOOST_CHECK_EQUAL( // x < lower
      cdf(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0))),
      static_cast<RealType>(1));
   BOOST_CHECK_EQUAL( // x == 0
      cdf(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0))),
      static_cast<RealType>(1));
   BOOST_CHECK_CLOSE_FRACTION( // x = 0.1
      cdf(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.1))),
      static_cast<RealType>(0.9),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION( // x = 0.5
      cdf(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.5))),
      static_cast<RealType>(0.5),
      tolerance);
   BOOST_CHECK_EQUAL( // x == 1
      cdf(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(1))),
      static_cast<RealType>(0));
   BOOST_CHECK_EQUAL( // x > upper
      cdf(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(2))),
      static_cast<RealType>(0));

   // quantile

   BOOST_CHECK_CLOSE_FRACTION(
      quantile(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.9)),
      static_cast<RealType>(0.9),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.1)),
      static_cast<RealType>(0.1),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.5)),
      static_cast<RealType>(0.5),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0)),
      static_cast<RealType>(0),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(uniform_distribution<RealType>(0, 1), static_cast<RealType>(1)),
      static_cast<RealType>(1),
      tolerance);

   // quantile complement

   BOOST_CHECK_CLOSE_FRACTION(
      quantile(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.1))),
      static_cast<RealType>(0.9),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.9))),
      static_cast<RealType>(0.1),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0.5))),
      static_cast<RealType>(0.5),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(0))),
      static_cast<RealType>(1),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      quantile(complement(uniform_distribution<RealType>(0, 1), static_cast<RealType>(1))),
      static_cast<RealType>(0),
      tolerance);

   // Some tests using a different location & scale, neight zero or unity.
   BOOST_CHECK_CLOSE_FRACTION( // x == mid
      pdf(uniform_distribution<RealType>(-1, 2), static_cast<RealType>(1)),
      static_cast<RealType>(0.3333333333333333333333333333333333333333333333333333),
      tolerance);

   BOOST_CHECK_CLOSE_FRACTION( // x == upper
      pdf(uniform_distribution<RealType>(-1, 2), static_cast<RealType>(+2)),
      static_cast<RealType>(0.3333333333333333333333333333333333333333333333333333),  // 1 / (2 - -1) = 1/3
      tolerance);

   BOOST_CHECK_CLOSE_FRACTION( // x == lower
      cdf(uniform_distribution<RealType>(-1, 2), static_cast<RealType>(-1)),
      static_cast<RealType>(0),
      tolerance);
   BOOST_CHECK_CLOSE_FRACTION( // x == upper
      cdf(uniform_distribution<RealType>(-1, 2), static_cast<RealType>(0)),
      static_cast<RealType>(0.3333333333333333333333333333333333333333333333333333),
      tolerance);

   BOOST_CHECK_CLOSE_FRACTION( // x == upper
      cdf(uniform_distribution<RealType>(-1, 2), static_cast<RealType>(1)),
      static_cast<RealType>(0.6666666666666666666666666666666666666666666666666667),
      tolerance);

   BOOST_CHECK_CLOSE_FRACTION( // x == lower
      cdf(uniform_distribution<RealType>(-1, 2), static_cast<RealType>(2)),
      static_cast<RealType>(1),
      tolerance);

   BOOST_CHECK_CLOSE_FRACTION( // x == upper
      quantile(uniform_distribution<RealType>(-1, 2), static_cast<RealType>(0.6666666666666666666666666666666666666666666666666667)),
      static_cast<RealType>(1),
      tolerance);

      check_uniform(
      static_cast<RealType>(0),       // lower
      static_cast<RealType>(1),       // upper
      static_cast<RealType>(0.5),     // x
      static_cast<RealType>(0.5),     // p
      static_cast<RealType>(1 - 0.5), // q
      tolerance);

      // Some Not-standard uniform tests.
      check_uniform(
      static_cast<RealType>(-1),    // lower
      static_cast<RealType>(1),     // upper
      static_cast<RealType>(0),     // x
      static_cast<RealType>(0.5),   // p
      static_cast<RealType>(1 - 0.5), // q = 1 - p
      tolerance);

      check_uniform(
      static_cast<RealType>(1),    // lower
      static_cast<RealType>(3),     // upper
      static_cast<RealType>(2),     // x
      static_cast<RealType>(0.5),   // p
      static_cast<RealType>(1 - 0.5), // q = 1 - p
      tolerance);

      check_uniform(
      static_cast<RealType>(-1),    // lower
      static_cast<RealType>(2),     // upper
      static_cast<RealType>(1),     // x
      static_cast<RealType>(0.66666666666666666666666666666666666666666667),   // p
      static_cast<RealType>(0.33333333333333333333333333333333333333333333), // q = 1 - p
      tolerance);
   tolerance = (std::max)(
      boost::math::tools::epsilon<RealType>(),
      static_cast<RealType>(boost::math::tools::epsilon<double>())) * 5; // 5 eps as a fraction.
    cout << "Tolerance (as fraction) for type " << typeid(RealType).name()  << " is " << tolerance << "." << endl;
   uniform_distribution<RealType> distu01(0, 1);
   RealType x = static_cast<RealType>(0.5);
   using namespace std; // ADL of std names.
   // mean:
   BOOST_CHECK_CLOSE_FRACTION(
      mean(distu01), static_cast<RealType>(0.5), tolerance);
   // variance:
   BOOST_CHECK_CLOSE_FRACTION(
      variance(distu01), static_cast<RealType>(0.0833333333333333333333333333333333333333333), tolerance);
   // std deviation:
   BOOST_CHECK_CLOSE_FRACTION(
    standard_deviation(distu01), sqrt(variance(distu01)), tolerance);
   // hazard:
   BOOST_CHECK_CLOSE_FRACTION(
    hazard(distu01, x), pdf(distu01, x) / cdf(complement(distu01, x)), tolerance);
   // cumulative hazard:
   BOOST_CHECK_CLOSE_FRACTION(
    chf(distu01, x), -log(cdf(complement(distu01, x))), tolerance);
   // coefficient_of_variation:
   BOOST_CHECK_CLOSE_FRACTION(
    coefficient_of_variation(distu01), standard_deviation(distu01) / mean(distu01), tolerance);
   // mode:
   BOOST_CHECK_CLOSE_FRACTION(
    mode(distu01), static_cast<RealType>(0), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(
      median(distu01), static_cast<RealType>(0.5), tolerance);
   // skewness:
   BOOST_CHECK_EQUAL(
    skewness(distu01), static_cast<RealType>(0));
   // kertosis:
   BOOST_CHECK_CLOSE_FRACTION(
    kurtosis(distu01), kurtosis_excess(distu01) + static_cast<RealType>(3), tolerance);
   // kertosis excess:
   BOOST_CHECK_CLOSE_FRACTION(
    kurtosis_excess(distu01), static_cast<RealType>(-1.2), tolerance);

   if(std::numeric_limits<RealType>::has_infinity)
  { // BOOST_CHECK tests for infinity using std::numeric_limits<>::infinity()
    // Note that infinity is not implemented for real_concept, so these tests
    // are only done for types, like built-in float, double, long double, that have infinity.
    // Note that these assume that  BOOST_MATH_OVERFLOW_ERROR_POLICY is NOT throw_on_error.
    // #define BOOST_MATH_OVERFLOW_ERROR_POLICY == throw_on_error would give a throw here.
    // #define BOOST_MATH_DOMAIN_ERROR_POLICY == throw_on_error IS defined, so the throw path
    // of error handling is tested below with BOOST_MATH_CHECK_THROW tests.

    BOOST_MATH_CHECK_THROW(pdf(distu01, std::numeric_limits<RealType>::infinity()),  std::domain_error);
    BOOST_MATH_CHECK_THROW(pdf(distu01, -std::numeric_limits<RealType>::infinity()),  std::domain_error);
   } // test for infinity using std::numeric_limits<>::infinity()
   else
   { // real_concept case, does has_infinfity == false, so can't check it throws.
     // cout << std::numeric_limits<RealType>::infinity() << ' '
     // << (boost::math::fpclassify)(std::numeric_limits<RealType>::infinity()) << endl;
     // value of std::numeric_limits<RealType>::infinity() is zero, so FPclassify is zero,
     // so (boost::math::isfinite)(std::numeric_limits<RealType>::infinity()) does not detect infinity.
     // so these tests would never throw.
     //BOOST_MATH_CHECK_THROW(pdf(distu01, std::numeric_limits<RealType>::infinity()),  std::domain_error);
     //BOOST_MATH_CHECK_THROW(pdf(distu01, std::numeric_limits<RealType>::quiet_NaN()),  std::domain_error);
     // BOOST_MATH_CHECK_THROW(pdf(distu01, boost::math::tools::max_value<RealType>() * 2),  std::domain_error); // Doesn't throw.
     BOOST_CHECK_EQUAL(pdf(distu01, boost::math::tools::max_value<RealType>()), 0);
   }
   // Special cases:
   BOOST_CHECK(pdf(distu01, 0) == 1);
   BOOST_CHECK(cdf(distu01, 0) == 0);
   BOOST_CHECK(pdf(distu01, 1) == 1);
   BOOST_CHECK(cdf(distu01, 1) == 1);
   BOOST_CHECK(cdf(complement(distu01, 0)) == 1);
   BOOST_CHECK(cdf(complement(distu01, 1)) == 0);
   BOOST_CHECK(quantile(distu01, 0) == 0);
   BOOST_CHECK(quantile(complement(distu01, 0)) == 1);
   BOOST_CHECK(quantile(distu01, 1) == 1);
   BOOST_CHECK(quantile(complement(distu01, 1)) == 0);

   // Error checks:
   if(std::numeric_limits<RealType>::has_quiet_NaN)
   { // BOOST_CHECK tests for constructing with quiet_NaN (not for real_concept, for example - see notes above).
     BOOST_MATH_CHECK_THROW(uniform_distribution<RealType>(0, std::numeric_limits<RealType>::quiet_NaN()), std::domain_error);
     BOOST_MATH_CHECK_THROW(uniform_distribution<RealType>(0, -std::numeric_limits<RealType>::quiet_NaN()), std::domain_error);
   }
   BOOST_MATH_CHECK_THROW(uniform_distribution<RealType>(1, 0), std::domain_error); // lower > upper!
   BOOST_MATH_CHECK_THROW(uniform_distribution<RealType>(1, 1), std::domain_error); // lower == upper!

   check_out_of_range<uniform_distribution<RealType> >(1, 5);
} // template <class RealType>void test_spots(RealType)

BOOST_AUTO_TEST_CASE( test_main )
{
  // Check that can construct uniform distribution using the two convenience methods:
  using namespace boost::math;
  uniform unistd; // Using typedef
  // == uniform_distribution<double> unistd;
  BOOST_CHECK_EQUAL(unistd.lower(), 0); // Check defaults.
  BOOST_CHECK_EQUAL(unistd.upper(), 1);
   uniform_distribution<> myu01(0, 1); // Using default RealType double.
  BOOST_CHECK_EQUAL(myu01.lower(), 0); // Check defaults again.
  BOOST_CHECK_EQUAL(myu01.upper(), 1);

  // Test on extreme values of random variate x, using just double because it has numeric_limit infinity etc..
  // No longer allow x to be + or - infinity, then these tests should throw.
  BOOST_MATH_CHECK_THROW(pdf(unistd, +std::numeric_limits<double>::infinity()), std::domain_error); // x = + infinity
  BOOST_MATH_CHECK_THROW(pdf(unistd, -std::numeric_limits<double>::infinity()), std::domain_error); // x = - infinity
  BOOST_MATH_CHECK_THROW(cdf(unistd, +std::numeric_limits<double>::infinity()), std::domain_error); // x = + infinity
  BOOST_MATH_CHECK_THROW(cdf(unistd, -std::numeric_limits<double>::infinity()), std::domain_error); // x = - infinity

  BOOST_CHECK_EQUAL(pdf(unistd, +(std::numeric_limits<double>::max)()), 0); // x = + max
  BOOST_CHECK_EQUAL(pdf(unistd, -(std::numeric_limits<double>::min)()), 0); // x = - min
  BOOST_CHECK_EQUAL(cdf(unistd, +(std::numeric_limits<double>::max)()), 1); // x = + max
  BOOST_CHECK_EQUAL(cdf(unistd, -(std::numeric_limits<double>::min)()), 0); // x = - min
#ifndef BOOST_NO_EXCEPTIONS
  BOOST_MATH_CHECK_THROW(uniform_distribution<> zinf(0, +std::numeric_limits<double>::infinity()), std::domain_error); // zero to infinity using default RealType double.
#else
  BOOST_MATH_CHECK_THROW(uniform_distribution<>(0, +std::numeric_limits<double>::infinity()), std::domain_error); // zero to infinity using default RealType double.
#endif
   uniform_distribution<> zmax(0, +(std::numeric_limits<double>::max)()); // zero to max using default RealType double.
  BOOST_CHECK_EQUAL(zmax.lower(), 0); // Check defaults again.
  BOOST_CHECK_EQUAL(zmax.upper(), +(std::numeric_limits<double>::max)());

  BOOST_CHECK_EQUAL(pdf(zmax, -1), 0); // pdf is 1/(0 - max) = almost zero for all x
  BOOST_CHECK_EQUAL(pdf(zmax, 0), (std::numeric_limits<double>::min)()/4); // x =
  BOOST_CHECK_EQUAL(pdf(zmax, 1), (std::numeric_limits<double>::min)()/4); // x =
  BOOST_MATH_CHECK_THROW(pdf(zmax, +std::numeric_limits<double>::infinity()), std::domain_error); // pdf is 1/(0 - infinity) = zero for all x
  BOOST_MATH_CHECK_THROW(pdf(zmax, -std::numeric_limits<double>::infinity()), std::domain_error);
  BOOST_CHECK_EQUAL(pdf(zmax, +(std::numeric_limits<double>::max)()), (std::numeric_limits<double>::min)()/4); // x =
  BOOST_CHECK_EQUAL(pdf(zmax, -(std::numeric_limits<double>::max)()), 0); // x =
#ifndef BOOST_NO_EXCEPTIONS
  // Ensure NaN throws an exception.
  BOOST_MATH_CHECK_THROW(uniform_distribution<> zNaN(0, std::numeric_limits<double>::quiet_NaN()), std::domain_error);
  BOOST_MATH_CHECK_THROW(pdf(unistd, std::numeric_limits<double>::quiet_NaN()), std::domain_error);
#else
  BOOST_MATH_CHECK_THROW(uniform_distribution<>(0, std::numeric_limits<double>::quiet_NaN()), std::domain_error);
  BOOST_MATH_CHECK_THROW(pdf(unistd, std::numeric_limits<double>::quiet_NaN()), std::domain_error);
#endif
    // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
  test_spots(0.0F); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
  test_spots(0.0); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x0582))
  test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::endl;
#endif

} // BOOST_AUTO_TEST_CASE( test_main )

/*

Output:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_uniform.exe"
Running 1 test case...
Tolerance for type float is 2e-005.
Tolerance (as fraction) for type float is 5.96046e-007.
Tolerance for type double is 2e-005.
Tolerance (as fraction) for type double is 1.11022e-015.
Tolerance for type long double is 2e-005.
Tolerance (as fraction) for type long double is 1.11022e-015.
Tolerance for type class boost::math::concepts::real_concept is 2e-005.
Tolerance (as fraction) for type class boost::math::concepts::real_concept is 1.11022e-015.
*** No errors detected

*/


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3.cpp
 * @brief   Rotation, common code between Rotation matrix and Quaternion
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 */

#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

static const Matrix3 I3 = Matrix3::Identity();

/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
  gtsam::print((Matrix)matrix(), s);
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
  return rodriguez((Vector)w.vector(),theta);
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Unit3& w, double theta) {
  return rodriguez(w.point3(),theta);
}

/* ************************************************************************* */
Rot3 Rot3::Random(boost::mt19937 & rng) {
  // TODO allow any engine without including all of boost :-(
  Unit3 w = Unit3::Random(rng);
  boost::uniform_real<double> randomAngle(-M_PI,M_PI);
  double angle = randomAngle(rng);
  return rodriguez(w,angle);
}

/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
  double t = w.norm();
  if (t < 1e-10) return Rot3();
  return rodriguez(w/t, t);
}

/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
  return equal_with_abs_tol(matrix(), R.matrix(), tol);
}

/* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
  Unit3 q = Unit3(rotate(p.point3(Hp)));
  if (Hp)
    (*Hp) = q.basis().transpose() * matrix() * (*Hp);
  if (HR)
    (*HR) = -q.basis().transpose() * matrix() * p.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
  Unit3 q = Unit3(unrotate(p.point3(Hp)));
  if (Hp)
    (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
  if (HR)
    (*HR) = q.basis().transpose() * q.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::operator*(const Unit3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
  Point3 q(transpose()*p.vector()); // q = Rt*p
  if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
  if (H2) *H2 = transpose();
  return q;
}

/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
  if(zero(v)) return eye(3);
  Matrix x = skewSymmetric(v);
  Matrix x2 = x*x;
  double theta = v.norm(), vi = theta/2.0;
  double s1 = sin(vi)/vi;
  double s2 = (theta - sin(theta))/(theta*theta*theta);
  Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
  return res;
}

/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
  if(zero(v)) return eye(3);
  Matrix x = skewSymmetric(v);
  Matrix x2 = x*x;
  double theta = v.norm(), vi = theta/2.0;
  double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
  Matrix res = eye(3) + 0.5*x - s2*x2;
  return res;
}

/* ************************************************************************* */
Point3 Rot3::column(int index) const{
  if(index == 3)
    return r3();
  else if(index == 2)
    return r2();
  else if(index == 1)
    return r1(); // default returns r1
  else
    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}

/* ************************************************************************* */
Vector3 Rot3::xyz() const {
  Matrix I;Vector3 q;
  boost::tie(I,q)=RQ(matrix());
  return q;
}

/* ************************************************************************* */
Vector3 Rot3::ypr() const {
  Vector3 q = xyz();
  return Vector3(q(2),q(1),q(0));
}

/* ************************************************************************* */
Vector3 Rot3::rpy() const {
  return xyz();
}

/* ************************************************************************* */
Vector Rot3::quaternion() const {
  Quaternion q = toQuaternion();
  Vector v(4);
  v(0) = q.w();
  v(1) = q.x();
  v(2) = q.y();
  v(3) = q.z();
  return v;
}

/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x)    {
  // x is the axis-angle representation (exponential coordinates) for a rotation
  double normx = norm_2(x); // rotation angle
  Matrix3 Jr;
  if (normx < 10e-8){
    Jr = Matrix3::Identity();
  }
  else{
    const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
    Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
        ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
  }
  return Jr;
}

/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x)    {
  // x is the axis-angle representation (exponential coordinates) for a rotation
  double normx = norm_2(x); // rotation angle
  Matrix3 Jrinv;

  if (normx < 10e-8){
    Jrinv = Matrix3::Identity();
  }
  else{
    const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
    Jrinv = Matrix3::Identity() +
        0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx))   ) * X * X;
  }
  return Jrinv;
}

/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) {

  double x = -atan2(-A(2, 1), A(2, 2));
  Rot3 Qx = Rot3::Rx(-x);
  Matrix3 B = A * Qx.matrix();

  double y = -atan2(B(2, 0), B(2, 2));
  Rot3 Qy = Rot3::Ry(-y);
  Matrix3 C = B * Qy.matrix();

  double z = -atan2(-C(1, 0), C(1, 1));
  Rot3 Qz = Rot3::Rz(-z);
  Matrix3 R = C * Qz.matrix();

  Vector xyz = Vector3(x, y, z);
  return make_pair(R, xyz);
}

/* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
  os << "\n";
  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
  return os;
}

/* ************************************************************************* */

} // namespace gtsam


// Petter Strandmark 2012.

#include <cstdio>
#include <iomanip>
#include <iostream>
#include <limits>
#include <memory>
#include <random>
#include <stdexcept>

// GNU 4.8.1 define _X on Cygwin.
// This breaks Eigen.
// http://eigen.tuxfamily.org/bz/process_bug.cgi
#ifdef _X
#undef _X
#endif
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include <spii/spii.h>
#include <spii/solver.h>

namespace spii {

void NewtonSolver::solve(const Function& function,
                         SolverResults* results) const
{
	double global_start_time = wall_time();

	// Random number engine for random pertubation.
	std::mt19937 prng(unsigned(1));
	std::uniform_real_distribution<double> uniform11(-1.0, 1.0);
	auto rand11 = std::bind(uniform11, prng);

	// Dimension of problem.
	size_t n = function.get_number_of_scalars();

	if (n == 0) {
		results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
		return;
	}

	// Determine whether to use sparse representation
	// and matrix factorization.
	bool use_sparsity;
	if (this->sparsity_mode == SparsityMode::DENSE) {
		use_sparsity = false;
	}
	else if (this->sparsity_mode == SparsityMode::SPARSE) {
		use_sparsity = true;
	}
	else {
		if (n <= 50) {
			use_sparsity = false;
		}
		else {
			use_sparsity = true;
		}
	}

	auto factorization_method = this->factorization_method;
	if (use_sparsity && this->factorization_method == FactorizationMethod::MESCHACH) {
		if (this->log_function) {
			this->log_function("Can not use the Meschach library for sparse problems. Switching to iterative factorization.");
		}
		factorization_method = FactorizationMethod::ITERATIVE;
	}

	// Current point, gradient and Hessian.
	double fval   = std::numeric_limits<double>::quiet_NaN();;
	double fprev  = std::numeric_limits<double>::quiet_NaN();
	double normg0 = std::numeric_limits<double>::quiet_NaN();
	double normg  = std::numeric_limits<double>::quiet_NaN();
	double normdx = std::numeric_limits<double>::quiet_NaN();

	Eigen::VectorXd x, g;
	Eigen::MatrixXd H;
	Eigen::SparseMatrix<double> sparse_H;
	if (use_sparsity) {
		// Create sparsity pattern for H.
		function.create_sparse_hessian(&sparse_H);
		if (this->log_function) {
			double nnz = double(sparse_H.nonZeros()) / double(n * n);
			char str[1024];
			std::sprintf(str, "H is %dx%d with %d (%.5f%%) non-zeroes.",
				sparse_H.rows(), sparse_H.cols(), sparse_H.nonZeros(), 100.0 * nnz);
			this->log_function(str);
		}
	}

	// Copy the user state to the current point.
	function.copy_user_to_global(&x);
	Eigen::VectorXd x2(n);

	// p will store the search direction.
	Eigen::VectorXd p(function.get_number_of_scalars());

	// Dense and sparse Cholesky factorizers.
	typedef Eigen::LLT<Eigen::MatrixXd> LLT;
	typedef Eigen::SimplicialLLT<Eigen::SparseMatrix<double> > SparseLLT;
	std::unique_ptr<LLT> factorization;
	std::unique_ptr<SparseLLT> sparse_factorization;
	if (!use_sparsity) {
		factorization.reset(new LLT(n));
	}
	else {
		sparse_factorization.reset(new SparseLLT);
		// The sparsity pattern of H is always the same. Therefore, it is enough
		// to analyze it once.
		sparse_factorization->analyzePattern(sparse_H);
	}

	FactorizationCache factorization_cache((int)n);
	CheckExitConditionsCache exit_condition_cache;

	//
	// START MAIN ITERATION
	//
	results->startup_time   += wall_time() - global_start_time;
	results->exit_condition = SolverResults::INTERNAL_ERROR;
	int iter = 0;
	while (true) {

		int log_interval = 1;
		if (iter > 30) {
			log_interval = 10;
		}
		if (iter > 200) {
			log_interval = 100;
		}
		if (iter > 2000) {
			log_interval = 1000;
		}

		//
		// Evaluate function and derivatives.
		//
		double start_time = wall_time();
		if (use_sparsity) {
			fval = function.evaluate(x, &g, &sparse_H);
		}
		else {
			fval = function.evaluate(x, &g, &H);
		}

		normg = std::max(g.maxCoeff(), -g.minCoeff());
		if (iter == 0) {
			normg0 = normg;
		}

		// Check for NaN.
		if (normg != normg) {
			results->exit_condition = SolverResults::FUNCTION_NAN;
			break;
		}

		results->function_evaluation_time += wall_time() - start_time;

		//
		// Test stopping criteriea
		//
		start_time = wall_time();
		if (this->check_exit_conditions(fval, fprev, normg,
			                            normg0, x.norm(), normdx,
			                            true, &exit_condition_cache, results)) {
			break;
		}
		if (iter >= this->maximum_iterations) {
			results->exit_condition = SolverResults::NO_CONVERGENCE;
			break;
		}
		if (this->callback_function) {
			CallbackInformation information;
			information.objective_value = fval;
			information.x = &x;
			information.g = &g;
			if (use_sparsity) {
				information.H_sparse = &sparse_H;
			}
			else {
				information.H_dense = &H;
			}

			if (!callback_function(information)) {
				results->exit_condition = SolverResults::USER_ABORT;
				break;
			}
		}
		results->stopping_criteria_time += wall_time() - start_time;

		int factorizations = 0;
		double tau = 0;
		double mindiag = 0;
		Eigen::VectorXd dH;
		if (use_sparsity) {
			dH = sparse_H.diagonal();
		}
		else {
			dH = H.diagonal();
		}
		mindiag = dH.minCoeff();

		if (factorization_method == FactorizationMethod::ITERATIVE) {
			//
			// Attempt repeated Cholesky factorization until the Hessian
			// becomes positive semidefinite.
			//
			//start_time = wall_time();
			double beta = 1.0;

			if (mindiag > 0) {
				tau = 0;
			}
			else {
				tau = -mindiag + beta;
			}
			while (true) {
				// Add tau*I to the Hessian.
				if (tau > 0) {
					for (size_t i = 0; i < n; ++i) {
						if (use_sparsity) {
							int ii = static_cast<int>(i);
							sparse_H.coeffRef(ii, ii) = dH(i) + tau;
						}
						else {
							H(i, i) = dH(i) + tau;
						}
					}
				}
				// Attempt Cholesky factorization.
				bool success;
				if (use_sparsity) {
					sparse_factorization->factorize(sparse_H);
					success = sparse_factorization->info() == Eigen::Success;
				}
				else {
					factorization->compute(H);
					success = factorization->info() == Eigen::Success;
				}
				factorizations++;
				// Check for success.
				if (success) {
					break;
				}
				tau = std::max(2*tau, beta);

				spii_assert(factorizations <= 100,
				            "Solver::solve: factorization failed.");
			}

			results->matrix_factorization_time += wall_time() - start_time;

			//
			// Solve linear system to obtain search direction.
			//
			start_time = wall_time();

			if (use_sparsity) {
				p = sparse_factorization->solve(-g);
			}
			else {
				p = factorization->solve(-g);
			}

			results->linear_solver_time += wall_time() - start_time;
		}
		else if (factorization_method == FactorizationMethod::MESCHACH) {
			spii_assert(!use_sparsity);

			// Performs a BKP block diagonal factorization, modifies it, and
			// solvers the linear system.
			this->BKP_dense(H, g, factorization_cache, &p, results);
			factorizations = 1;
		}
		else if (factorization_method == FactorizationMethod::SYM_ILDL) {
			factorizations = 1;
			if (use_sparsity) {
				this->BKP_sym_ildl(sparse_H, g, &p, results);
			}
			else {
				this->BKP_sym_ildl(H, g, &p, results);
			}
		}
		else {
			throw std::runtime_error("Unknown factorization method.");
		}

		//
		// Perform line search.
		//
		start_time = wall_time();
		double start_alpha = 1.0;
		double alpha = this->perform_linesearch(function, x, fval, g, p, &x2,
		                                        start_alpha);

		if (alpha <= 1e-15) {

			// Attempt a simple steepest descent instead.
			p = -g;
			alpha = this->perform_linesearch(function, x, fval, g, p, &x2,
			                                 1.0);
			if (alpha <= 0) {

				if (this->log_function) {
					this->log_function("Steepest descent step failed. Numerical problems?");
				}

				// This happens in really rare cases with numerical problems or
				// incorrectly defined objective functions. In the latter case,
				// there is not much to do. In the former case, randomly perturbing
				// x has been effective.
				for (size_t i = 0; i < n; ++i) {
					x[i] = x[i] + 1e-6 * rand11() * x[i];
				}

				alpha = 0;
			}
		}

		// Record length of this step.
		normdx = alpha * p.norm();
		// Update current point.
		x = x + alpha * p;

		results->backtracking_time += wall_time() - start_time;

		//
		// Log the results of this iteration.
		//
		start_time = wall_time();

		if (this->log_function && iter % log_interval == 0) {
			if (use_sparsity) {
				if (iter == 0) {
					this->log_function("Itr        f        max|g_i|   alpha    fac    tau   min(H_ii)");
				}
				this->log_function(
					to_string(std::setw(4), iter) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(6), std::setw(10), fval) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), normg) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), alpha) + " " +
					to_string(std::setw(3), factorizations) + "   " +
					to_string(std::scientific, std::setprecision(1), tau) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(2), mindiag)
					);
			}
			else {
				double detH = H.determinant();
				double normH = H.norm();

				if (iter == 0) {
					this->log_function("Itr        f        max|g_i|   ||H||     det(H)    min(H_ii)  alpha    fac");
				}

				this->log_function(
					to_string(std::setw(4), iter) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(6), std::setw(10), fval) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), normg) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), normH) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(3), std::setw(10), detH) + " " +
					to_string(std::scientific, std::showpos, std::setprecision(2), mindiag) + " " +
					to_string(std::scientific, std::setprecision(3), std::setw(9), alpha) + " " +
					to_string(std::setw(3), factorizations)
					);
			}
		}
		results->log_time += wall_time() - start_time;

		fprev = fval;
		iter++;
	}

	function.copy_global_to_user(x);
	results->total_time += wall_time() - global_start_time;

	if (this->log_function) {
		char str[1024];
		std::sprintf(str, " end %+10.6e %.3e", fval, normg);
		this->log_function(str);
	}
}

}  // namespace spii


#include <NTL/ZZ.h>

template<>
void Elt<NTL::ZZ>::mul(const Elt& x) { NTL::mul(this->t, this->t, x.t); }

template<>
void Elt<NTL::ZZ>::mod(const Elt& x) { NTL::QuickRem(this->t, x.t); }

template<>
void Elt<NTL::ZZ>::div(const Elt& x) { NTL::div(this->t, this->t, x.t); }

template<>
void Elt<NTL::ZZ>::mulmod(const Elt& x, const Elt& y) { NTL::MulMod(this->t, this->t, x.t, y.t); }


// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/* least_squares.cc
   Jeremy Barnes, 15 June 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.

   Least squares solution of problems.  Various versions.
*/

#include "least_squares.h"

#include "mldb/plugins/jml/algebra/matrix_ops.h"
#include "mldb/base/exc_assert.h"
#include "svd.h"
#include <boost/timer.hpp>
#include "mldb/arch/timers.h"
#include "lapack.h"
#include <cmath>
#include "mldb/utils/string_functions.h"
#include "mldb/arch/simd_vector.h"
#include "mldb/base/parallel.h"

using namespace std;

namespace ML {

#if 0
template<class Float>
distribution<Float>
least_squares_impl(const boost::multi_array<Float, 2> & A,
                   const distribution<Float> & c,
                   const boost::multi_array<Float, 2> & B,
                   const distribution<Float> & d)
{
    using namespace LAPack;

    size_t m = A.shape()[0];
    size_t n = A.shape()[1];
    size_t p = B.shape()[0];

    //cerr << "A: (mxn) " << A.shape()[0] << " x " << A.shape()[1] << endl;
    //cerr << "B: (pxn) " << B.shape()[0] << " x " << B.shape()[1] << endl;
    //cerr << "c: m     " << c.size() << endl;
    //cerr << "d: p     " << d.size() << endl;

    if (c.size() != m || B.shape()[1] != n || d.size() != p)
        throw Exception("least_squares: sizes didn't match");

    if (p > n || n > (m + p))
        throw Exception("least_squares: overconstrained system");

    // todo: check that B has full row rank p and that the matrix (A B)' has
    // full column rank n.

    distribution<Float> result(n);

    /* We need to transpose them for Fortran, but since they are destroyed
       anyway it's no big deal since they would have been copied. */
    boost::multi_array<Float, 2> AF = fortran(A);
    boost::multi_array<Float, 2> BF = fortran(B);
    distribution<Float> c2 = c;
    distribution<Float> d2 = d;

    int res = gglse(m, n, p,
                    AF.data_begin(), AF.shape()[0],
                    BF.data_begin(), BF.shape()[1],
                    &c2[0], &d2[0], &result[0]);

    if (res != 0)
        throw Exception(format("least_squares(): gglse returned error in arg "
                               "%d", res));

    return result;
}

distribution<float>
least_squares(const boost::multi_array<float, 2> & A,
              const distribution<float> & c,
              const boost::multi_array<float, 2> & B,
              const distribution<float> & d)
{
    return least_squares_impl(A, c, B, d);
}

distribution<double>
least_squares(const boost::multi_array<double, 2> & A,
              const distribution<double> & c,
              const boost::multi_array<double, 2> & B,
              const distribution<double> & d)
{
    return least_squares_impl(A, c, B, d);
}
#endif

template<class Float>
distribution<Float>
least_squares_impl(const boost::multi_array<Float, 2> & A, const distribution<Float> & b)
{
    using namespace std;

    //boost::timer t;

    if (A.shape()[0] != b.size()) {
        cerr << "A.shape()[0] = " << A.shape()[0] << endl;
        cerr << "A.shape()[1] = " << A.shape()[1] << endl;
        cerr << "b.size() = " << b.size() << endl;
        throw Exception("incompatible dimensions for least_squares");
    }

    using namespace LAPack;

    int m = A.shape()[0];
    int n = A.shape()[1];

    distribution<Float> x = b;
    x.resize(std::max<size_t>(m, n));

    boost::multi_array<Float, 2> A2 = A;

#if 0
    using namespace std;
    cerr << "m = " << m << " n = " << n << " A2.shape()[0] = " << A2.shape()[0]
         << " A2.shape()[1] = " << A2.shape()[1] << endl;
    cerr << "A2 = " << endl << A2 << endl;
    cerr << "b = " << b << endl;
#endif
    int res = gels('T', n, m, 1, A2.data(), n, &x[0],
                   x.size());

    if (res < 0)
        throw Exception(format("least_squares(): gels returned error in arg "
                               "%d", -res));

    //if (debug_irls) {
    //    (*debug_irls) << "gels returned " << res << endl;
    //    (*debug_irls) << "x = " << x << endl;
    //}

    if (res > 0) {
        //if (debug_irls)
        //      (*debug_irls) << "retrying; " << res << " are too small" << endl;

        /* Rank-deficient matrix.  Use the more efficient routine. */
        int rank;
        Float rcond = -1.0;
        Float sv[std::min(m, n)];
        std::fill(sv, sv + std::min(m, n), 0.0);

        // Rebuild A2, transposed this time
        A2.resize(boost::extents[n][m]);
        A2 = transpose(A);

        // Rebuild x as it was previously overwritten
        x = b;
        x.resize(std::max<size_t>(m, n));

        res = gelsd(m, n, 1, A2.data(), m, &x[0], x.size(), sv, rcond, rank);

        //if (debug_irls)
        //    (*debug_irls) << "rcond: " << rcond << " rank: "
        //                  << rank << endl;
    }

    if (res < 0) {
        throw Exception(format("least_squares(): gelsy returned error in arg "
                               "%d", -res));
    }

    x.resize(n);

    //using namespace std;
    //cerr << "least_squares: took " << t.elapsed() << "s" << endl;

    return x;
    //cerr << "least squares: gels returned " << x2 << endl;
    //cerr << "least squares: A2 = " << endl << A2 << endl;

    //cerr << "least_squares: " << t.elapsed() << "s" << endl;
    //distribution<Float> x3
    //    = least_squares(A, b, boost::multi_array<Float, 2>(0, n), distribution<Float>());

    //cerr << "least squares: gglse returned " << x3 << endl;

}

distribution<float>
least_squares(const boost::multi_array<float, 2> & A,
              const distribution<float> & b)
{
    return least_squares_impl(A, b);
}

distribution<double>
least_squares(const boost::multi_array<double, 2> & A,
              const distribution<double> & b)
{
    return least_squares_impl(A, b);
}

template<typename Float>
void doDiagMultColumn(const boost::multi_array<Float, 2> & U,
                      const distribution<Float> & d,
                      const boost::multi_array<Float, 2> & V,
                      boost::multi_array<Float, 2> & result,
                      int j)
{
    size_t m = U.shape()[0], x = d.size();

    Float Vj_values[x];
    for (unsigned k = 0;  k < x;  ++k) {
        Vj_values[k] = V[k][j];
    }
    for (unsigned i = 0;  i < m;  ++i) {
        result[i][j] = MLDB::SIMD::vec_accum_prod3(&U[i][0], &d[0], Vj_values, x);
    }
}

template<class Float>
boost::multi_array<Float, 2>
diag_mult_impl(const boost::multi_array<Float, 2> & U,
               const distribution<Float> & d,
               const boost::multi_array<Float, 2> & V,
               bool parallel)
{
    size_t m = U.shape()[0], n = V.shape()[1], x = d.size();

    boost::multi_array<Float, 2> result(boost::extents[m][n]);

    if (U.shape()[1] != x || V.shape()[0] != x)
        throw Exception("diag_mult(): wrong shape");

    auto doColumn = std::bind(&doDiagMultColumn<Float>,
                              std::cref(U),
                              std::cref(d),
                              std::cref(V),
                              std::ref(result),
                              std::placeholders::_1);

    if (parallel)
        MLDB::parallelMap(0, n, doColumn);
    else {
        for (unsigned j = 0;  j < n;  ++j)
            doColumn(j);
    }

    return result;
}

boost::multi_array<float, 2>
diag_mult(const boost::multi_array<float, 2> & U,
          const distribution<float> & d,
          const boost::multi_array<float, 2> & V,
          bool parallel)
{
    return diag_mult_impl<float>(U, d, V, parallel);
}

boost::multi_array<double, 2>
diag_mult(const boost::multi_array<double, 2> & U,
          const distribution<double> & d,
          const boost::multi_array<double, 2> & V,
          bool parallel)
{
    return diag_mult_impl<double>(U, d, V, parallel);
}

template<typename Float>
struct RidgeRegressionIteration {
    double lambda;
    double current_lambda;
    double total_mse_unbiased;
    distribution<Float> x;

    void run(const distribution<Float> & singular_values,
             const boost::multi_array<Float, 2> & A,
             const distribution<Float> & b,
             const boost::multi_array<Float, 2> & VT,
             const boost::multi_array<Float, 2> & U,
             const boost::multi_array<Float, 2> & GK,
             bool debug)
    {
        int m = A.shape()[0];
        int n = A.shape()[1];

        Timer t(debug);

        auto doneStep = [&] (const std::string & where)
        {
            if (!debug)
                return;
            cerr << "      " << where << ": " << t.elapsed() << endl;
            t.restart();
        };

        //cerr << "i = " << i << " current_lambda = " << current_lambda << endl;
        // Adjust the singular values for the new lambda
        distribution<Float> my_singular = singular_values;
        if (current_lambda != lambda)
            my_singular += (current_lambda - lambda);

        //boost::multi_array<Float, 2> GK_pinv
        //    = U * diag((Float)1.0 / my_singular) * VT;

        boost::multi_array<Float, 2> GK_pinv
            = diag_mult(U, (Float)1.0 / my_singular, VT, true /* parallel */);

        doneStep("diag_mult");

        // TODO: reduce GK by removing those basis vectors where the singular
        // values are too close to lambda

        if (debug && false) {
            cerr << "GK_pinv = " << endl << GK_pinv
                 << endl;
            cerr << "prod = " << endl << (GK * GK_pinv * GK) << endl;
            cerr << "prod2 = " << endl << (GK_pinv * GK * GK) << endl;
        }

        boost::multi_array<Float, 2> A_pinv
            = (m < n ? GK_pinv * A : A * GK_pinv);

        doneStep("A_pinv");

        if (debug && false)
            cerr << "A_pinv = " << endl << A_pinv << endl;

        x = b * A_pinv;

        if (debug)
            cerr << "x = " << x << endl;

        distribution<Float> predictions = A * x;

        //cerr << "A: " << A.shape()[0] << "x" << A.shape()[1] << endl;
        //cerr << "A_pinv: " << A_pinv.shape()[0] << "x" << A_pinv.shape()[1]
        //     << endl;

        //boost::multi_array<Float, 2> A_A_pinv
        //    = A * transpose(A_pinv);

        doneStep("predictions");

#if 0
        boost::multi_array<Float, 2> A_A_pinv
        = multiply_transposed(A, A_pinv);

        cerr << "A_A_pinv: " << A_A_pinv.shape()[0] << "x"
        << A_A_pinv.shape()[1] << " m = " << m << endl;

        if (debug && false)
            cerr << "A_A_pinv = " << endl << A_A_pinv << endl;
#else
        // We only need the diagonal of A * A_pinv

        distribution<Float> A_A_pinv_diag(m);
        for (unsigned j = 0;  j < m;  ++j)
            A_A_pinv_diag[j] = SIMD::vec_dotprod_dp(&A[j][0], &A_pinv[j][0], n);
#endif

        doneStep("A_A_pinv");

        // Now figure out the performance
        double total_mse_biased = 0.0;
        for (unsigned j = 0;  j < m;  ++j) {

            if (j < 10 && false)
                cerr << "j = " << j << " b[j] = " << b[j]
                     << " predictions[j] = " << predictions[j]
                     << endl;

            double resid = b[j] - predictions[j];

            // Adjust for the bias cause by training on this example.  This is
            // A * pinv(A), which is A *

            double factor = 1.0 - A_A_pinv_diag[j];

            double resid_unbiased = resid / factor;

            total_mse_biased += (1.0 / m) * resid * resid;
            total_mse_unbiased += (1.0 / m) * resid_unbiased * resid_unbiased;
        }

        doneStep("mse");

        //cerr << "lambda " << current_lambda
        //     << " rmse_biased = " << sqrt(total_mse_biased)
        //     << " rmse_unbiased = " << sqrt(total_mse_unbiased)
        //     << endl;

        //if (sqrt(total_mse_biased) > 1.0) {
        //    cerr << "rmse_biased: x = " << x << endl;
        //}

#if 0
        cerr << "m = " << m << endl;
        cerr << "total_mse_biased   = " << total_mse_biased << endl;
        cerr << "total_mse_unbiased = " << total_mse_unbiased << endl;
        cerr << "best_error = " << best_error << endl;
        cerr << "x = " << x << endl;
#endif
    };

};

// NOTE: this cumbersome construction is to keep clang 3.4 from
// ICEing
template<typename Float>
struct RidgeRegressionIterations: public std::vector<RidgeRegressionIteration<Float> > {
    void run(const distribution<Float> & singular_values,
             const boost::multi_array<Float, 2> & A,
             const distribution<Float> & b,
             const boost::multi_array<Float, 2> & VT,
             const boost::multi_array<Float, 2> & U,
             const boost::multi_array<Float, 2> & GK,
             bool debug,
             int n)
    {
        this->at(n).run(singular_values, A, b, VT, U, GK, debug);
    }
};

template<class Float>
distribution<Float>
ridge_regression_impl(const boost::multi_array<Float, 2> & A,
                      const distribution<Float> & b,
                      float& lambda)
{
    using namespace std;
    float initialLambda = lambda < 0 ? 1e-5 : lambda;
    //cerr << "ridge_regression: A = " << A.shape()[0] << "x" << A.shape()[1]
    //     << " b = " << b.size() << endl;

    //cerr << "b = " << b << endl;
    //cerr << "A = " << A << endl;

    bool debug = false;
    //debug = true;

    Timer t(debug);

    auto doneStep = [&] (const std::string & where)
        {
            if (!debug)
                return;
            cerr << where << ": " << t.elapsed() << endl;
            t.restart();
        };

    // Step 1: SVD

    if (A.shape()[0] != b.size())
        throw Exception("incompatible dimensions for least_squares");

    using namespace LAPack;

    int m = A.shape()[0];
    int n = A.shape()[1];

    int minmn = std::min(m, n);

    // See http://www.clopinet.com/isabelle/Projects/ETH/KernelRidge.pdf

    // The matrix to decompose is square
    boost::multi_array<Float, 2> GK(boost::extents[minmn][minmn]);

    //cerr << "m = " << m << " n = " << n << endl;

    // Take either A * transpose(A) or (A transpose) * A, whichever is smaller
    if (m < n) {
        for (unsigned i1 = 0;  i1 < m;  ++i1)
            for (unsigned i2 = 0;  i2 < m;  ++i2)
                GK[i1][i2] = SIMD::vec_dotprod_dp(&A[i1][0], &A[i2][0], n);

        //for (unsigned i1 = 0;  i1 < m;  ++i1)
        //    for (unsigned i2 = 0;  i2 < m;  ++i2)
        //        for (unsigned j = 0;  j < n;  ++j)
        //            GK[i1][i2] += A[i1][j] * A[i2][j];
    } else {
        // TODO: vectorize and look at loop order
        for (unsigned i = 0;  i < m;  ++i)
            for (unsigned j1 = 0;  j1 < n;  ++j1)
                for (unsigned j2 = 0;  j2 < n;  ++j2)
                    GK[j1][j2] += A[i][j1] * A[i][j2];
    }

    doneStep("    square");

    if (debug)
        cerr << "GK = " << endl << GK << endl;

    //cerr << "GK.shape()[0] = " << GK.shape()[0] << endl;
    //cerr << "GK.shape()[1] = " << GK.shape()[1] << endl;

    // Add in the ridge
    for (unsigned i = 0;  i < minmn;  ++i)
        GK[i][i] += initialLambda;

    if (debug)
        cerr << "GK with ridge = " << endl << GK << endl;

    // Decompose to get the pseudoinverse
    distribution<Float> svalues(minmn);
    boost::multi_array<Float, 2> VT(boost::extents[minmn][minmn]);
    boost::multi_array<Float, 2> U(boost::extents[minmn][minmn]);

    svd_square(GK, VT, U, svalues);

    distribution<Float> singular_values
        (svalues.begin(), svalues.begin() + minmn);

    if (debug)
        cerr << "singular values = " << singular_values << endl;

    if (debug) {
        // Multiply decomposition back to make sure that we get the original
        // matrix
        boost::multi_array<Float, 2> D = diag(singular_values);

        boost::multi_array<Float, 2> GK_test
            = U * D * VT;

        cerr << "GK_test = " << endl << GK_test << endl;
        //cerr << "errors = " << endl << (GK_test - GK) << endl;
    }

    // Figure out the optimal value of lambda based upon leave-one-out cross
    // validation
    distribution<Float> x_best;
    Float best_lambda = initialLambda;
    typedef RidgeRegressionIteration<Float> Iteration;
    if (lambda < 0) {
        double current_lambda = 10.0;

        RidgeRegressionIterations<Float> iterations;

        for (; current_lambda >= 1e-14; current_lambda /= 10.0) {
            Iteration iter;
            iter.lambda = initialLambda;
            iter.current_lambda = current_lambda;
            iterations.push_back(iter);
        };

        MLDB::parallelMap(0, iterations.size(),
                        std::bind(std::mem_fn(&RidgeRegressionIterations<Float>::run),
                                  std::ref(iterations),
                                  std::cref(singular_values),
                                  std::cref(A), std::cref(b),
                                  std::cref(VT), std::cref(U),
                                  std::cref(GK), debug,
                                  std::placeholders::_1));

        //double best_lambda = -1000;
        double best_error = 1000000;

        for (unsigned i = 0;  i < iterations.size();  ++i) {

            if (iterations[i].total_mse_unbiased < best_error || i == 0) {

                //cerr << "best_lambda " << iterations[i].current_lambda << " error : " << iterations[i].total_mse_unbiased << endl;

                x_best = iterations[i].x;
                best_lambda = iterations[i].current_lambda;
                best_error = iterations[i].total_mse_unbiased;
            }
        }
    }
    else {
        Iteration iter;
        iter.lambda = initialLambda;
        iter.current_lambda = initialLambda;
        iter.run(singular_values, A, b, VT, U, GK, debug);
        x_best = iter.x;
    }

    doneStep("    lambda");

    //cerr << "total: " << t.elapsed() << endl;

    lambda = best_lambda; //return the lambda we ended up using
    return x_best;
}

distribution<float>
ridge_regression(const boost::multi_array<float, 2> & A,
                 const distribution<float> & b,
                 float lambda)
{
    return ridge_regression_impl(A, b, lambda);
}

distribution<double>
ridge_regression(const boost::multi_array<double, 2> & A,
                 const distribution<double> & b,
                 float lambda)
{
    return ridge_regression_impl(A, b, lambda);
}

template<class Float>
distribution<Float>
lasso_regression_impl(const boost::multi_array<Float, 2> & A,
                      const distribution<Float> & b,
                      float lambda,
                      int maxIter,
                      float epsilon)
{
    // cerr << "lasso_regression: A = " << A.shape()[0] << "x" << A.shape()[1]
    //     << " b = " << b.size() << " lambda = " << lambda <<" maxIter = "
    //     << maxIter << " epsilon = " << epsilon << endl;

    //  ref: https://www.coursera.org/learn/ml-regression/lecture/AsCvQ/coordinate-descent-for-lasso-unnormalized-features
    //
    //  on why standardisation might be required before using LASSO:
    //  http://stats.stackexchange.com/q/86434/22296

    int n = A.shape()[0];   //Number of samples
    int p = A.shape()[1];   //Number of variables

     distribution<Float> x(p, 0.); //our solution vector

    if (lambda <= 0) {
        x = ridge_regression_impl(A, b, lambda); //Use the ridge regression to determine lambda and use the result as initialization
    }

    Float halflambda = lambda / 2.0f;
    distribution<Float> Atb(p, 0.);                        //Correlation of each variable with the target vector
    boost::multi_array<Float, 2> AtA(boost::extents[p][p]); //Correlation betwen each variables

    //Precompute Atb and AtA
    for (int j = 0; j < p; ++j) {
        for (int i = 0; i < n; ++i) {
            Atb[j] += A[i][j] * b[i];
        }
        //each column dot each column
        for (int i = 0; i < p; ++i) { //todo: optimise for triangular matrix
            Float dotprod = 0.;
            for (int r = 0; r < n; ++r) {
                dotprod += A[r][i] * A[r][j];
            }
            AtA[j][i] = dotprod;
        }
    }

    //Main lasso loop, which is really a coordinate descent where we optimize each variable independently.
    //We do this in a roundrobin fashion.
    int iter = 0;
    do {
        Float max_step = 0.;
        distribution<Float> oldX(p);
        oldX = x;

        for (int j = 0; j < p; ++j) { //for each column / variable

            Float rho = Atb[j];

            for (int i = 0; i < p; ++i){    //scales with the number of variables
                if (i != j)
                    rho -= AtA[j][i] * x[i];
            }

            if (rho > halflambda) {
                x[j] = (rho - halflambda) / (AtA[j][j]);
            }
            else if (rho < -halflambda) {
                x[j] = (rho + halflambda) / (AtA[j][j]);
            }
            else {
                x[j] = 0;
            }

	        //cerr << "x[j]: " << x[j] << endl;
            Float step = fabs(x[j] - oldX[j]);
            if (step > max_step)
                max_step = step;
        }

        // cerr << "max_step: " << max_step << endl;

        // if the biggest step we took was smaller than our threshold, let's
        // stop there and assume we have converged
        if (max_step < epsilon) {
            break;
        }
        if (iter >= maxIter) {
            // if we stopped after max iteration and not because we have
            // converge, let's issue a warning
            cerr << MLDB::format("LASSO did not converge in %i iterations, last "
                               " max_step > eps (%f > %f)",
                               iter, max_step, epsilon);
            break;
        }
        iter++;
    } while (true); //until convergence

    return x;
}

distribution<float>
lasso_regression(const boost::multi_array<float, 2> & A,
                 const distribution<float> & b,
                 float lambda,
                 int maxIter,
                 float epsilon)
{
    return lasso_regression_impl(A, b, lambda, maxIter, epsilon);
}

distribution<double>
lasso_regression(const boost::multi_array<double, 2> & A,
                 const distribution<double> & b,
                 float lambda,
                 int maxIter,
                 float epsilon)
{
    return lasso_regression_impl(A, b, lambda, maxIter, epsilon);
}

//***********************************************

template<class Float>
void doWeightedSquareRow(const boost::multi_array<Float, 2> & XT,
                         const distribution<Float> & d,
                         boost::multi_array<Float, 2> & result,
                         int i)
{
    int chunk_size = 2048;  // ensure we fit in the cache

    size_t nx = XT.shape()[1];
    size_t nv = XT.shape()[0];

    int x = 0;
    while (x < nx) {
        int nxc = std::min<size_t>(chunk_size, nx - x);
        distribution<Float> Xid(chunk_size);
        SIMD::vec_prod(&XT[i][x], &d[x], &Xid[0], nxc);

        for (unsigned j = 0;  j < nv;  ++j) {
            result[i][j] += SIMD::vec_dotprod_dp(&XT[j][x], &Xid[0], nxc);
        }

        x += nxc;
    }

}

template<class Float>
boost::multi_array<Float, 2>
weighted_square_impl(const boost::multi_array<Float, 2> & XT,
                     const distribution<Float> & d)
{
    if (XT.shape()[1] != d.size())
        throw Exception("Incompatible matrix sizes for weighted_square");

    size_t nx = XT.shape()[1];
    size_t nv = XT.shape()[0];

    //cerr << "nx = " << nx << " nv = " << nv << endl;

    boost::multi_array<Float, 2> result(boost::extents[nv][nv]);

    if (false) {
        int chunk_size = 2048;  // ensure we fit in the cache
        distribution<Float> Xid(chunk_size);

        int x = 0;
        while (x < nx) {
            int nxc = std::min<size_t>(chunk_size, nx - x);

            for (unsigned i = 0;  i < nv;  ++i) {
                SIMD::vec_prod(&XT[i][x], &d[x], &Xid[0], nxc);

                for (unsigned j = 0;  j < nv;  ++j) {
                    result[i][j] += SIMD::vec_dotprod_dp(&XT[j][x], &Xid[0], nxc);
                }
            }

            x += nxc;
        }
    } else {
        MLDB::parallelMap(0, nv, std::bind(doWeightedSquareRow<Float>,
                                                 std::cref(XT),
                                                 std::cref(d),
                                                 std::ref(result),
                                                 std::placeholders::_1));
    }

    return result;
}

boost::multi_array<float, 2>
weighted_square(const boost::multi_array<float, 2> & XT,
                const distribution<float> & d)
{
    return weighted_square_impl(XT, d);
}

boost::multi_array<double, 2>
weighted_square(const boost::multi_array<double, 2> & XT,
                const distribution<double> & d)
{
    return weighted_square_impl(XT, d);
}

template<typename Float>
void svd_square_impl(boost::multi_array<Float, 2> & X,
                     boost::multi_array<Float, 2> & VT,
                     boost::multi_array<Float, 2> & U,
                     distribution<Float> & svalues)
{
    size_t minmn = X.shape()[0];
    ExcAssertEqual(minmn, X.shape()[1]);

    // Decompose to get the pseudoinverse
    svalues.clear();
    svalues.resize(minmn);

    VT.resize(boost::extents[minmn][minmn]);
    U.resize(boost::extents[minmn][minmn]);

    // SVD
    int result = LAPack::gesdd("S", minmn, minmn,
                               X.data(), minmn,
                               &svalues[0],
                               &VT[0][0], minmn,
                               &U[0][0], minmn);

    if (result != 0)
        throw Exception("gesdd returned non-zero");
}

void svd_square(boost::multi_array<float, 2> & X,
                boost::multi_array<float, 2> & VT,
                boost::multi_array<float, 2> & U,
                distribution<float> & svalues)
{
    return svd_square_impl(X, VT, U, svalues);
}

void svd_square(boost::multi_array<double, 2> & X,
                boost::multi_array<double, 2> & VT,
                boost::multi_array<double, 2> & U,
                distribution<double> & svalues)
{
    return svd_square_impl(X, VT, U, svalues);
}

} // namespace ML


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IDIVROUND2EVEN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IDIVROUND2EVEN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing idivround2even capabilities

    Computes the integer conversion of the rounded to even
    division of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = idivround2even_(x, y);
    @endcode

    The code is similar to:

    @code
    as_integer_t<T> r = toints(round2even(x/y));
    @endcode

    If y is @ref Zero, it returns @ref Valmax (resp. @ref Valmin)
    if x is positive (resp. negative) and 0 if x is @ref Zero.

    @see toints, round2even

  **/
  const boost::dispatch::functor<tag::idivround2even_> idivround2even = {};
} }
#endif

#include <boost/simd/function/scalar/idivround2even.hpp>
#include <boost/simd/function/simd/idivround2even.hpp>

#endif


#include <vector>
#include <string>
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <set>

int findBestPack( const std::vector<boost::tuple<std::string , int , int> > & items ,std::set<int> & bestItems , const int weightlimit ) {
	//dynamic programming approach sacrificing storage space for execution
	//time , creating a table of optimal values for every weight and a
	//second table of sets with the items collected so far in the knapsack
	//the best value is in the bottom right corner of the values table,
	//the set of items in the bottom right corner of the sets' table.
@@ begin question optimal_knapsack
@@ description: use dynamic programming to find the best assingment of weights to the knapsack
@@ points: 100
@@ time: 40 minutes

@@ end question
	bestItems.swap( solutionSets[ n - 1][ weightlimit - 1 ] ) ;
	return bestValues[ n - 1 ][ weightlimit - 1 ] ;
}


// All content Copyright (C) 2018 Genomics plc
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "caller/diploid/genotypeUtils.hpp"
#include "utils/indexedProduct.hpp"

namespace wecall
{
namespace caller
{
    namespace model
    {
        std::vector< double > computeGenotypeLikelihoods( const variant::GenotypeVector & genotypes,
                                                          const utils::matrix_t & probReadsGivenHaplotypes,
                                                          const variant::HaplotypeVector & haplotypes )
        {
            std::vector< double > genotypeLikelihoods( genotypes.size() );
            const auto nReads = probReadsGivenHaplotypes.size1();
            WECALL_ASSERT( nReads > 0, "Only makes sense to compute likelihoods if there is read-data" );

            for ( std::size_t genotypeIndex = 0; genotypeIndex < genotypes.size(); ++genotypeIndex )
            {
                double thisGenotypeLogLikelihood = 0.0;
                const auto hapIndicies = genotypes.getHaplotypeIndices( genotypeIndex );

                // prior probability of any haplotype (given genotype).  Assumes uniform distribution across haplotypes
                const auto haplotypePriorProbability = 1.0 / static_cast< double >( hapIndicies.size() );

                for ( std::size_t readIndex = 0; readIndex < nReads; ++readIndex )
                {
                    double sumReadLikelihoodsOverHaplotypes = 0.0;
                    for ( const auto & hapIndex : hapIndicies )
                    {
                        sumReadLikelihoodsOverHaplotypes += probReadsGivenHaplotypes( readIndex, hapIndex );
                    }
                    thisGenotypeLogLikelihood +=
                        std::log( sumReadLikelihoodsOverHaplotypes * haplotypePriorProbability );
                }

                genotypeLikelihoods[genotypeIndex] = thisGenotypeLogLikelihood;
            }

            // Now re-scale all the genotype likelihoods for this individual, to prevent underflows, and
            rescaleLogLikelihoods( genotypeLikelihoods, nReads > 0 );

            // store the likelihoods as non-log values
            std::transform( genotypeLikelihoods.begin(), genotypeLikelihoods.end(), genotypeLikelihoods.begin(), exp );

            if ( false )
            {
                // print likelihoods of genotypes
                WECALL_LOG( SUPER_DEBUG, "Genotype likelihoods:-" );
                for ( std::size_t genotypeIndex = 0; genotypeIndex < genotypes.size(); ++genotypeIndex )
                {
                    if ( genotypeLikelihoods[genotypeIndex] > 1e-6 )
                    {
                        WECALL_LOG( SUPER_DEBUG, std::to_string( genotypeLikelihoods[genotypeIndex] ) + " for " +
                                                      genotypes[genotypeIndex]->toString( haplotypes ) );
                    }
                }
            }

            return genotypeLikelihoods;
        }
    }
}
}


#include "fbstab/components/dense_cholesky_solver.h"

#include <Eigen/Dense>
#include <cmath>

#include "fbstab/components/dense_data.h"
#include "fbstab/components/full_residual.h"
#include "fbstab/components/full_variable.h"

namespace fbstab {

DenseCholeskySolver::DenseCholeskySolver(int nz, int nl, int nv)
    : ldlt_(nz + nl) {
  if (nz <= 0 || nv <= 0 || nl < 0) {
    throw std::runtime_error(
        "In DenseCholeskySolver: nz and nv must be > 0 and nl >= 0");
  }
  nz_ = nz;
  nl_ = nl;
  nv_ = nv;

  K_.resize(nz_ + nl_, nz_ + nl_);
  E_.resize(nz_, nz_);
  r1_.resize(nz_ + nl_);
  r2_.resize(nv_);
  Gamma_.resize(nv_);
  mus_.resize(nv_);
  gamma_.resize(nv_);
  B_.resize(nv_, nz_);
}

bool DenseCholeskySolver::Initialize(const FullVariable& x,
                                     const FullVariable& xbar, double sigma) {
  NullDataCheck();
  if (!x.SameSize(xbar)) {
    throw std::runtime_error(
        "In DenseCholeskySolver::Factor: inputs must be the same size");
  }
  if (xbar.nz_ != nz_ || xbar.nv_ != nv_) {
    throw std::runtime_error(
        "In DenseCholeskySolver::Factor: inputs must match object size.");
  }
  if (sigma <= 0) {
    throw std::runtime_error(
        "In DenseCholeskySolver::Factor: sigma must be positive.");
  }
  const auto& H = data_->H_;
  const auto& G = data_->G_;
  const auto& A = data_->A_;

  // E = H + sigma I + A'*diag(Gamma(x))*A
  E_ = H + sigma * Eigen::MatrixXd::Identity(nz_, nz_);
  Eigen::Vector2d pfb_gradient;
  for (int i = 0; i < nv_; i++) {
    const double ys = x.y(i) + sigma * (x.v(i) - xbar.v(i));
    pfb_gradient = PFBGradient(ys, x.v(i));
    gamma_(i) = pfb_gradient(0);
    mus_(i) = pfb_gradient(1) + sigma * pfb_gradient(0);
    Gamma_(i) = gamma_(i) / mus_(i);
  }
  // B is used to avoid temporaries
  B_.noalias() = Gamma_.asDiagonal() * A;
  E_.noalias() += A.transpose() * B_;

  // K = [E G']
  //     [G -S]
  K_.block(0, 0, nz_, nz_) = E_;
  K_.block(nz_, 0, nl_, nz_) = G;
  K_.block(nz_, nz_, nl_, nl_) = -sigma * Eigen::MatrixXd::Identity(nl_, nl_);

  // Factor K = LDL'
  ldlt_.compute(K_);
  Eigen::ComputationInfo status = ldlt_.info();
  if (status != Eigen::Success) {
    return false;
  } else {
    return true;
  }
}

bool DenseCholeskySolver::Solve(const FullResidual& r, FullVariable* x) const {
  if (x == nullptr) {
    throw std::runtime_error(
        "In DenseCholeskySolver::Solve: x cannot be null.");
  }
  if (!r.SameSize(*x)) {
    throw std::runtime_error(
        "In DenseCholeskySolver::Solve residual and variable objects must be "
        "the same size");
  }
  if (x->nz_ != nz_ || x->nv_ != nv_ || x->nl_ != nl_) {
    throw std::runtime_error(
        "In DenseCholeskySolver::Factor: inputs must match object size.");
  }
  const auto& A = data_->A_;
  const auto& b = data_->b_;

  // This method solves the system:
  // [E G'] [z] = [rz - A'*D^-1 * rv]
  // [G -S] [l]   [-rl              ]
  // Dv = rv + C*A*z
  // Where D = diag(mus), C = diag(gamma) and K has been precomputed by the
  // factor routine. See (28) and (29) in https://arxiv.org/pdf/1901.04046.pdf

  // Compute rz - A'*(rv./mus) and store it in r1_.
  r2_ = r.v().cwiseQuotient(mus_);
  // r1_.noalias() = r.z() - A.transpose() * r2_;
  r1_.segment(0, nz_).noalias() = r.z() - A.transpose() * r2_;
  r1_.segment(nz_, nl_) = -r.l();

  // Solve using the precomputed factorization then extract
  r1_ = ldlt_.solve(r1_);
  x->z() = r1_.segment(0, nz_);
  x->l() = r1_.segment(nz_, nl_);

  // Compute v = diag(1/mus) * (rv + diag(gamma)*A*z)
  // written so as to avoid temporary creation
  r2_.noalias() = A * x->z();
  r2_.noalias() = gamma_.asDiagonal() * r2_;
  r2_.noalias() += r.v();
  x->v() = r2_.cwiseQuotient(mus_);

  // y = b - Az
  x->y() = b - A * x->z();

  return true;
}

Eigen::Vector2d DenseCholeskySolver::PFBGradient(double a, double b) const {
  const double r = sqrt(a * a + b * b);
  const double d = 1.0 / sqrt(2.0);

  Eigen::Vector2d v;
  if (r < zero_tolerance_) {
    v(0) = alpha_ * (1.0 - d);
    v(1) = alpha_ * (1.0 - d);

  } else if ((a > 0) && (b > 0)) {
    v(0) = alpha_ * (1.0 - a / r) + (1.0 - alpha_) * b;
    v(1) = alpha_ * (1.0 - b / r) + (1.0 - alpha_) * a;

  } else {
    v(0) = alpha_ * (1.0 - a / r);
    v(1) = alpha_ * (1.0 - b / r);
  }

  return v;
}

void DenseCholeskySolver::NullDataCheck() const {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "DenseCholeskySolver tried to access problem data before it's linked.");
  }
}

}  // namespace fbstab


//
// Copyright (c) 2019 INRIA
//

#include "pinocchio/multibody/model.hpp"

# include <eigen3/Eigen/Core>
#include "pinocchio/math/matrix.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_matrix_matrix_product)
{
  using namespace pinocchio;
  using namespace Eigen;
  const Eigen::DenseIndex m = 20, n = 100;
  MatrixXd M1(MatrixXd::Ones(m,n)), M2(MatrixXd::Ones(n,m));
  MatrixMatrixProduct<MatrixXd,MatrixXd>::type res = M1 * M2;
  BOOST_CHECK(!res.eval().isZero());
}

BOOST_AUTO_TEST_CASE(test_scalar_matrix_product)
{
  using namespace pinocchio;
  using namespace Eigen;
  const Eigen::DenseIndex m = 20, n = 100;
  MatrixXd M(MatrixXd::Ones(m,n));
  const double alpha = 0.;
  ScalarMatrixProduct<double,MatrixXd>::type res = alpha * M;
  BOOST_CHECK(res.eval().isZero());
}

BOOST_AUTO_TEST_CASE(test_matrix_scalar_product)
{
  using namespace pinocchio;
  using namespace Eigen;
  const Eigen::DenseIndex m = 20, n = 100;
  MatrixXd M(MatrixXd::Ones(m,n));
  const double alpha = 1.;
  MatrixScalarProduct<MatrixXd,double>::type res = M * alpha;
  BOOST_CHECK(res.eval() == M);
}

BOOST_AUTO_TEST_SUITE_END()


#include <NTL/mat_GF2.h>
#include <NTL/mat_lzz_p.h>

NTL_CLIENT

void random(mat_zz_p& X, long n, long m)
{
   X.SetDims(n, m);
   long i, j;

   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         random(X[i][j]);
}

void random(vec_zz_p& X, long n)
{
   X.SetLength(n);
   long i;

   for (i = 0; i < n; i++)
      random(X[i]);
}

void cvt(mat_GF2& x, const mat_zz_p& a)
{
   long n = a.NumRows();
   long m = a.NumCols();

   x.SetDims(n, m);

   long i, j;

   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         x.put(i, j, rep(a[i][j]));
}

void cvt(vec_GF2& x, const vec_zz_p& a)
{
   long n = a.length();

   x.SetLength(n);

   long i;

   for (i = 0; i < n; i++)
      x.put(i, rep(a[i]));
}

int main()
{
   zz_p::init(2);

   long i;

   vec_GF2 v;
   v.SetLength(5);
   v[1] = 1;
   v[0] = v[1];

   if (v[0] != v[1]) Error("BitMatTest not OK!!");

   for (i=0; i < 8; i++) {
      mat_zz_p a, x;
      mat_GF2 A, X, X1;

      long n = RandomBnd(500) + 1;
      long m = RandomBnd(500) + 1;
      cerr << n << " " << m << "\n";

      double t;

      random(a, n, m);

      t = GetTime();
      kernel(x, a);
      t = GetTime() - t;  cerr << t << "\n";

      cvt(A, a);

      t = GetTime();
      kernel(X, A);
      t = GetTime() - t;  cerr << t << "\n";

      cerr << x.NumRows() << "\n";

      cvt(X1, x);

      if (X1 != X) Error("BitMatTest NOT OK!!");

      if (!IsZero(X*A)) Error("BitMatTest NOT OK!!");

      cerr << "\n";
   }

   cerr << "BitMatTest OK\n";

}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#include "test.hpp"
#include <boost/math/special_functions/legendre.hpp>
#include <eve/function/legendre.hpp>
#include <eve/function/diff/legendre.hpp>
#include <eve/function/is_odd.hpp>
#include <cmath>

 //==================================================================================================
 //== Types tests
 //==================================================================================================
 EVE_TEST_TYPES( "Check return types of legendre on wide"
         , eve::test::simd::ieee_reals

         )
 <typename T>(eve::as<T>)
 {
   using v_t = eve::element_type_t<T>;
   using wi_t = eve::as_integer_t<T>;
   using i_t  = eve::as_integer_t<v_t>;
   TTS_EXPR_IS( eve::legendre(i_t(), T())  , T);
   TTS_EXPR_IS( eve::legendre(wi_t(), T())  , T);
   TTS_EXPR_IS( eve::legendre(i_t(), v_t())  , v_t);
   TTS_EXPR_IS( eve::legendre(wi_t(), v_t())  , T);

 };

 //==================================================================================================
 //== legendre tests
 //==================================================================================================
 EVE_TEST( "Check behavior of legendre p on wide"
         , eve::test::simd::ieee_reals
         , eve::test::generate(eve::test::between(-1, 1), eve::test::as_integer(eve::test::ramp(0)))
         )
   <typename T, typename I>(T const& a0,I const & i0)
 {
   using v_t = eve::element_type_t<T>;
   auto eve__legendrev  =  [](auto n, auto x) { return eve::legendre(n, x); };
   for(unsigned int n=0; n < 5; ++n)
   {
     auto boost_legendre =  [&](auto i, auto) { return boost::math::legendre_p(n, a0.get(i)); };
     TTS_ULP_EQUAL(eve__legendrev(n, a0), T(boost_legendre), 100);
   }
   auto boost_legendrev =  [&](auto i, auto) { return boost::math::legendre_p(i0.get(i), a0.get(i)); };
   TTS_ULP_EQUAL(eve__legendrev(i0    , a0), T(boost_legendrev), 100);
   for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
   {
     auto boost_legendre2 =  [&](auto i, auto) { return boost::math::legendre_p(i0.get(i), a0.get(j)); };
     TTS_RELATIVE_EQUAL(eve__legendrev(i0 , a0.get(j)), T(boost_legendre2), 0.01);
   }
   for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
   {
     for(unsigned int n=0; n < eve::cardinal_v<T>; ++n)
     {
       TTS_RELATIVE_EQUAL(eve__legendrev(i0.get(j) , a0.get(n)), v_t(boost::math::legendre_p(i0.get(j), a0.get(n))), 0.01);
     }
   }
 };

 //==================================================================================================
 //== legendre tests
 //==================================================================================================
 EVE_TEST( "Check behavior of legendre q on wide"
         , eve::test::simd::ieee_reals
         , eve::test::generate(eve::test::between(-1.0, 1.0), eve::test::as_integer(eve::test::ramp(0)))
         )
   <typename T, typename I>(T const& a0,I const & i0)
 {
   using v_t = eve::element_type_t<T>;
   auto eve__legendrev  =  [](auto n, auto x) { return eve::q_kind(eve::legendre)(n, x); };
   for(unsigned int n=0; n < 5; ++n)
   {
     auto boost_legendre =  [&](auto i, auto) { return boost::math::legendre_q(n, a0.get(i)); };
     TTS_ULP_EQUAL(eve__legendrev(n, a0), T(boost_legendre), 100);
   }

   auto boost_legendrev =  [&](auto i, auto) { return boost::math::legendre_q(i0.get(i), a0.get(i)); };
   TTS_ULP_EQUAL(eve__legendrev(i0    , a0), T(boost_legendrev), 100);
   for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
   {
     auto boost_legendre2 =  [&](auto i, auto) { return boost::math::legendre_q(i0.get(i), a0.get(j)); };
     TTS_RELATIVE_EQUAL(eve__legendrev(i0 , a0.get(j)), T(boost_legendre2),  0.01);
   }

   for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
   {
     for(unsigned int n=0; n < eve::cardinal_v<T>; ++n)
     {
       TTS_RELATIVE_EQUAL(eve__legendrev(i0.get(j) , a0.get(n)), v_t(boost::math::legendre_q(i0.get(j), a0.get(n))), 0.01);
     }
   }
 };

 EVE_TEST( "Check behavior of diff(legendre) on wide"
         , eve::test::simd::ieee_reals
         , eve::test::generate(eve::test::between(-1, 1), eve::test::as_integer(eve::test::ramp(0)))
         )
   <typename T, typename I>(T const& a0,I const & i0)
 {
   using v_t = eve::element_type_t<T>;
   auto eve__legendrev  =  [](auto n, auto x) { return eve::diff(eve::legendre)(n, x); };
   for(unsigned int n=0; n < 5; ++n)
   {
     auto boost_legendre =  [&](auto i, auto) { return boost::math::legendre_p_prime(n, a0.get(i)); };
     TTS_ULP_EQUAL(eve__legendrev(n, a0), T(boost_legendre), 100);
   }

   auto boost_legendrev =  [&](auto i, auto) { return boost::math::legendre_p_prime(i0.get(i), a0.get(i)); };
   TTS_ULP_EQUAL(eve__legendrev(i0    , a0), T(boost_legendrev), 100);

   for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
   {
     auto boost_legendre2 =  [i0, a0, j](auto i, auto) { return boost::math::legendre_p_prime(i0.get(i), a0.get(j)); };
     TTS_RELATIVE_EQUAL(eve__legendrev(i0 , a0.get(j)), T(boost_legendre2), 0.01);
   }

   for(unsigned int j=0; j < eve::cardinal_v<T>; ++j)
   {
     for(unsigned int n=0; n < eve::cardinal_v<T>; ++n)
     {
       TTS_RELATIVE_EQUAL(eve__legendrev(i0.get(j) , a0.get(n)), v_t(boost::math::legendre_p_prime(i0.get(j), a0.get(n))), 0.01);
     }
   }
 };

/////////////associated p legendre
EVE_TEST( "Check behavior of associated legendre p on wide"
        , eve::test::simd::ieee_doubles
        , eve::test::generate(eve::test::between(-1.0, 1.0)
                             , eve::test::as_integer(eve::test::ramp(0))
                             , eve::test::as_integer(eve::test::reverse_ramp(0)))
        )
 <typename T, typename I>(T a0, I i0, I j0)
{
//   using v_t = eve::element_type_t<T>;
  auto eve__legendrev  =  [](auto m, auto n, auto x) { return eve::legendre(m, n, x); };
  auto cse__legendrev  =  [](auto m, auto n, auto x) { return eve::condon_shortey(eve::legendre)(m, n, x); };
  auto boost_legendrev =  [](auto m, auto n, auto x) { return boost::math::legendre_p(m, n, x); };
  auto std_assoc = [](auto m, auto n, auto x) { return std::assoc_legendre(m, n, x); };
  for(unsigned int k=0; k < eve::cardinal_v < T > ; ++k)
  {
    for(unsigned int n=0; n < eve::cardinal_v < I > ; ++n)
    {
      for(unsigned int p=0; p < eve::cardinal_v < I > ; ++p)
      {
        TTS_ULP_EQUAL(eve__legendrev(n, p, a0.get(k)), std_assoc(n, p, a0.get(k)), 100);
        TTS_RELATIVE_EQUAL(cse__legendrev(n, p, a0.get(k)), boost_legendrev(n, p, a0.get(k)), 0.01);
      }
    }
  }
  TTS_ULP_EQUAL(eve__legendrev(j0, i0, a0), map(std_assoc, j0, i0, a0), 100);
  TTS_ULP_EQUAL(cse__legendrev(i0, j0, a0), map(boost_legendrev, i0, j0, a0), 100);
};


// Copyright (c) 2013-2016. This code was produced by the
// Australian Centre for Field Robotics, The University of Sydney under
// the Future Flight Planning project, University Reference 13996, contract
// NSW-CPS-2011-015264, Work Orders 5, 7 and 8. The intellectual property
// ownership is as set out in these contracts, as registered with
// Commercial Development and Industry Partnerships.

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <Eigen/Geometry>
#include <comma/base/exception.h>
#include "../../math/range_bearing_elevation.h"
#include "sample.h"

namespace snark { namespace spherical {

namespace impl {

boost::mt19937 generator;
boost::uniform_real< double > distribution( 0, 1 );
boost::variate_generator< boost::mt19937&, boost::uniform_real< double > > random( generator, distribution );

} // namespace impl {

coordinates pretty_uniform_sample( const coordinates& centre, double radius )
{
    if( radius >= M_PI ) { COMMA_THROW( comma::exception, "support containing circle radius less than pi; got " << radius ); }
    const Eigen::Matrix3d& r1 = Eigen::AngleAxis< double >( centre.longitude, Eigen::Vector3d( 0, 0, 1 ) ).toRotationMatrix();
    double a = ( impl::random() * 2 - 1 ) * radius; // double a = impl::random() * M_PI * 2;
    double b = ( impl::random() * 2 - 1 ) * radius; // double b = std::sqrt( impl::random() ) * radius;
    const Eigen::Vector3d& s = snark::range_bearing_elevation( 1, a, b ).to_cartesian(); // const Eigen::Vector3d& s = snark::range_bearing_elevation( 1, b * std::cos( a ), b * std::sin( a ) ).to_cartesian();
    const Eigen::Matrix3d& r2 = Eigen::AngleAxis< double >( centre.latitude, Eigen::Vector3d( 0, -1, 0 ) ).toRotationMatrix();
    return coordinates( r1 * r2 * s );
}

} } // namespace snark { namespace spherical {