// solving A * X = B
// using driver function gesv()

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <boost/numeric/bindings/atlas/cblas.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::size_t;
using std::cout;
using std::endl;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
#endif

int main() {

  cout << endl;

  size_t n = 5;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs), bb (n, nrhs);
  // b -- right-hand side matrix, see below

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t const aa (a); // copy of a, because a is `lost' after gesv()

  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  atlas::set (1., xc0);
  atlas::set (2., xc1);
  atlas::gemm (a, x, bb);  // bb = a x, so we know the result ;o)

  print_m (a, "A");
  cout << endl;
  print_m (bb, "B");
  cout << endl;

  // see leading comments for `gesv()' in clapack.hpp
#ifndef F_ROW_MAJOR
  m_t b (bb);
#else
  m_t b (ublas::trans (bb));
#endif
  print_m (b, "B for gesv()");
  cout << endl;

  atlas::gesv (a, b);  // solving the system, b contains x

#ifndef F_ROW_MAJOR
  print_m (b, "X");
  cout << endl;
  atlas::gemm (aa, b, x);
#else
  print_m (b, "X^T");
  cout << endl;
  atlas::gemm (CblasNoTrans, CblasTrans, 1.0, aa, b, 0.0, x);
#endif
  print_m (x, "B = A X");

  cout << endl;

}


/*
 * Matrix-vector multiply
 *
 * Information on Intel vector pragmas is available in the following link:
 * https://software.intel.com/en-us/cpp-compiler-developer-guide-and-reference-vector-1#58209E46-70EA-4C47-BED6-E69236C6680C
 */

// https://en.cppreference.com/w/cpp/memory/c/aligned_alloc
#include <cstdlib>  // aligned_alloc (C++11), std::aligned_alloc (C++17)
#include <cstring>
#include <ctime>
#include <iostream>
#include <vector>
#include <boost/align/aligned_allocator.hpp>
#include <cmath>
// #include "papi.h"
#include "simd.h"
#include "environ.h"
#include "utils.h"
#include "vutils.h"

///////////////////////////////////////////////////////////////////////////////
// USER CONFIGURATION
///////////////////////////////////////////////////////////////////////////////
// Square matrix dimension
const size_t N = 6;

// Precision for floating-point operations
// Valid values are: 4, 8
#define REAL_TYPE 4

///////////////////////////////////////////////////////////////////////////////
// FP and SIMD
///////////////////////////////////////////////////////////////////////////////
// Precision for floating-point operations
#if REAL_TYPE == 4
typedef float real;
typedef SIMD_FLT vreal;
#elif REAL_TYPE == 8
typedef double real;
typedef SIMD_DBL vreal;
#endif

// Number of floating-point values that fit into a SIMD register
const int SIMD_STREAMS = SIMD_WIDTH_BYTES / sizeof(real);
const int LOG2STREAMS = std::log2(SIMD_STREAMS);

///////////////////////////////////////////////////////////////////////////////
// PROGRAM
///////////////////////////////////////////////////////////////////////////////
template <typename T>
using aligned_vector = std::vector<T, boost::alignment::aligned_allocator<T, SIMD_WIDTH_BYTES>>;

void gemv(
    const size_t n,
    const size_t lda,
    const aligned_vector<real> v1,
    const aligned_vector<real> v2,
    aligned_vector<real> &dp)
{
    for (size_t row = 0; row < n; row++) {
        for (size_t col = 0; col < n; col++) {
            dp[row] += v1[row * lda + col] * v2[col];
        }
    }
}

void gemv_simd_tree_sum(
    const size_t n,
    const size_t lda,
    const real *v1,
    const real *v2,
    real *dp)
{
    // const real *_v1 = v1; __SIMD_ASSUME_ALIGNED__(v1);
    // const real *_v2 = v2; __SIMD_ASSUME_ALIGNED__(v2);
    const real *_v1 = (real *)__SIMD_ASSUME_ALIGNED__(v1);
    const real *_v2 = (real *)__SIMD_ASSUME_ALIGNED__(v2);

    for (size_t row = 0; row < n; row++) {
        vreal vdp;
        simd_set_zero(&vdp);
        for (size_t col = 0; col < lda; col+=SIMD_STREAMS) {
            vreal vv1 = simd_load(&_v1[row * lda + col]);
            vreal vv2 = simd_load(&_v2[col]);
            vdp = simd_fmadd(vv1, vv2, vdp);
        }

        // Binary tree sum reduction
        for (size_t i = 0; i < LOG2STREAMS - 1; i++) {
            vdp = simd_hadd(vdp, vdp);
        }

        real tdp[SIMD_STREAMS] __SIMD_ALIGN__;
        simd_store(tdp, vdp);
        // NOTE: 'dp' does need to be aligned because it is used to store a scalar value.
        if (SIMD_WIDTH_BYTES == 16) {
            // HADD from SSE3 does not interleave horizontal sums.
            dp[row] = tdp[0] + tdp[1];
        } else if (SIMD_WIDTH_BYTES == 32) {
            dp[row] = tdp[0] + tdp[SIMD_STREAMS / 2];
        }
    }
}

void print_matrix(
    const size_t n,
    const size_t m,
    const size_t lda,
    const real *v)
{
    for (size_t row = 0; row < n; row++) {
        for (size_t col = 0; col < m; col++) {
            std::cout << v[row * lda + col] << ", ";
        }
        std::cout << std::endl;
    }

}

int main(int argc, char *argv[])
{
    size_t num_matrices = 1;
    if (argc > 1) {
        num_matrices = std::atoi(argv[1]);
    }

    detectCPU();
    detectSIMD();

    std::cout << "Alignment: " << SIMD_WIDTH_BYTES << std::endl;
    std::cout << "Num. elems: " << SIMD_STREAMS << std::endl;
    std::cout << "Log2 SIMD: " << LOG2STREAMS << std::endl;

    // Number of elements in padded matrix column to conform with SIMD alignment
    const size_t lda = (((N * sizeof(real)) / SIMD_WIDTH_BYTES) * SIMD_WIDTH_BYTES + SIMD_WIDTH_BYTES) / sizeof(real);
    // For unaligned rows, set LDA to N
    // const size_t lda = N;

    // Create a vector of given size
    aligned_vector<real> v1(N * lda);      // matrix
    aligned_vector<real> v2(1 * lda, 1.);  // column vector, set to 1 --> add rows of matrix
    aligned_vector<real> dp(N, 0.);  // resulting column vector (dot products)

    // Zero out extra rows used for padding, to prevent floating-point exception during vector multiplication.
    // This memory elements are never modified, so set once.
    std::memset(v2.data() + N, 0, (lda - N) * sizeof(real));

    // real *v1 = NULL;
    // real *v2 = NULL;
    // real *dp = NULL;
    // scalar_malloc(&v1, SIMD_WIDTH_BYTES, N * lda);
    // scalar_malloc(&v2, SIMD_WIDTH_BYTES, 1 * lda);
    // scalar_malloc(&dp, SIMD_WIDTH_BYTES, N);

    for (size_t k = 0; k < num_matrices; k++) {

        // Initialize vector
        for (size_t row = 0; row < N; row++) {
            for (size_t col = 0; col < N; col++) {
                v1[row * lda + col] = k + 1. * (row + col);
            }
        }

#if defined(DEBUG)
        // Print matrix and vector
        std::cout << "Matrix:" << std::endl;
        print_matrix(N, lda, lda, v1.data());
        std::cout << std::endl;

        std::cout << "Vector:" << std::endl;
        print_matrix(lda, 1, 1, v2.data());
        std::cout << std::endl;
#endif

        // Matrix-vector multiply
        gemv_simd_tree_sum(N, lda, v1.data(), v2.data(), dp.data());
        gemv(N, lda, v1, v2, dp);

#if defined(DEBUG)
        // Print resulting column vector
        std::cout << "Result:" << std::endl;
        print_matrix(N, 1, 1, dp.data());
        std::cout << std::endl;
#endif
    }

    std::cout << "Matrix size: " << N << " x " << lda << std::endl;

    // Only needed if this array was allocated using 'scalar_malloc'.
    // scalar_free(&v1);
    // scalar_free(&v2);
    // scalar_free(&dp);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup trigo_constant
 * \defgroup trigo_constant_radindeg Radindeg
 *
 * \par Description
 * Constant Radindeg : Degree in radian multiplier, \f$\frac{180}\pi\f$.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/radindeg.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::_radindeg_(A0)>::type
 *     Radindeg();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Radindeg
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Radindeg, double
                                , 57, 0x42652ee1
                                , 0x404ca5dc1a63c1f8ll
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Radindeg, Radindeg);
}

namespace nt2
{
  static const long double long_radindeg =  57.295779513082320876798154814105l;
}

#endif


#include <boost/test/unit_test.hpp>
#include "algorithms/math/square_root.hpp"
#include "common/equality.hpp"

BOOST_AUTO_TEST_SUITE(TestSquareRoot)

BOOST_AUTO_TEST_CASE(test_sqroot_rough)
{
    BOOST_CHECK(-1 == SquareRoot::RoughSR(-1));
    BOOST_CHECK(-1 == SquareRoot::RoughSR(0));
    BOOST_CHECK(1 == SquareRoot::RoughSR(1));
    BOOST_CHECK(1 == SquareRoot::RoughSR(2));
    BOOST_CHECK(2 == SquareRoot::RoughSR(3));
    BOOST_CHECK(2 == SquareRoot::RoughSR(4));
    BOOST_CHECK(4 == SquareRoot::RoughSR(15));
    BOOST_CHECK(16 == SquareRoot::RoughSR(257));
    BOOST_CHECK(256 == SquareRoot::RoughSR(125348));
}

BOOST_AUTO_TEST_CASE(test_sqroot_babylonial)
{
    const double epsilon = 0.0001;
    BOOST_CHECK(equalDoubles(0.0, SquareRoot::BabylonialSR(-1, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(0.0, SquareRoot::BabylonialSR(0, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(1.0, SquareRoot::BabylonialSR(1, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(1.4142, SquareRoot::BabylonialSR(2, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(1.7320, SquareRoot::BabylonialSR(3, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(2.0, SquareRoot::BabylonialSR(4, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(3.8729, SquareRoot::BabylonialSR(15, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(16.0312, SquareRoot::BabylonialSR(257, epsilon), epsilon));
    BOOST_CHECK(equalDoubles(354.0451, SquareRoot::BabylonialSR(125348, epsilon), epsilon));
}

BOOST_AUTO_TEST_CASE(test_sqroot_with_rounding)
{
    SquareRoot::Solution solution;
    BOOST_CHECK(0 == solution.mySqrt(0));
    BOOST_CHECK(1 == solution.mySqrt(1));
    BOOST_CHECK(1 == solution.mySqrt(2));
    BOOST_CHECK(1 == solution.mySqrt(3));
    BOOST_CHECK(2 == solution.mySqrt(4));
    BOOST_CHECK(3 == solution.mySqrt(15));
    BOOST_CHECK(16 == solution.mySqrt(257));
    BOOST_CHECK(354 == solution.mySqrt(125348));
}

BOOST_AUTO_TEST_SUITE_END()


#include <catch.hpp>
#include <UnitConvert.hpp>

#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs.hpp>

TEST_CASE("UnitRegistry Example")
{

using namespace UnitConvert;
UnitRegistry ureg;

// add base units to the registry
ureg.addUnit("m = [L]");
ureg.addUnit("kg = [M]");
ureg.addUnit("s = [T]");
ureg.addUnit("K = [THETA]");
ureg.addUnit("mol = [N]");

// add some derived units to the registry
ureg.addUnit("100 cm = 1 m");
ureg.addUnit("1 in = 2.54 cm");
ureg.addUnit("1 ft = 12 in");
ureg.addUnit("1 J = 1 kg*m^2*s^-2");
ureg.addUnit("1 W = 1 J/s");
ureg.addUnit("1 cal = 4.184 J");

Quantity<double> q = ureg.makeQuantity<double>(200, "cm");

// q.value() returns the numerical value of the quantity
CHECK(q.value() == Approx(200));
CHECK(q.to("m").value() == Approx(2));
// millimeter was not defined, but we can use SI-prefixed versions of any unit in the registry
CHECK(q.to("mm").value() == Approx(2000));
// converting to a unit with different dimensions will throw an error
CHECK_THROWS(q.to(ureg.getUnit("s")));

// quantities  can be assigned to new dimensions
q = ureg.makeQuantity<double>(0.25, "s");
CHECK(q.value() == Approx(0.25));
CHECK(q.to("ms").value() == Approx(250));
// conversions have to match the quantities current dimensions.
CHECK_THROWS(q.to(ureg.getUnit("m")));

q = ureg.makeQuantity<double>(100, "cal");
CHECK(q.value() == Approx(100));
CHECK(q.to("J").value() == Approx(100 * 4.184));
// again, we can use SI prefixes
CHECK(q.to("mJ").value() == Approx(100 * 4.184*1000));
// we can convert to arbitrary combinations of defined dimensions
CHECK(q.to("kg*m^2*s^-2").value() == Approx(100 * 4.184));
CHECK(q.to("kg*m^2*ms^-2").value() == Approx(100 * 4.184 / 1000 / 1000));
CHECK(q.to("W*s").value() == Approx(100 * 4.184));
CHECK(q.to("kW*s").value() == Approx(100 * 4.184/1000));

CHECK_THROWS(q.to("m"));

// The unit registry can create Boost.Units quantities.
// So we can easily convert to a unit in one of the Boost.Unit
// systems and then create a Boost.Unit quantity.
q = ureg.makeQuantity<double>(100, "ft");
boost::units::quantity<boost::units::si::length> L = q.to<boost::units::si::length>();
CHECK(boost::units::quantity_cast<double>(L) == Approx(30.48));

}

TEST_CASE("BoostUnitRegisty Example")
{
  using namespace UnitConvert;
BoostUnitRegistry<boost::units::si::system> ureg;

Quantity<double> q;

// all SI base units and their prefixed versions
// are automatically defined.

q = ureg.makeQuantity<double>( 24, "cm" );
CHECK( q.to("m").value() == Approx(0.24) );

// mass
q = ureg.makeQuantity<double>( 24, "kg" );
CHECK( q.to("kg").value() == Approx(24) );
//CHECK( q.to("g").value() == Approx(24000) ); // 'g' does not exist yet
CHECK( q.to("mkg").value() == Approx(24000) );

// time
q = ureg.makeQuantity<double>( 24, "ms" );
CHECK( q.to("ms").value() == Approx(24) );
CHECK( q.to("s").value() == Approx(0.024) );
CHECK( q.to_base_units().value() == Approx(0.024) );

// electrical current
q = ureg.makeQuantity<double>( 24, "A" );
CHECK( q.to("mA").value() == Approx(24000) );

// temperature
q = ureg.makeQuantity<double>( 24, "K" );
CHECK( q.to("mK").value() == Approx(24000) );

// amount
q = ureg.makeQuantity<double>( 24, "mol" );
CHECK( q.to("mmol").value() == Approx(24000) );

// luminous intensity
q = ureg.makeQuantity<double>( 24, "cd" );
CHECK( q.to("mcd").value() == Approx(24000) );

// now add some non-base units
// we can add boost unit directly.
// however, the unit will be added using whatever it string-ify's to
ureg.addUnit( boost::units::cgs::mass() );
// or we can define the units with a string
ureg.addUnit( "N = kg m / s^2" );
ureg.addUnit( "J = N m" );
ureg.addUnit( "W = J s" );

q = ureg.makeQuantity<double>( 24, "kg" );
CHECK( q.to("kg").value() == Approx(24) );
CHECK( q.to("g").value() == Approx(24000) ); // 'g' exists now

q = ureg.makeQuantity<double>( 24, "kJ" );
CHECK( q.to("mJ").value() == Approx(24000000) );
CHECK( q.to_base_units().value() == Approx(24000) );

q = ureg.makeQuantity<double>( 24, "mW" );
CHECK( q.to("kW").value() == Approx(0.000024) );

}


#include "CRTP/Categories/Sets/PosetElement.h"

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

using CRTP::Categories::Sets::Posets::BoostPartiallyOrderedElement;
using CRTP::Categories::Sets::Posets::PartiallyOrderedSetElement;

BOOST_AUTO_TEST_SUITE(CRTP)
BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Sets)

BOOST_AUTO_TEST_SUITE(PosetElement_tests)

BOOST_AUTO_TEST_SUITE(BoostPartiallyOrderedElement_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(Reflexivity)
{
  {
    //BoostPartiallyOrderedElement<unsigned int> a {97};
    //BoostPartiallyOrderedElement<unsigned int> b {97};

    //BOOST_TEST((a <= b));
  }
  BOOST_TEST(true);
}

BOOST_AUTO_TEST_SUITE_END() // BoostPartiallyOrderedElement_tests

BOOST_AUTO_TEST_SUITE(PartiallyOrderedSetElement_tests)

template <typename T, typename = std::enable_if_t<std::is_integral<T>::value>>
class PartiallyOrderedNaturalNumber :
  PartiallyOrderedSetElement<PartiallyOrderedNaturalNumber<T>>
{
  public:

    PartiallyOrderedNaturalNumber():
      data_{static_cast<T>(0)}
    {}

    PartiallyOrderedNaturalNumber(const T n):
      data_{n}
    {}

    bool operator<=(const PartiallyOrderedNaturalNumber& rhs) const
    {
      return this->data_ <= rhs.data_;
    }

    /*
    friend bool operator<=(
      const PartiallyOrderedNaturalNumber& lhs,
      const PartiallyOrderedNaturalNumber& rhs)
    {
      return lhs.data_ <= rhs.data_
    }
    */

  private:

    T data_;
};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(Reflexivity)
{
  PartiallyOrderedNaturalNumber<unsigned int> a {97};
  PartiallyOrderedNaturalNumber<unsigned int> b {97};

  BOOST_TEST((a <= b));
  BOOST_TEST((a <= a));
  BOOST_TEST((b <= b));
}

BOOST_AUTO_TEST_SUITE_END() // PartiallyOrderedSetElement_tests

BOOST_AUTO_TEST_SUITE_END() // PosetElement_tests
BOOST_AUTO_TEST_SUITE_END() // Sets
BOOST_AUTO_TEST_SUITE_END() // Categories
BOOST_AUTO_TEST_SUITE_END() // CRTP


#include "AsteroidMap.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/integer/common_factor.hpp>
#include <boost/math/constants/constants.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace
{
const int SOME_NUMBER_LARGER_THAN_GRID_SIZE = 10000;
}

namespace AdventOfCode
{
namespace Year2019
{
namespace Day10
{

AsteroidMap::AsteroidMap(CoordinatesSet asteroidCoordinatesSet)
    : m_asteroidCoordinatesSet{std::move(asteroidCoordinatesSet)}
    , m_laserVector{-1, -SOME_NUMBER_LARGER_THAN_GRID_SIZE}
{
    m_locatorPosition = determineLocatorPosition();
    m_asteroidCoordinatesSet.erase(m_locatorPosition);
}

unsigned AsteroidMap::getMaxNumAsteroidsDetected() const
{
    return getNumAsteroidsDetected(m_locatorPosition);
}

std::vector<Coordinates> AsteroidMap::vaporize()
{
    while (!m_asteroidCoordinatesSet.empty())
    {
        vaporizeNextAsteroid();
    }

    return m_vaporizationOrder;
}

Coordinates AsteroidMap::determineLocatorPosition() const
{
    return *std::max_element(m_asteroidCoordinatesSet.cbegin(), m_asteroidCoordinatesSet.cend(), [this](const auto& lhs, const auto& rhs)
                             {
                                 return this->getNumAsteroidsDetected(lhs) < this->getNumAsteroidsDetected(rhs);
                             });
}

unsigned AsteroidMap::getNumAsteroidsDetected(const Coordinates& stationPos) const
{
    unsigned numAsteroidsDetected = 0;

    for (const auto& asteroidCoordinates : m_asteroidCoordinatesSet)
    {
        if (asteroidCoordinates == stationPos)
        {
            continue;
        }

        if (isThereDirectLineOfSight(stationPos, asteroidCoordinates))
        {
            ++numAsteroidsDetected;
        }
    }

    return numAsteroidsDetected;
}

bool AsteroidMap::isThereDirectLineOfSight(const Coordinates& fromCoordinates, const Coordinates& toCoordinates) const
{
    Vector2D fromVector{fromCoordinates.first, fromCoordinates.second};
    Vector2D toVector{toCoordinates.first, toCoordinates.second};

    Vector2D differenceVector = toVector - fromVector;
    Vector2D differenceVectorAbs = differenceVector.cwiseAbs();

    int gcd = boost::integer::gcd(differenceVectorAbs[0], differenceVectorAbs[1]);

    Vector2D differenceBetweenTwoMidpointsVector = differenceVector / gcd;

    for (Vector2D midpointVector = fromVector + differenceBetweenTwoMidpointsVector; midpointVector != toVector; midpointVector += differenceBetweenTwoMidpointsVector)
    {
        Coordinates midpointCoordinates{midpointVector[0], midpointVector[1]};
        if (m_asteroidCoordinatesSet.find(midpointCoordinates) != m_asteroidCoordinatesSet.cend())
        {
            return false;
        }
    }

    return true;
}

void AsteroidMap::vaporizeNextAsteroid()
{
    Coordinates nextAsteroidToVaporize = findNextAsteroidToVaporize();
    Vector2D nextAsteroidToVaporizeVector{nextAsteroidToVaporize.first, nextAsteroidToVaporize.second};

    Vector2D locatorVector{m_locatorPosition.first, m_locatorPosition.second};

    m_laserVector = nextAsteroidToVaporizeVector - locatorVector;

    m_vaporizationOrder.push_back(nextAsteroidToVaporize);
    m_asteroidCoordinatesSet.erase(nextAsteroidToVaporize);
}

Coordinates AsteroidMap::findNextAsteroidToVaporize() const
{
    Coordinates closestAsteroid;
    double minAngle = std::numeric_limits<double>::max();

    for (const auto& asteroidCoordinates : m_asteroidCoordinatesSet)
    {
        double angle = getLaserAngleToAsteroid(asteroidCoordinates);

        // The laser has to spin to some non-zero degree, so zero degrees
        // actually means that  a full revolution has to be made
        if (angle == 0)
        {
            angle = 2 * boost::math::constants::pi<double>();
        }

        if (angle <= minAngle && isThereDirectLineOfSight(m_locatorPosition, asteroidCoordinates))
        {
            minAngle = angle;
            closestAsteroid = asteroidCoordinates;
        }
    }

    return closestAsteroid;
}

double AsteroidMap::getLaserAngleToAsteroid(const Coordinates& asteroidCoordinates) const
{
    Vector2D fromVector{m_laserVector};

    Vector2D asteroidVector{asteroidCoordinates.first, asteroidCoordinates.second};
    Vector2D locatorVector{m_locatorPosition.first, m_locatorPosition.second};

    Vector2D toVector{asteroidVector - locatorVector};

    double angle = std::atan2(toVector[1], toVector[0]) - std::atan2(fromVector[1], fromVector[0]);

    double pi = boost::math::constants::pi<double>();
    if (angle < 0)
    {
        angle += 2 * pi;
    }

    // We are looking for negative angles, so we need to multiply the angle by -1.
    // And we are in a mirrored coordinate sytem, so we need to multiply it by -1 again.
    // These two operations simplified just mean that we can return the angle as is.
    return angle;
}

}
}
}


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   CartesianToGeodeticFukushima.hpp
 * Author: jordan
 */

#ifndef CARTESIANTOGEODETICFUKUSHIMA_HPP
#define CARTESIANTOGEODETICFUKUSHIMA_HPP

#ifdef _WIN32
#define _USE_MATH_DEFINES
#include <math.h>
#else
#include <cmath>
#endif

#include <vector>
#include <Eigen/Dense>
#include "../Position.hpp"
#include "../utils/Constants.hpp"

class CartesianToGeodeticFukushima {
    /*
     * This class implements the method given in Fukushima (2006):
     *
     * Transformation from Cartesian to geodetic coordinates
     * accelerated by Halley's method
     * DOI: 10.1007/s00190-006-0023-2
     */
private:

    unsigned int numberOfIterations;

    // Ellipsoid parameters
    double a; // semi-major axis
    double e2; // first eccentricity squared

    // Derived parameters
    double b; // semi-minor axis
    double a_inverse; // 1/a
    double ec; // sqrt(1 - e*e)

public:

    CartesianToGeodeticFukushima(unsigned int numberOfIterations, double a=a_wgs84, double e2=e2_wgs84) :
    numberOfIterations(numberOfIterations), a(a), e2(e2) {
        ec = std::sqrt(1 - e2);
        b = a*ec;
        a_inverse = 1 / a;
    }

    ~CartesianToGeodeticFukushima() {};

    void ecefToLongitudeLatitudeElevation(Eigen::Vector3d & ecefPosition, Position & positionGeographic) {
        double x = ecefPosition(0);
        double y = ecefPosition(1);
        double z = ecefPosition(2);

        // Center of the Earth
        if (x == 0.0 && y == 0.0 && z == 0.0) {
            positionGeographic.setLatitude(0.0);
            positionGeographic.setLongitude(0.0);
            positionGeographic.setEllipsoidalHeight(0.0);
            return;
        }

        // Position at Poles
        if (x == 0.0 && y == 0.0 && z != 0.0) {
            if (z > 0) {
                positionGeographic.setLatitude(M_PI_2*R2D);
            } else {
                positionGeographic.setLatitude(-M_PI_2*R2D);
            }

            positionGeographic.setLongitude(0.0);
            positionGeographic.setEllipsoidalHeight(std::abs(z) - b);
            return;
        }

        double pp = x * x + y * y;
        double p = std::sqrt(pp);

        // Position at Equator
        if (z == 0.0) {
            positionGeographic.setLatitude(0.0);
            positionGeographic.setLongitude(estimateLongitude(x, y, p)*R2D);
            positionGeographic.setEllipsoidalHeight(std::sqrt(x * x + y * y) - a);
            return;
        }

        double P = p*a_inverse;
        double Z = a_inverse * ec * std::abs(z);

        //double R = std::sqrt(pp + z * z);
        std::vector<double> S(numberOfIterations + 1, 0);
        std::vector<double> C(numberOfIterations + 1, 0);

        std::vector<double> D(numberOfIterations + 1, 0);
        std::vector<double> F(numberOfIterations + 1, 0);

        std::vector<double> A(numberOfIterations + 1, 0);
        std::vector<double> B(numberOfIterations + 1, 0);

        S[0] = Z; //starter variables. See (Fukushima, 2006) p.691 equation (17)
        C[0] = ec*P; //starter variables. See (Fukushima, 2006) p.691 equation (17)
        A[0] = std::sqrt(S[0] * S[0] + C[0] * C[0]);
        B[0] = 1.5 * e2 * e2 * P * S[0] * S[0] * C[0] * C[0] * (A[0] - ec); //starter variables. See (Fukushima, 2006) p.691 equation  (18)

        unsigned int iterationNumber = 1;

        while (iterationNumber <= numberOfIterations) {

            D[iterationNumber - 1] =
                    Z * A[iterationNumber - 1] * A[iterationNumber - 1] * A[iterationNumber - 1] +
                    e2 * S[iterationNumber - 1] * S[iterationNumber - 1] * S[iterationNumber - 1];
            F[iterationNumber - 1] =
                    P * A[iterationNumber - 1] * A[iterationNumber - 1] * A[iterationNumber - 1] -
                    e2 * C[iterationNumber - 1] * C[iterationNumber - 1] * C[iterationNumber - 1];

            S[iterationNumber] =
                    D[iterationNumber - 1] * F[iterationNumber - 1] -
                    B[iterationNumber - 1] * S[iterationNumber - 1];
            C[iterationNumber] =
                    F[iterationNumber - 1] * F[iterationNumber - 1] -
                    B[iterationNumber - 1] * C[iterationNumber - 1];

            A[iterationNumber] = std::sqrt(
                    S[iterationNumber] * S[iterationNumber] +
                    C[iterationNumber] * C[iterationNumber]);

            B[iterationNumber] =
                    1.5 *
                    e2 * S[iterationNumber] *
                    C[iterationNumber] * C[iterationNumber] *
                    ((P * S[iterationNumber] - Z * C[iterationNumber]) * A[iterationNumber] -
                    e2 * S[iterationNumber] * C[iterationNumber]);

            ++iterationNumber;
        }

        double lon = estimateLongitude(x, y, p);

        double Cc = ec*C[numberOfIterations];
        double lat = estimateLatitude(z, S[numberOfIterations], Cc);
        double h = estimateHeight(z, p, A[numberOfIterations], S[numberOfIterations], Cc);

        positionGeographic.setLatitude(lat*R2D);
        positionGeographic.setLongitude(lon*R2D);
        positionGeographic.setEllipsoidalHeight(h);
    }

    double estimateLongitude(double x, double y, double p) {
        // Vermeille (2004), stable longitude calculation
        // atan(y/x) suffers when x = 0

        if (y < 0) {
            return -M_PI_2 + 2*std::atan(x/(p - y));
        }

        return M_PI_2 - 2*std::atan(x/(p + y));
    }

    double estimateLatitude(double z, double S, double Cc) {
        double lat = std::abs(std::atan(S / Cc));

        if (z < 0) {
            return -lat;
        }

        return lat;
    }

    double estimateHeight(double z, double p, double A, double S, double Cc) {
        return (p * Cc + std::abs(z) * S - b * A) / std::sqrt(Cc * Cc + S * S);
    }
};

#endif /* CARTESIANTOGEODETICFUKUSHIMA_HPP */


/*
    $ sudo apt-get install libboost-math*

    real   4m15.388s
    user    4m10.272s
    sys 0m0.588s
*/

#include <sstream>
#include <string>
#include <fstream>
#include <iostream>
#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;

struct Result {
    boost::numeric::ublas::matrix<int> A;
    boost::numeric::ublas::matrix<int> B;
};

int getMatrixSize(string filename) {
    string line;
    ifstream infile;
    infile.open (filename.c_str());
    getline(infile, line);
    return count(line.begin(), line.end(), '\t') + 1;
}

void printMatrix(boost::numeric::ublas::matrix<int> matrix) {
    for (unsigned int i=0; i < matrix.size1(); i++) {
        for (unsigned int j=0; j < matrix.size2(); j++) {
            cout << matrix(i, j);
            if(j+1 != matrix.size2()) {
                cout << "\t";
            }
        }
        cout << endl;
    }
}

Result read(string filename) {
    Result ab;
    string line;
    ifstream infile;
    infile.open (filename.c_str());

    // get dimension
    getline(infile, line);
    int n = getMatrixSize(filename);

    boost::numeric::ublas::matrix<int> A(n,n), B(n,n);

    // process first line
    istringstream iss(line);
    int a, i = 0, j = 0;
    while (iss >> a) {
        A(i,j) = a;
        j++;
    }
    i++;

    while (getline(infile, line) && !line.empty()) {
        istringstream iss(line);
        j = 0;
        while (iss >> a) {
            A(i,j) = a;
            j++;
        }
        i++;
    }

    i = 0;
    while (getline(infile, line)) {
        istringstream iss(line);
        j = 0;
        while (iss >> a) {
            B(i,j) = a;
            j++;
        }
        i++;
    }

    infile.close();
    ab.A = A;
    ab.B = B;
    return ab;
}

int main (int argc, char* argv[]) {
    string filename;
    if (argc < 3) {
        filename = "2000.in";
    } else {
        filename = argv[2];
    }
    Result result = read (filename);

    boost::numeric::ublas::matrix<int> C;
    C = boost::numeric::ublas::prod(result.A, result.B);
    printMatrix(C);

    return 0;
}


#ifndef ABC
#define ABC
#include <vector>
#include <random>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>

namespace ublas = boost::numeric::ublas;

namespace ABC
{
   template<typename T>
   using Func=std::function<T(ublas::vector<T>)>;
   std::random_device seed_gen;
   std::mt19937 engine(seed_gen());
   std::uniform_real_distribution<double> rand_double(-1, 1);
   template<typename T>
   std::uniform_real_distribution<T> rand_init(-1, 1);

   template<typename T>
   void init(ublas::vector<T> &x)
   {
      for (int i = 0; i < x.size(); i++)
      {
         x[i] = rand_init<T>(engine);
      }
   }
   template<typename T>
   bool update(int const i, std::vector<ublas::vector<T>> &x, std::vector<int> &cnt, ublas::vector<double> &v, Func<T> f)
   {
      int dimension = x[i].size();
      int population = x.size();
      std::uniform_int_distribution<int> rand_dim(0, dimension-1);
      std::uniform_int_distribution<int> rand_pop(0, population-1);
      int j = rand_dim(engine);
      int k = rand_pop(engine);
      double phi = rand_double(engine);
      ublas::vector<T> x_i = ublas::vector<T>(x[i]);
      x_i[j] -= phi*(x_i[j] - x[k][j]);
      double v_new = f(x_i);
      cnt[i]++;
      if(v_new <= v[i])
      {
         x[i] = x_i;
         v[i] = v_new;
         return true;
      }
      return false;
   }
   template<typename T>
   void update_random(int const max_visit, std::vector<ublas::vector<T>> &x, std::vector<int> &cnt, ublas::vector<double> &v, Func<T> f)
   {
      int dimension = x[0].size();
      int population = x.size();
      for(int i = 0; i < population; i++)
      {
         if(cnt[i] < max_visit) continue;
         ublas::vector<T> x_i(dimension);
         init(x_i);
         double v_new = f(x_i);
         cnt[i] = 1;
         if(v_new <= v[i])
         {
            x[i] = x_i;
            v[i] = v_new;
         }
      }
   }
   template<typename T>
   double minimize(int const dimension, int const num_population, int const max_visit, int const max_step, Func<T> f)
   {
      std::vector<ublas::vector<T>> x(num_population);
      std::vector<int> intervals(num_population);
      std::vector<int> cnt(num_population);
      ublas::vector<double> one(num_population);
      ublas::vector<double> v(num_population);
      ublas::vector<double> p(num_population);
      std::uniform_int_distribution<int> rand_dim(0, dimension-1);
      double best_obj = 1 << 20;
      ublas::vector<T> best_x(dimension);

      for (int i = 0; i < num_population; i++)
      {
         x[i].resize(dimension);
         init(x[i]);
         v[i] = f(x[i]);
         intervals[i] = i;
         one[i] = 1.0;
      }
      for(int step = 0; step < max_step; step++)
      {
         int i = rand_dim(engine);
         update<T>(i, x, cnt, v, f);
         auto m = std::min_element(v.begin(), v.end());
         if(*m < 0)
         {
            ublas::vector<double> w = v - ublas::vector<double>(num_population, *m);
            p = w / ublas::sum(w);
         }
         else
         {
            p = v / ublas::sum(v);
         }
         p = one - p;
         p /= ublas::sum(p);
         std::piecewise_constant_distribution<> dist(intervals.begin(), intervals.end(), p.begin());
         i = static_cast<int>(dist(engine));
         update<T>(i, x, cnt, v, f);
         update_random<T>(max_visit, x, cnt, v, f);

         auto iter =  std::min_element(v.begin(), v.end());
         int idx = std::distance(v.begin(), iter);
         if(best_obj >= *iter)
         {
            best_obj = *iter;
            best_x = x[idx];
         }
      }
      return best_obj;
   }

};
#endif // ABC


#include "catch.hpp"

#include <boost/units/systems/si.hpp>

#include <UnitConvert.hpp>
using namespace UnitConvert;

/**
 * This file is used for development. As new classes are created,
 * small tests are written here so that we can try to compile and use
 * them.
 */

TEST_CASE("Dimension Devel", "[devel]")
{
  BaseDimension<Dimension::Name::Length>            L;
  BaseDimension<Dimension::Name::Mass>              M;
  BaseDimension<Dimension::Name::Time>              T;
  BaseDimension<Dimension::Name::ElectricalCurrent> I;
  BaseDimension<Dimension::Name::Temperature>       THETA;
  BaseDimension<Dimension::Name::Amount>            N;
  BaseDimension<Dimension::Name::LuminousIntensity> J;

  Dimension V = L / T;
  Dimension E = M * V * V;
  Dimension P = E / T;

  BaseDimension<Dimension::Name::Length> Length;

  CHECK(P[Dimension::Name::Length] == 2);
  CHECK(P[Dimension::Name::Mass] == 1);
  CHECK(P[Dimension::Name::Time] == -3);
  CHECK(P[Dimension::Name::ElectricalCurrent] == 0);
  CHECK(P[Dimension::Name::Temperature] == 0);
  CHECK(P[Dimension::Name::Amount] == 0);
  CHECK(P[Dimension::Name::LuminousIntensity] == 0);

  CHECK(L == Length);
  CHECK(M != Length);
  CHECK(M != L);
  CHECK(E != P);
  CHECK(E == P * T);
}

TEST_CASE("Unit Devel", "[devel]")
{
  BaseUnit<Dimension::Name::Length>      meter;
  BaseUnit<Dimension::Name::Mass>        kilogram;
  BaseUnit<Dimension::Name::Time>        second;
  BaseUnit<Dimension::Name::Temperature> kelvin;

  Unit gram = kilogram / 1000;

  Unit centimeter = meter / 100;
  Unit inch       = 2.54 * centimeter;
  Unit foot       = 12 * inch;

  Unit joule = kilogram * meter * meter / second / second;

  Unit celsius    = kelvin - 273.15;
  Unit fahrenheit = (5. / 9) * celsius + 32;

  Unit unit = celsius;

  CHECK(meter.scale() == 1);
  CHECK(meter.offset() == 0);
  CHECK(kilogram.scale() == 1);
  CHECK(kilogram.offset() == 0);
  CHECK(second.scale() == 1);
  CHECK(second.offset() == 0);
  CHECK(kelvin.scale() == 1);
  CHECK(kelvin.offset() == 0);

  CHECK(gram.scale() == Approx(0.001));
  CHECK(gram.offset() == 0);

  CHECK(centimeter.scale() == Approx(0.01));
  CHECK(centimeter.offset() == 0);
  CHECK(inch.scale() == Approx(0.0254));
  CHECK(inch.offset() == 0);
  CHECK(foot.scale() == Approx(12 * 0.0254));
  CHECK(foot.offset() == 0);

  CHECK(joule.scale() == Approx(1));
  CHECK(joule.offset() == Approx(0));

  CHECK(celsius.scale() == Approx(1));
  CHECK(celsius.offset() == Approx(-273.15));

  CHECK(fahrenheit.scale() == Approx(5. / 9));
  CHECK(fahrenheit.offset() == Approx(32 - (9. / 5) * 273.15));

  CHECK(unit.offset() == Approx(-273.15));
  unit += 100;
  CHECK(unit.offset() == Approx(-173.15));
  unit += 100;
  CHECK(unit.offset() == Approx(-73.15));
  unit -= 50;
  CHECK(unit.offset() == Approx(-123.15));

  SECTION("Quantity Devel", "[devel]")
  {
    Quantity<double> distance(100, foot);

    CHECK(distance.to(meter).value() == Approx(30.48));
    CHECK_THROWS(distance.to(second));

    Quantity<double> temperature(37, celsius);

    CHECK(temperature.to(kelvin).value() == Approx(273.15 + 37));
    CHECK(temperature.to(fahrenheit).value() == Approx(98.6));
  }
}

TEST_CASE("UnitRegistry Devel", "[devel]")
{
  UnitRegistry ureg;

  ureg.addBaseUnit<Dimension::Name::Length>("m");
  ureg.addBaseUnit<Dimension::Name::Mass>("kg");
  ureg.addBaseUnit<Dimension::Name::Time>("s");
  ureg.addBaseUnit<Dimension::Name::Temperature>("K");
  ureg.addBaseUnit<Dimension::Name::Amount>("mol");

  ureg.addUnit("100 cm = 1 m");
  ureg.addUnit("1 in = 2.54 cm");
  ureg.addUnit("1 ft = 12 in");
  ureg.addUnit("1 J = 1 kg*m^2*s^-2");
  ureg.addUnit("1 W = 1 J/s");
  ureg.addUnit("1 cal = 4.184 J");

  Unit u = BaseUnit<Dimension::Name::Dimensionless>();
  Quantity<double> q = ureg.makeQuantity<double>(200, "cm");

  u = ureg.getUnit("m");
  CHECK(u.scale() == Approx(1));
  u = ureg.getUnit("cm");
  CHECK(u.scale() == Approx(0.01));
  CHECK_THROWS(ureg.getUnit("mm"));
  u = ureg.getUnit("mm",true);
  CHECK(u.scale() == Approx(0.001));
  u = ureg.getUnit("millim",true);
  CHECK(u.scale() == Approx(0.001));

  CHECK(q.value() == Approx(200));
  // getUnit returns a unit that is in the registry
  CHECK(q.to(ureg.getUnit("m")).value() == Approx(2));
  CHECK_THROWS(q.to(ureg.getUnit("s")));

  q = ureg.makeQuantity<double>(0.25, "s");
  CHECK(q.value() == Approx(0.25));
  // makeUnit can create a unit from the registry,
  // including SI prefixed versions of the units in the registry
  CHECK(q.to(ureg.makeUnit("ms")).value() == Approx(250));
  CHECK_THROWS(q.to(ureg.getUnit("m")));

  q = ureg.makeQuantity<double>(100, "cal");
  CHECK(q.value() == Approx(100));
  // the Quantity class can convert to a unit string
  // if it was created by a UnitRegistry
  CHECK(q.to("J").value() == Approx(100 * 4.184));

  // string based conversion can convert to SI prefixed version of any unit
  // in the registry, even if they are not explicitly put in the registry,
  CHECK(q.to("mJ").value() == Approx(100 * 4.184 * 1000));

  // string based conversion can also convert to derived unit representations
  // that contain SI prefixed versions of units in the registry
  CHECK(q.to("kg*km^2/ms^2").value() == Approx(100 * 4.184 / 1e12));
  CHECK(q.to("kg*m*km/ms/s").value() == Approx(100 * 4.184 / 1e6));
  CHECK(q.to("mW*s").value() == Approx(100 * 4.184 * 1000));
  CHECK(q.to("mW*cs").value() == Approx(100 * 4.184 * 1000 * 100));

  CHECK_THROWS(q.to("m"));

  SECTION("Converting to Boost.Units quantity")
  {
    Quantity<double> distance = ureg.makeQuantity<double>(100, "ft");
    boost::units::quantity<boost::units::si::length> L =
        distance.to<boost::units::si::length>();
    CHECK(boost::units::quantity_cast<double>(L) == Approx(30.48));
  }

}

TEST_CASE("BoostUnitRegistry Devel", "[devel]")
{
  BoostUnitRegistry<boost::units::si::system> ureg;

  // base units are automatically loaded into the
  // registry.

  ureg.addUnit("100 cm = 1 m");
  ureg.addUnit("1 in = 2.54 cm");
  ureg.addUnit("1 ft = 12 in");
  ureg.addUnit("1 J = 1 kg*m^2*s^-2");
  ureg.addUnit("1 W = 1 J/s");
  ureg.addUnit("1 cal = 4.184 J");

  Quantity<double> q = ureg.makeQuantity<double>(200, "cm");

  CHECK(q.value() == Approx(200));
  // getUnit returns a unit that is in the registry
  CHECK(q.to(ureg.getUnit("m")).value() == Approx(2));
  CHECK_THROWS(q.to(ureg.getUnit("s")));

  q = ureg.makeQuantity<double>(0.25, "s");
  CHECK(q.value() == Approx(0.25));
  // makeUnit can create a unit from the registry,
  // including SI prefixed versions of the units in the registry
  CHECK(q.to(ureg.makeUnit("ms")).value() == Approx(250));
  CHECK_THROWS(q.to(ureg.getUnit("m")));

  q = ureg.makeQuantity<double>(100, "cal");
  CHECK(q.value() == Approx(100));
  // the Quantity class can convert to a unit string
  // if it was created by a UnitRegistry
  CHECK(q.to("J").value() == Approx(100 * 4.184));

  // string based conversion can convert to SI prefixed version of any unit
  // in the registry, even if they are not explicitly put in the registry,
  CHECK(q.to("mJ").value() == Approx(100 * 4.184 * 1000));

  // string based conversion can also convert to derived unit representations
  // that contain SI prefixed versions of units in the registry
  CHECK(q.to("kg*km^2/ms^2").value() == Approx(100 * 4.184 / 1e12));
  CHECK(q.to("kg*m*km/ms/s").value() == Approx(100 * 4.184 / 1e6));
  CHECK(q.to("mW*s").value() == Approx(100 * 4.184 * 1000));
  CHECK(q.to("mW*cs").value() == Approx(100 * 4.184 * 1000 * 100));

  CHECK_THROWS(q.to("m"));

  SECTION("Converting to Boost.Units quantity")
  {
    Quantity<double> distance = ureg.makeQuantity<double>(100, "ft");
    boost::units::quantity<boost::units::si::length> L =
        distance.to<boost::units::si::length>();
    CHECK(boost::units::quantity_cast<double>(L) == Approx(30.48));
  }
}


/**
 * @file random_test.cpp
 * @author Konstantin Sidorov
 *
 * Tests for generators of random numbers from math:: namespace.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/math/random.hpp>
#include <mlpack/core/dists/discrete_distribution.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace math;

BOOST_AUTO_TEST_SUITE(RandomTest);

// Test for RandInt() sampler from discrete uniform distribution.
BOOST_AUTO_TEST_CASE(DiscreteUniformRandomTest)
{
  std::vector<std::pair<int, int>> ranges =
  {
    std::make_pair(0, 1),
    std::make_pair(0, 2),
    std::make_pair(-6, -2),
    std::make_pair(-3, 4),
    std::make_pair(13, 37)
  };
  const size_t iterations = 10000;
  for (std::pair<int, int> range : ranges)
  {
    int lo = range.first, hiExclusive = range.second;
    std::vector<int> count(hiExclusive - lo, 0);
    for (size_t iter = 0; iter < iterations; ++iter)
    {
      count[RandInt(lo, hiExclusive) - lo]++;
    }
    for (size_t i = 0; i < count.size(); ++i)
    {
      BOOST_REQUIRE_SMALL(
        1.0 / (hiExclusive - lo) - count[i] * 1.0 / iterations, 0.15);
    }
  }

  // Here we also test RandInt(hiExclusive) overload.
  for (std::pair<int, int> range : ranges)
  {
    int lo = range.first, hiExclusive = range.second;
    if (lo != 0) continue;
    std::vector<int> count(hiExclusive - lo, 0);
    for (size_t iter = 0; iter < iterations; ++iter)
    {
      count[RandInt(lo, hiExclusive) - lo]++;
    }

    for (size_t i = 0; i < count.size(); ++i)
    {
      BOOST_REQUIRE_SMALL(
          1.0 / (hiExclusive - lo) - count[i] * 1.0 / iterations, 0.15);
    }
  }
}

// Test for RandInt() sampler from discrete (possibly nonuniform) distribution.
BOOST_AUTO_TEST_CASE(WeightedRandomTest)
{
  std::vector<std::vector<double>> weights = {
    {1},
    {0, 0, 1, 0, 0},
    {1, 0, 0},
    {0, 0, 1},
    {0.25, 0.25, 0.5},
    {0.9, 0.05, 0.05},
    {0.5, 0.1, 0.1, 0.1, 0.1, 0.1}
  };
  const size_t iterations = 50000;
  for (std::vector<double> weightSet : weights)
  {
    mlpack::distribution::DiscreteDistribution d(1);
    d.Probabilities(0) =  arma::vec(weightSet);
    std::vector<int> count(weightSet.size(), 0);
    for (size_t iter = 0; iter < iterations; ++iter)
    {
      count[d.Random()(0)]++;
    }

    for (size_t i = 0; i < weightSet.size(); ++i)
    {
      BOOST_REQUIRE_SMALL(weightSet[i] - count[i] * 1.0 / iterations, 0.15);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


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

/**
 * @file kf_test.cpp
 * @brief Unit tests using Google Test framework to validate class
 *  implementation.
 * @author Sean Crutchlow <sean.GH1231@gmail.com>
 * @version 1.0
 */

/// System
#include <gtest/gtest.h>

#include <cmath>
#include <fstream>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <vector>

/// Library
#include <Eigen/Dense>

/// Project
#include "kalman_filter/kf.h"
#include "kalman_filter/ekf.h"
#include "kalman_filter/ukf.h"
#include "kalman_filter/utils.h"

namespace kalman_filter {
class KfTest : public ::testing::Test {
 protected:
  /// Pointer for Kalman Filter object
  std::shared_ptr<KF> kf_ptr;
  /// Pointers for Eigen Matrices
  Eigen::MatrixXd *kf_H_lidar_ptr;
  Eigen::MatrixXd *kf_H_radar_ptr;
  Eigen::MatrixXd *kf_R_lidar_ptr;
  Eigen::MatrixXd *kf_R_radar_ptr;
  Eigen::MatrixXd *kf_Q_ptr;

  /// Pointer for Extended Kalman Filter object
  std::shared_ptr<EKF> ekf_ptr;
  /// Pointers for Eigen Matrices
  Eigen::MatrixXd *ekf_H_lidar_ptr;
  Eigen::MatrixXd *ekf_H_radar_ptr;
  Eigen::MatrixXd *ekf_R_lidar_ptr;
  Eigen::MatrixXd *ekf_R_radar_ptr;
  Eigen::MatrixXd *ekf_Q_ptr;

  /// File names for input data
  std::string f_lidar_radar;

  /// Generic structure for sensor measurements
  struct Measurement {
    std::string type;
    std::vector<double> data;
    double gt_x, gt_y, gt_vx, gt_vy;
    int64_t timestamp;
  };

  virtual void SetUp() {
    /// Files containing measurement data
    f_lidar_radar = "lidar_and_radar.csv";

    /// state vector - x
    Eigen::VectorXd kf_x(4);
    // covariance matrix - P
    Eigen::MatrixXd kf_P(4, 4);
    /// measurement matrix - H
    kf_H_lidar_ptr = new Eigen::MatrixXd(2, 4);
    kf_H_radar_ptr = new Eigen::MatrixXd(2, 4);
    /// measurement covariance matrix - R
    kf_R_lidar_ptr = new Eigen::MatrixXd(2, 2);
    kf_R_radar_ptr = new Eigen::MatrixXd(2, 2);
    /// state noise covariance matrix - Q
    kf_Q_ptr = new Eigen::MatrixXd(4, 4);
    /// control vector - u
    Eigen::VectorXd kf_u(4);

    /// Initialize Kalman Filter Variables
    InitKfVars(&kf_x,
               &kf_P,
               kf_H_lidar_ptr,
               kf_H_radar_ptr,
               kf_R_lidar_ptr,
               kf_R_radar_ptr,
               kf_Q_ptr,
               &kf_u);

    /// Create Kalman Filter Object
    kf_ptr = std::make_shared<KF>(kf_x, kf_P, kf_u);

    /// state vector - x
    Eigen::VectorXd ekf_x(6);
    // covariance matrix - P
    Eigen::MatrixXd ekf_P(6, 6);
    /// measurement matrix - H
    ekf_H_lidar_ptr = new Eigen::MatrixXd(2, 6);
    ekf_H_radar_ptr = new Eigen::MatrixXd(2, 6);
    /// measurement covariance matrix - R
    ekf_R_lidar_ptr = new Eigen::MatrixXd(2, 2);
    ekf_R_radar_ptr = new Eigen::MatrixXd(2, 2);
    /// state noise covariance matrix - Q
    ekf_Q_ptr = new Eigen::MatrixXd(6, 6);
    /// control vector - u
    Eigen::VectorXd ekf_u(6);

    /// Initialize Kalman Filter Variables
    InitEkfVars(&ekf_x,
               &ekf_P,
               ekf_H_lidar_ptr,
               ekf_H_radar_ptr,
               ekf_R_lidar_ptr,
               ekf_R_radar_ptr,
               ekf_Q_ptr,
               &ekf_u);

    /// Create Extended Kalman Filter Object
    ekf_ptr = std::make_shared<EKF>(ekf_x, ekf_P, ekf_u);
  }

  virtual void TearDown() {
    delete kf_H_lidar_ptr;
    delete kf_H_radar_ptr;
    delete kf_R_lidar_ptr;
    delete kf_R_radar_ptr;
    delete kf_Q_ptr;

    delete ekf_H_lidar_ptr;
    delete ekf_H_radar_ptr;
    delete ekf_R_lidar_ptr;
    delete ekf_R_radar_ptr;
    delete ekf_Q_ptr;
  }

  /**
   * @brief      Initializes the Kalman Filter variables.
   *
   * @param      _x        State vector
   * @param      _P        The covariance matrix
   * @param      _lidar_H  The lidar measurment matrix
   * @param      _radar_H  The radar measurment matrix
   * @param      _lidar_R  The lidar measurment noise matrix
   * @param      _radar_R  The radar measurment noise matrix
   * @param      _Q        The state covariance matrix
   * @param      _u        The control vector
   */
  void InitKfVars(KF::vec_t *_x,
                  KF::mat_t *_P,
                  KF::mat_t *_lidar_H,
                  KF::mat_t *_radar_H,
                  KF::mat_t *_lidar_R,
                  KF::mat_t *_radar_R,
                  KF::mat_t *_Q,
                  KF::vec_t *_u) {
    *_x << 0.0,  /// x
           0.0,  /// y
           0.0,  /// vx
           0.0;  /// vy

    *_P << 0.1, 0.0,  0.0,  0.0,  /// x
           0.0, 0.1,  0.0,  0.0,  /// y
           0.0, 0.0, 10.0,  0.0,  /// vx
           0.0, 0.0,  0.0, 10.0;  /// vy

    *_lidar_H << 1.0, 0.0, 0.0, 0.0,  /// x
                 0.0, 1.0, 0.0, 0.0;  /// y

    *_radar_H << 1.0, 0.0, 0.0, 0.0,  /// x
                 0.0, 1.0, 0.0, 0.0;  /// y

    *_lidar_R << 0.0225, 0.0,     /// x
                 0.0,    0.0225;  /// y

    *_radar_R << 0.09, 0.0,       /// x
                 0.0,  0.0009;    /// y

    *_Q << 0.1, 0.0, 0.0, 0.0,  /// x
           0.0, 0.1, 0.0, 0.0,  /// y
           0.0, 0.0, 0.1, 0.0,  /// vx
           0.0, 0.0, 0.0, 0.1;  /// vy

    *_u << 0.0,  /// x
           0.0,  /// y
           0.0,  /// vx
           0.0;  /// vy
  }

  /**
   * @brief      Initializes the Extended Kalman Filter variables.
   *
   * @param      _x        State vector
   * @param      _P        The covariance matrix
   * @param      _lidar_H  The lidar measurment matrix
   * @param      _radar_H  The radar measurment matrix
   * @param      _lidar_R  The lidar measurment noise matrix
   * @param      _radar_R  The radar measurment noise matrix
   * @param      _Q        The state covariance matrix
   * @param      _u        The control vector
   */
  void InitEkfVars(KF::vec_t *_x,
                   KF::mat_t *_P,
                   KF::mat_t *_lidar_H,
                   KF::mat_t *_radar_H,
                   KF::mat_t *_lidar_R,
                   KF::mat_t *_radar_R,
                   KF::mat_t *_Q,
                   KF::vec_t *_u) {
    *_x << 0.0,  /// x
           0.0,  /// y
           0.0,  /// vx
           0.0,  /// vy
           0.0,  /// ax
           0.0;  /// ay

    *_P << 0.1, 0.0,  0.0,   0.0,   0.0,   0.0,  /// x
           0.0, 0.1,  0.0,   0.0,   0.0,   0.0,  /// y
           0.0, 0.0, 10.0,   0.0,   0.0,   0.0,  /// vx
           0.0, 0.0,  0.0,  10.0,   0.0,   0.0,  /// vy
           0.0, 0.0,  0.0,   0.0, 100.0,   0.0,  /// ax
           0.0, 0.0,  0.0,   0.0,   0.0, 100.0;  /// ay

    *_lidar_H << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,  /// x
                 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;  /// y

    *_radar_H << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,  /// x
                 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;  /// y

    *_lidar_R << 0.0225, 0.0,     /// x
                 0.0,    0.0225;  /// y

    *_radar_R << 0.09, 0.0,       /// x
                 0.0,  0.0009;    /// y

    *_Q << 0.1, 0.0, 0.0, 0.0, 0.0, 0.0,   /// x
           0.0, 0.1, 0.0, 0.0, 0.0, 0.0,   /// y
           0.0, 0.0, 0.1, 0.0, 0.0, 0.0,   /// vx
           0.0, 0.0, 0.0, 0.1, 0.0, 0.0,  /// vy
           0.0, 0.0, 0.0, 0.0, 0.1, 0.0,   /// ax
           0.0, 0.0, 0.0, 0.0, 0.0, 0.1;   /// ay

    *_u << 0.0,  /// x
           0.0,  /// y
           0.0,  /// vx
           0.0,  /// vy
           0.0,  /// ax
           0.0;  /// ay
  }

  /**
   * @brief      Parses file with sensor measurements.
   *
   * @param[in]  _file  The file
   *
   * @return     Vector of sensor measurements.
   */
  std::vector<Measurement> Parse(const std::string& _file) {
    /// Store parsed measurements
    std::vector<Measurement> measurements;
    /// Parsed line
    std::string line;
    /// File handle for measurements
    std::ifstream f(_file);

    /// If file can be opened
    if (f.is_open()) {
      /// While there is a line to be retrieved from file handle
      while (getline(f, line)) {
        /// Convert string to stringstream
        std::stringstream stream(line);
        /// Define type for storing string without delimeters
        std::vector<std::string> delim_line;
        /// While there are no errors in stringstream
        while (stream.good()) {
          /// Remove delimeter and store data
          std::string substr;
          getline(stream, substr, ',');
          delim_line.push_back(substr);
        }
          Measurement measurement;
          /// If measurement is lidar, parse directly, otherwise,
          // measurement is radar, convert from polar to Cartesian & parse
          if (delim_line[0].compare("L") == 0) {
            measurement.type      = "Lidar";

            measurement.data.push_back(std::stod(delim_line[1]));  /// x
            measurement.data.push_back(std::stod(delim_line[2]));  /// y

            measurement.timestamp = std::stoll(delim_line[3]);
            measurement.gt_x      = std::stod(delim_line[4]);
            measurement.gt_y      = std::stod(delim_line[5]);
            measurement.gt_vx     = std::stod(delim_line[6]);
            measurement.gt_vy     = std::stod(delim_line[7]);
          } else {
            double rho       = std::stod(delim_line[1]);
            double phi       = std::stod(delim_line[2]);
            double rho_dot   = std::stod(delim_line[3]);

            /**
             * This is now a converted measurement Kalman Filter - Jacobian
             * and Symbolic Differentiation can be used in other applications
             * to linearize the measurement error. For this test, the main
             * focus is library.
             */
            coord2D_t radar_cartesian = Polar2Cartesian(rho, phi);

            measurement.type      = "Radar";

            measurement.data.push_back(std::get<0>(radar_cartesian));
            measurement.data.push_back(std::get<1>(radar_cartesian));

            measurement.timestamp = std::stoll(delim_line[4]);
            measurement.gt_x      = std::stod(delim_line[5]);
            measurement.gt_y      = std::stod(delim_line[6]);
            measurement.gt_vx     = std::stod(delim_line[7]);
            measurement.gt_vy     = std::stod(delim_line[8]);
          }
          measurements.push_back(measurement);
      }
      f.close();
    }

    return measurements;
  }

  /**
   * @brief      Processes the measurements'
   *
   * @param[in]  _z         The measurement vector
   * @param[in]  _H_lidar   The lidar measurement matrix
   * @param[in]  _H_radar   The radar measurement matrix
   * @param[in]  _R_lidar   The lidar measurment noise matrix
   * @param[in]  _R_radar   The radar measurment noise matrix
   * @param[in]  _Q         The state covariance matrix
   * @param      _estimate  The estimated states
   * @param      _actual    The actual states (ground truth)
   * @param[in]  padding    The padding
   *
   * @return     The final state vector.
   */
  Eigen::VectorXd Process(const std::vector<Measurement>& _z,
                          const Eigen::MatrixXd* _H_lidar,
                          const Eigen::MatrixXd* _H_radar,
                          const Eigen::MatrixXd* _R_lidar,
                          const Eigen::MatrixXd* _R_radar,
                          const Eigen::MatrixXd* _Q,
                          std::vector<Eigen::VectorXd>* _estimate,
                          std::vector<Eigen::VectorXd>* _actual,
                          size_t padding = 0) {
    for (int i = 0; i < _z.size(); i++) {
      /// Define measurment vector based on raw measurement data
      Eigen::VectorXd measurement(
        Eigen::Map<const Eigen::VectorXd>(
          _z[i].data.data(), _z[i].data.size()));

      /// Have Kalman Filter complete a single iteration (Update & Predict)
      if (_z[i].type.compare("Lidar") == 0) {
        kf_ptr->Update(measurement,
                       *_H_lidar,
                       *_R_lidar);
      } else {
        kf_ptr->Update(measurement,
                       *_H_radar,
                       *_R_radar);
      }
      kf_ptr->Predict(*_Q, _z[i].timestamp);

      /// Populate estimated & actual states if needed for RMSE calcuation
      if (_estimate != nullptr) {
        _estimate->push_back(kf_ptr->GetState());
      }
      if (_actual != nullptr) {
        Eigen::VectorXd ground_truth(4);
        ground_truth << _z[i].gt_x,
                        _z[i].gt_y,
                        _z[i].gt_vx,
                        _z[i].gt_vy;
        /// Added padding to state vector since data set in this
        /// test is 4-dimensional
        if (padding > 0) {
            Eigen::VectorXd ground_truth_padded(4 + padding);
            Eigen::VectorXd padded = Eigen::VectorXd::Zero(padding);
            ground_truth_padded << ground_truth,
                                   padded;

          _actual->push_back(ground_truth_padded);
        } else {
          _actual->push_back(ground_truth);
        }
      }
    }
    /// Return final state
    return kf_ptr->GetState();
  }
};

TEST_F(KfTest, CMKF_RMSE) {
  /// Vectors to hold data used in RMSE calculation
  std::vector<Eigen::VectorXd> estimated_states;
  std::vector<Eigen::VectorXd> actual_states;

  /// Parse CSV to extract sensor measurements
  std::vector<Measurement> measurements = Parse(f_lidar_radar);

  /// Process measurements to estimate final state
  Eigen::VectorXd curr_state = Process(measurements,
                                       kf_H_lidar_ptr,
                                       kf_H_radar_ptr,
                                       kf_R_lidar_ptr,
                                       kf_R_radar_ptr,
                                       kf_Q_ptr,
                                       &estimated_states,
                                       &actual_states);
  /// Define allowable RMSE
  Eigen::VectorXd allowable_rmse(4);
  allowable_rmse << 1.0,
                    1.0,
                    10.0,
                    10.0;

  /// Calculate RMSE for estimated & actual state vectors
  Eigen::VectorXd calculated_rmse = CalcRMSE(estimated_states, actual_states);

  /// Check that calculated RMSE is less than allowable
  bool result = !(calculated_rmse.array() > allowable_rmse.array()).any();

  /// RMSE for state vector
  std::cerr << "\nCMKF RMSE:\n" << calculated_rmse << std::endl;

  EXPECT_TRUE(result);
}

TEST_F(KfTest, CMKF_FinalPose) {
  /// Parse CSV to extract sensor measurements
  std::vector<Measurement> measurements = Parse(f_lidar_radar);

  /// Extract expected state from final ground truth measurement
  Eigen::VectorXd expected_state(4);
  Measurement final_state = measurements[measurements.size() - 1];
  expected_state << final_state.gt_x,
                    final_state.gt_y,
                    final_state.gt_vx,
                    final_state.gt_vy;

  /// Process measurements to estimate final state
  Eigen::VectorXd curr_state = Process(measurements,
                                       kf_H_lidar_ptr,
                                       kf_H_radar_ptr,
                                       kf_R_lidar_ptr,
                                       kf_R_radar_ptr,
                                       kf_Q_ptr,
                                       nullptr,
                                       nullptr);

  /// Print out states as reference
  std::cerr << "\nCMKF curr_state:\n" << curr_state << std::endl;
  std::cerr << "\nCMKF expected_state:\n" << expected_state << std::endl;

  EXPECT_TRUE(curr_state.isApprox(expected_state));
}

TEST_F(KfTest, CMEKF_RMSE) {
  /// Vectors to hold data used in RMSE calculation
  std::vector<Eigen::VectorXd> estimated_states;
  std::vector<Eigen::VectorXd> actual_states;

  /// Parse CSV to extract sensor measurements
  std::vector<Measurement> measurements = Parse(f_lidar_radar);

  /// Set pointer to derived class
  kf_ptr = ekf_ptr;

  /// Process measurements to estimate final state
  Eigen::VectorXd curr_state = Process(measurements,
                                       ekf_H_lidar_ptr,
                                       ekf_H_radar_ptr,
                                       ekf_R_lidar_ptr,
                                       ekf_R_radar_ptr,
                                       ekf_Q_ptr,
                                       &estimated_states,
                                       &actual_states,
                                       2);
  /// Define allowable RMSE
  Eigen::VectorXd allowable_rmse(6);
  allowable_rmse << 1.0,
                    1.0,
                    10.0,
                    10.0,
                    100.0,
                    100.0;

  /// Calculate RMSE for estimated & actual state vectors
  Eigen::VectorXd calculated_rmse = CalcRMSE(estimated_states, actual_states);

  /// Check that calculated RMSE is less than allowable
  bool result = !(calculated_rmse.array() > allowable_rmse.array()).any();

  /// RMSE for state vector
  std::cerr << "\nCMEKF RMSE:\n" << calculated_rmse << std::endl;

  EXPECT_TRUE(result);
}

TEST_F(KfTest, CMEKF_FinalPose) {
  /// Parse CSV to extract sensor measurements
  std::vector<Measurement> measurements = Parse(f_lidar_radar);

  /// Extract expected state from final ground truth measurement
  Eigen::VectorXd expected_state(6);
  Measurement final_state = measurements[measurements.size() - 1];
  expected_state << final_state.gt_x,
                    final_state.gt_y,
                    final_state.gt_vx,
                    final_state.gt_vy,
                    0.0,
                    0.0;

  /// Set pointer to derived class
  kf_ptr = ekf_ptr;

  /// Process measurements to estimate final state
  Eigen::VectorXd curr_state = Process(measurements,
                                       ekf_H_lidar_ptr,
                                       ekf_H_radar_ptr,
                                       ekf_R_lidar_ptr,
                                       ekf_R_radar_ptr,
                                       ekf_Q_ptr,
                                       nullptr,
                                       nullptr,
                                       2);

  /// Print out states as reference
  std::cerr << "\nCMEKF curr_state:\n" << curr_state << std::endl;
  std::cerr << "\nCMEKF expected_state:\n" << expected_state << std::endl;

  EXPECT_TRUE(curr_state.isApprox(expected_state));
}
}   //  namespace kalman_filter

int main(int argc, char **argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include "stdafx.h"
#include "VectorProcessor.h"
#include <algorithm>
#include <numeric>
#include <functional>
#include <boost/range/algorithm/transform.hpp>
#include <boost/phoenix.hpp>

using namespace std;
using namespace std::placeholders;
using namespace boost::phoenix::placeholders;
using namespace boost::phoenix;
using boost::transform;

void ProcessVector(std::vector<double> & numbers)
{
	auto IsPositive = [](double number){return number > 0; };

	size_t numberOfPositives = 0;

	// функция, суммирующая только положительные числа с подсчетом их количества
	auto addIfPositive = [&numberOfPositives](double acc, double current) {
		if (current > 0.0)
		{
			++numberOfPositives;
			return acc + current;
		}
		return acc;
	};

	auto sumOfPositives = accumulate(numbers.begin(), numbers.end(), 0.0, addIfPositive);

	double avg = (numberOfPositives > 0) ? sumOfPositives / numberOfPositives : 0.0;

	// TODO: Используй силу алгоритма std::transform, Люк, вместо этого цикла
	boost::transform(numbers, numbers.begin(), arg1 + avg);
	/*
	auto addAvg = bind(plus<double>(), _1, avg);
	transform(numbers, numbers.begin(), addAvg);
	*/
	/*transform(numbers, numbers.begin(), bind(plus<double>(), _1, avg));*/

	/*transform(numbers.begin(), numbers.end(), numbers.begin(), bind(plus<double>(), _1, avg));*/
	/*
	transform(numbers.begin(), numbers.end(), numbers.begin(), [&avg](double current){
		return current + avg;
	});
	*/
	/*
	for (auto &number : numbers)
	{
		number += avg;
	}*/
}


//  Copyright John Maddock 2009.
//  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)

#include "required_defines.hpp"

#include "performance_measure.hpp"

#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/array.hpp>

#define T double
#  include "../test/log1p_expm1_data.ipp"

template <std::size_t N>
double log1p_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::log1p(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(log1p_test, "log1p")
{
   double result = log1p_evaluate2(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

template <std::size_t N>
double expm1_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::expm1(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(expm1_test, "expm1")
{
   double result = expm1_evaluate2(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

#ifdef TEST_DCDFLIB
#include <dcdflib.h>

template <std::size_t N>
double log1p_evaluate2_dcd(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
   {
      double t = data[i][0];
      result += ::alnrel(&t);
   }
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(log1p_test_dcd, "log1p-dcd")
{
   double result = log1p_evaluate2_dcd(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

template <std::size_t N>
double expm1_evaluate2_dcd(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
   {
      double t = data[i][0];
      result += ::dexpm1(&t);
   }
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(expm1_test_dcd, "expm1-dcd")
{
   double result = expm1_evaluate2_dcd(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

#endif

#ifdef TEST_GSL

#include <gsl/gsl_sf.h>

template <std::size_t N>
double log1p_evaluate_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_log_1plusx(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(log1p_test, "log1p-gsl")
{
   double result = log1p_evaluate_gsl(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

template <std::size_t N>
double expm1_evaluate_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_expm1(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(expm1_test, "expm1-gsl")
{
   double result = expm1_evaluate_gsl(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

#endif

#ifdef TEST_CEPHES

extern "C" double expm1(double);
extern "C" double log1p(double);

template <std::size_t N>
double log1p_evaluate_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += ::log1p(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(log1p_test, "log1p-cephes")
{
   double result = log1p_evaluate_cephes(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

template <std::size_t N>
double expm1_evaluate_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += ::expm1(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(expm1_test, "expm1-cephes")
{
   double result = expm1_evaluate_cephes(log1p_expm1_data);

   consume_result(result);
   set_call_count(
      (sizeof(log1p_expm1_data)) / sizeof(log1p_expm1_data[0]));
}

#endif


#define BOOST_TEST_MODULE lue framework core math
#include <hpx/config.hpp>
#include <boost/test/unit_test.hpp>
#include "lue/framework/core/math.hpp"

namespace detail {

template<
    typename Input,
    typename Output>
auto check_equal = [](
        Input const from_min,
        Input const from_max,
        Output const to_min,
        Output const to_max,
        Input const value_to_map,
        Output const result_we_want)
    {
        BOOST_TEST_INFO("With value " << value_to_map);
        BOOST_CHECK_EQUAL(
            lue::map_to_range(
                from_min, from_max, to_min, to_max, value_to_map),
            result_we_want);
    };

}  // namespace detail

#define DEFINE_CHECK_EQUAL() \
    auto check_equal = [=]( \
            Input const value_to_map, \
            Output const result_we_want) \
        { \
            detail::check_equal<Input, Output>( \
                    from_min, from_max, to_min, to_max, value_to_map, \
                    result_we_want); \
        };

BOOST_AUTO_TEST_CASE(map_indices)
{
    using Input = std::size_t;
    using Output = std::size_t;

    // Equal input and output ranges
    {
        Input from_min = 0;
        Input from_max = 10;
        Output to_min = 0;
        Output to_max = 10;

        DEFINE_CHECK_EQUAL()

        check_equal(0, 0);
        check_equal(1, 1);
        check_equal(2, 2);
        check_equal(3, 3);
        check_equal(4, 4);
        check_equal(5, 5);
        check_equal(6, 6);
        check_equal(7, 7);
        check_equal(8, 8);
        check_equal(9, 9);
        check_equal(10, 10);
    }

    // Smaller input range than output range
    {
        Input from_min = 0;
        Input from_max = 4;
        Output to_min = 0;
        Output to_max = 9;

        DEFINE_CHECK_EQUAL()

        check_equal(0, 0);
        check_equal(1, 2);
        check_equal(2, 4);
        check_equal(3, 6);
        check_equal(4, 8);
    }

    // Larger input range than output range
    {
        Input from_min = 0;
        Input from_max = 9;
        Output to_min = 0;
        Output to_max = 4;

        DEFINE_CHECK_EQUAL()

        check_equal(0, 0);
        check_equal(1, 0);
        check_equal(2, 1);
        check_equal(3, 1);
        check_equal(4, 2);
        check_equal(5, 2);
        check_equal(6, 3);
        check_equal(7, 3);
        check_equal(8, 4);
        check_equal(9, 4);
    }

    // Input range with single element
    {
        Input from_min = 0;
        Input from_max = 0;
        Output to_min = 0;
        Output to_max = 4;

        DEFINE_CHECK_EQUAL()

        check_equal(0, 0);
    }

    // Output range with single element
    {
        Input from_min = 0;
        Input from_max = 9;
        Output to_min = 0;
        Output to_max = 0;

        DEFINE_CHECK_EQUAL()

        check_equal(0, 0);
        check_equal(5, 0);
        check_equal(9, 0);
    }
}

BOOST_AUTO_TEST_CASE(map_to_range_case_1)
{
    using Input = std::size_t;
    using Output = std::size_t;

    // 100 partitions → 3 localities
    Input from_min = 0;
    Input from_max = 99;
    Output to_min = 0;
    Output to_max = 2;

    DEFINE_CHECK_EQUAL()

    //  0 - 33: 0 (→ 34)
    // 33 - 66: 1 (→ 33)
    // 66 - 99: 2 (→ 33)
    check_equal( 0, 0);
    check_equal(33, 0);
    check_equal(34, 1);
    check_equal(66, 1);
    check_equal(67, 2);
    check_equal(99, 2);
}

#undef DEFINE_CHECK_EQUAL


/**
 * \file SecondOrderSVFFilter.cpp
 */

#include "SecondOrderSVFFilter.h"

#include <cassert>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename SVFCoefficients>
  struct SecondOrderSVFFilter<SVFCoefficients>::SVFState
  {
    typename SVFCoefficients::DataType iceq1;
    typename SVFCoefficients::DataType iceq2;

    SVFState()
    :iceq1(0), iceq2(0)
    {
    }
  };

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::SecondOrderSVFFilter(int nb_channels)
  :SVFCoefficients(nb_channels), state(new SVFState[nb_channels])
  {
  }

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::~SecondOrderSVFFilter()
  {
  }

  template<typename SVFCoefficients>
  void SecondOrderSVFFilter<SVFCoefficients>::full_setup()
  {
    state.reset(new SVFState[nb_input_ports]);
  }

  template<typename DataType>
  void SecondOrderSVFFilter<DataType>::process_impl(int64_t size) const
  {
    assert(nb_input_ports == nb_output_ports);

    for(int j = 0; j < nb_input_ports; ++j)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[j];
      DataType* ATK_RESTRICT output = outputs[j];

      for(int64_t i = 0; i < size; ++i)
      {
        DataType v3 = input[i] - state[j].iceq2;
        DataType v1 = a1 * state[j].iceq1 + a2 * v3;
        DataType v2 = state[j].iceq2 + a2 * state[j].iceq1 + a3 * v3;
        state[j].iceq1 = 2 * v1 - state[j].iceq1;
        state[j].iceq2 = 2 * v2 - state[j].iceq2;

        output[i] = m0 * input[i] + m1 * v1 + m2 * v2;
      }
    }
  }

  template<typename DataType>
  SecondOrderSVFBaseCoefficients<DataType>::SecondOrderSVFBaseCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels), cut_frequency(0), Q(1)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFBaseCoefficients<DataType>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    this->Q = Q;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFBaseCoefficients<DataType>::get_Q() const
  {
    return Q;
  }

  template<typename DataType_>
  SecondOrderSVFLowPassCoefficients<DataType_>::SecondOrderSVFLowPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFLowPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1/Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 0;
    m2 = 1;
  }

  template<typename DataType_>
  SecondOrderSVFBandPassCoefficients<DataType_>::SecondOrderSVFBandPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFBandPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 1;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFHighPassCoefficients<DataType_>::SecondOrderSVFHighPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFHighPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = -1;
  }

  template<typename DataType_>
  SecondOrderSVFNotchCoefficients<DataType_>::SecondOrderSVFNotchCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFNotchCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 2;
  }

  template<typename DataType_>
  SecondOrderSVFPeakCoefficients<DataType_>::SecondOrderSVFPeakCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFPeakCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFBellCoefficients<DataType_>::SecondOrderSVFBellCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {

  }

  template<typename DataType_>
  void SecondOrderSVFBellCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFBellCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFBellCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q* gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain * gain - 1);
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFLowShelfCoefficients<DataType_>::SecondOrderSVFLowShelfCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {

  }

  template<typename DataType_>
  void SecondOrderSVFLowShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFLowShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFLowShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain - 1);
    m2 = gain * gain - 1;
  }

  template<typename DataType_>
  SecondOrderSVFHighShelfCoefficients<DataType_>::SecondOrderSVFHighShelfCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFHighShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFHighShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFHighShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q* gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = gain * gain;
    m1 = k * (1 - gain) * gain;
    m2 = 1 - gain * gain;
  }

  template class SecondOrderSVFBaseCoefficients<float>;
  template class SecondOrderSVFBaseCoefficients<double>;

  template class SecondOrderSVFLowPassCoefficients<float>;
  template class SecondOrderSVFLowPassCoefficients<double>;
  template class SecondOrderSVFBandPassCoefficients<float>;
  template class SecondOrderSVFBandPassCoefficients<double>;
  template class SecondOrderSVFHighPassCoefficients<float>;
  template class SecondOrderSVFHighPassCoefficients<double>;
  template class SecondOrderSVFNotchCoefficients<float>;
  template class SecondOrderSVFNotchCoefficients<double>;
  template class SecondOrderSVFPeakCoefficients<float>;
  template class SecondOrderSVFPeakCoefficients<double>;
  template class SecondOrderSVFBellCoefficients<float>;
  template class SecondOrderSVFBellCoefficients<double>;
  template class SecondOrderSVFLowShelfCoefficients<float>;
  template class SecondOrderSVFLowShelfCoefficients<double>;
  template class SecondOrderSVFHighShelfCoefficients<float>;
  template class SecondOrderSVFHighShelfCoefficients<double>;

  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<double> >;
}


#define _USE_MATH_DEFINES

#include <fstream>
#include <iostream>
#include <omp.h>
#include <cmath>
#include <cstdio>
#include <chrono>
#include <string>
#include <armadillo>
#include <vector>
#include "wigner/gaunt.hpp"

using namespace std::chrono;
using namespace arma;

static const double kB = 1.3806504e-23;         // J/K
static const double NA = 6.02214179e23;         // 1/mol
static const double EHARTREE = 4.35974434e-18;  // J/Hartree
static const double AMU = 1.660538921e-27;      // kg/amu
static const double HBAR = 1.054571726e-34;     // J.s
static const double HBAR1 = HBAR / EHARTREE;    // Hartree.s
static const double HBAR2 = HBAR * 1e20 / AMU;  // amu.Å^2/s
static const double SCH4 = 186.25; // J/mol.K

Mat<double> getHamiltonian(int lmax, double Ix, double Iy, double Iz) {
    /* Construct the Hamiltonian Matrix
     * Inputs:  lmax;   the maximum quantum number for the spherical basis
     *             I;   the gas-phase moments of inertia [=] amu*Å^2
     * Outputs:    H;   the Hamiltonian matrix (Hermitian) [=] Hartree
     */
    std::cout << "Lmax = " << lmax << std::endl;
    double size = (double(1.0/3.0)*(lmax+1)*(2*lmax+1)*(2*lmax+3));
    std::cout << "Dimensions of the matrix: " << size << std::endl;
    Mat<double> H = zeros<mat>(size+1, size+1);
    //SpMat<double> H = sp_mat(size, size);

    // Define rotational constants:
    double B = HBAR1*HBAR2/(2.0*Iz);
    double A = HBAR1*HBAR2/(2.0*Iy);
    double C = HBAR1*HBAR2/(2.0*Ix);

    double kap;
    if (A == B && A == C) {     //SPHERICAL ROTOR
        kap = 0;
    } else {
        kap = (2.0*B - (A + C)) / (A - C);
    }

    std::cout << "kappa val is " << kap << "." << std::endl;
    #pragma omp parallel
    {
        double g;
        double a;
        #pragma omp for
        for (int el = 0; el < lmax+1; el++) {
            for (int m = -el; m <= el; m++) {
                for (int k = -el; k <= el; k++) {
                    unsigned long long j = (4*el*el*el/3.0) + 2*el*el + (5*el/3.0) + 2*m*el + m + k;
                    //std::cout << j << '\t';
                    for (int ell = 0; ell < lmax+1; ell++) {
                        for (int mm = -ell; mm <= ell; mm++) {
                            for (int kk = -ell; kk <= ell; kk++) {
                                unsigned long long i = (4*ell*ell*ell/3.0) + 2*ell*ell + (5*ell/3.0)
                                    + 2*mm*ell + mm + kk;
                                //if (j < i) {
                                //    continue;
                                //} else {
                                //    H(i,j) += 1;
                                //}
                                if (i == j) {
                                    try {
                                        //H(i,j) += B*el*(el+1);
                                        H(i,j) += 0.5*(A+C)*el*(el+1) + 0.5*(A-C)*kap*k*k;
                                        if (k-2 >= -el) {
                                            H(i-2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k-1))*sqrt(el*(el+1)-(k-1)*(k-2));
                                            if (abs(double(j - i+2)) != 2) {
                                                std::cout << "(i-2, j) index spacing incorrect @ (" << i-2 << ',' << j << ") --> ("
                                                    << el << ',' << m << ',' << k << ')' << std::endl;
                                            }

                                        }
                                        if (k+2 <= el) {
                                            H(i+2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k+1))*sqrt(el*(el+1)-(k+1)*(k+2));
                                            if (abs(double(i+2 - j)) != 2) {
                                                std::cout << "(i+2, j) index spacing incorrect @ (" << i+2 << ',' << j << ") --> ("
                                                    << el << ',' << m << ',' << k << ')' << std::endl;
                                            }

                                        }
                                    } catch (const std::exception& e) {
                                        std::cout << "Failure at index: " <<
                                            i << "\t(" << el << ',' << m << ',' <<
                                            k << ')' << std::endl;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    } // end parallel
    std::cout << std::endl;
    //H.print("H = ");
    return H;
}

SpMat<double> getSparseHam(int lmax, double Ix, double Iy, double Iz) {
    /* Construct the Hamiltonian Matrix
     * Inputs:  lmax;   the maximum quantum number for the spherical basis
     *             I;   the gas-phase moments of inertia [=] amu*Å^2
     * Outputs:    H;   the Hamiltonian matrix (Hermitian) [=] Hartree
     */
    std::cout << "Lmax = " << lmax << std::endl;
    double size = (double(1.0/3.0)*(lmax+1)*(2*lmax+1)*(2*lmax+3));
    std::cout << "Dimensions of the sparse matrix: " << size << std::endl;
    SpMat<double> H = sp_mat(size, size);

    // Define rotational constants:
    double B = HBAR1*HBAR2/(2.0*Iz);
    double A = HBAR1*HBAR2/(2.0*Iy);
    double C = HBAR1*HBAR2/(2.0*Ix);

    double kap;
    if (A == B && A == C) {     //SPHERICAL ROTOR
        kap = 0;
    } else {
        kap = (2.0*B - (A + C)) / (A - C);
    }
    #pragma omp parallel
    {
        double g;
        double a;
        #pragma omp for
        for (int el = 0; el < lmax+1; el++) {
            for (int m = -el; m <= el; m++) {
                for (int k = -el; k <= el; k++) {
                    unsigned long long j = (4*el*el*el/3.0) + 2*el*el + (5*el/3.0) + 2*m*el + m + k;
                    //std::cout << j << '\t';
                    for (int ell = 0; ell < lmax+1; ell++) {
                        for (int mm = -ell; mm <= ell; mm++) {
                            for (int kk = -ell; kk <= ell; kk++) {
                                unsigned long long i = (4*ell*ell*ell/3.0) + 2*ell*ell + (5*ell/3.0)
                                    + 2*mm*ell + mm + kk;
                                //if (j < i) {
                                //    continue;
                                //} else {
                                //    H(i,j) += 1;
                                //}
                                if (i == j) {
                                    try {
                                        //H(i,j) += B*el*(el+1);
                                        H(i,j) += 0.5*(A+C)*el*(el+1) + 0.5*(A-C)*kap*k*k;
                                        if (k-2 >= -el) {
                                            H(i-2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k-1))*sqrt(el*(el+1)-(k-1)*(k-2));
                                            if (isnan(H(i-2,j))) {
                                                std:: cout << "NaN @ (" << i-2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                            if (abs(double(j - i+2)) != 2) {
                                                std::cout << "Index spacing incorrect @ (" << i-2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                        }
                                        if (k+2 <= el) {
                                            H(i+2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k+1))*sqrt(el*(el+1)-(k+1)*(k+2));
                                            if (isnan(H(i+2,j))) {
                                                std:: cout << "NaN @ (" << i+2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                            if (abs(double(i+2 - j)) != 2) {
                                                std::cout << "Index spacing incorrect @ (" << i+2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                        }
                                    } catch (const std::exception& e) {
                                        std::cout << "Failure at index: " <<
                                            i << "\t(" << el << ',' << m << ',' <<
                                            k << ')' << std::endl;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    } // end parallel
    return H;
}

std::string getDirectory(std::string sysname) {
    std::string dirname = "/Users/lancebettinson/Thesis/umrr/code/hamiltonian-cpp/data";
    if (sysname == "") {
        std::string sysname;
        std::cout << "Enter system name:" << std::endl;
        std::cin >> sysname;
    }
    return dirname+'/'+sysname;
}

Col<double> getCoefficients(std::string sysname) {
    std::string dirname = getDirectory(sysname);
    std::string filename = dirname+'/'+"vdat.txt";
    std::ifstream is(filename);
    if (is.fail())
    {
        std::cout << "cannot open file " << filename;
    }
    double theta, phi, v;
    std::vector<double> my_vec;
    while (is) {
        if (!(is >> theta >> phi >> v)) {
            break;
        }
        //std::cout << theta << '\t' << phi << '\t' << v << std::endl;
        my_vec.push_back(v);
    }
    Col<double> cvec = conv_to<vec>::from(my_vec);
    //cvec.print();
    is.close();
    return cvec;
}

std::vector<double> getMomentOfInertia(std::string sysname="METH-CHA") {
    std::string dirname = getDirectory(sysname);
    std::string filename = dirname+'/'+"I.txt";
    std::ifstream is(filename);
    double val;
    std::vector<double> Ivec;
    while (is) {
        if (!(is >> val)) {
            break;
        }
        Ivec.push_back(val);
    }
    return Ivec;
}

double getPartitionFunction(double T, Mat<double>& H, int sym=1) {
    /* Solve the Eigenvalues
     * Inputs:  T;  the temperature [=] K
     *          H;  the Hamiltonian matrix [=] Hartree
     */
    double b = pow(kB * T, -1) * EHARTREE;

    //Col<double> eigval = eig_sym(H);
    //double Q = 0;
    ////b = 1;
    //int count = 0;
    //for (double e : eigval) {
    //    Q += exp(-b * e);
    //}
    //for (double e : eigval) {
    //    if (count == 5) {
    //        //std::cout << std::endl;
    //    } //std::cout << exp(-b*e)/Q << '\t';
    //    count++;
    //}
    //std::cout << std::endl << "Q predicted by eig_sym: " << Q/sym << std::endl;
    Mat<double> bH = -b*H;
    Mat<double> expbH = expmat_sym(bH);
    double tr = trace(expbH);
    std::cout << pow(b,-1) << std::endl;
    std::cout << std::endl << "Q predicted by eig_sym: " << tr/sym << std::endl;
    return double(tr/sym);
    //return double(Q/sym);
}

double getSparseQ(double T, SpMat<double>& H, int sym=1) {
    /* Solve the Eigenvalues for Sparse Matrix
     * Inputs:  T;  the temperature [=] K
     *          H;  the (sparse) Hamiltonian matrix [=] Hartree
     */
    double b = pow(kB * T, -1) * EHARTREE;
    //SpMat<double> bH = -b*H;

    vec eigval;
    mat eigvec;

    std::cout << "Number of rows: " << H.n_rows << std::endl;
    eigs_sym(eigval, eigvec, H, H.n_rows-1);
    double Q = 0;
    std::cout << "Eigenvalues: " << std::endl;
    for (double e : eigval) {
        std::cout << e << '\t';
        Q += exp(-b * e);
    }
    std::cout << std::endl;
    std::cout << "Q predicted by eigs_sym: " << Q/sym << std::endl;
    return double(Q/sym);
}

int main() {
    auto start = high_resolution_clock::now();

    std::string sysname = "ETH1-CHA";
    int sigma = 1;
    std::string dirname = getDirectory(sysname);

    std::vector<double> Ivec = getMomentOfInertia(sysname);
    std::cout << "Printing I:" << std::endl;
    for (double i : Ivec) {
        std::cout << i << '\t';
    }
    //std::cout << "Rotational Constant: = " << HBAR1*HBAR2/2.0/I << std::endl;
    std::cout << "kT = " << kB*300 / EHARTREE << std::endl;
    //std::cout << "Ratio = " << HBAR1*HBAR2/2.0/I *EHARTREE/(kB*300) << std::endl;
    //std::cout << "Qapprox = " << kB*300/EHARTREE / (HBAR1*HBAR2/2.0/I) << std::endl;

    Col<double> ahat = getCoefficients(sysname);
    std::cout << "Directory is: " << dirname << std::endl;

    /*
     *  Sparse Matrix Implementation
     */
    //SpMat<double> spH = getSparseHam(25, Ivec[0], Ivec[1], Ivec[2]);
    ////spH.print("My sparse Ham =");
    //if (!spH.is_hermitian()) {
    //    std::cout << "NOT HERMITIAN, CHECK" << std::endl;
    //} else {
    //    std::cout << "IS HERMITIAN, GOOD TO GO" << std::endl;
    //}
    //double spQ = getSparseQ(300, spH, 3);
    //std::cout << "spQ = " << spQ << std::endl;

    /*
     *  Dense Matrix Implementation
     */
    Mat<double> H = getHamiltonian(31, Ivec[0], Ivec[1], Ivec[2]);
    if (!H.is_hermitian()) {
        std::cout << "NOT HERMITIAN, CHECK" << std::endl;
    } else {
        std::cout << "HERMITIAN YAY" << std::endl;
    }
    //H.print("My ham:");
    double Q = getPartitionFunction(298, H, sigma);

    /*
     *  Classical Partition Function
     */
    double B = HBAR1*HBAR2/(2.0*Ivec[2]);
    double A = HBAR1*HBAR2/(2.0*Ivec[1]);
    double C = HBAR1*HBAR2/(2.0*Ivec[0]);
    std::cout << "Rotational constants / Hartree:\n" << A << '\t' << B << '\t' << C << '\t' << std::endl;
    std::cout << "kB T / Hartree:\n" << kB * 298 / EHARTREE << std::endl;
    std::cout << "Qapprox = " << sqrt(M_PI)/sigma * sqrt(pow(kB*300/EHARTREE, 3) / (A*B*C));

    std::cout << std::endl;
    std::cout << std::endl;
    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);
    std::cout << std::endl << duration.count()/1e6 << " secs" << std::endl;
    return 0;
}


#include <glm/models/links/logc.hpp>

#include <armadillo>

using namespace arma;

logc_link::logc_link()
    : glm_link::glm_link( "logc" )
{
}

vec
logc_link::init_beta(const mat &X, const vec &y) const
{
    vec mu = (y + 0.5) / 2.0;
    vec eta = log( 1 - mu );

    return pinv( X ) * eta;
}

vec
logc_link::mu(const arma::vec &eta) const
{
    return 1.0 - exp( eta );
}

vec
logc_link::eta(const arma::vec &mu) const
{
    return log( 1 - mu );
}

vec
logc_link::mu_eta(const arma::vec &mu) const
{
    return -1 / ( 1.0 - mu );
}