#pragma once

#include <boost/assert.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

namespace polar {

namespace detail {

template<class Matrix>
bool invert(Matrix const& input, Matrix& inverse)
{
    using namespace boost::numeric::ublas;

    typedef permutation_matrix<std::size_t> pmatrix;

    BOOST_ASSERT(input.size1() == input.size2());

    Matrix A(input);
    pmatrix pm(A.size1());

    if (lu_factorize(A, pm) != 0)
        return false;

    inverse.assign(identity_matrix<typename Matrix::value_type>(A.size1()));
    lu_substitute(A, pm, inverse);

    return true;
}

} // namespace detail

template<typename Matrix_in, typename Matrix_out>
void polar_decomposition(Matrix_in const& A, Matrix_out& U, Matrix_out& H,
        double rtol = 1.0e-8, std::size_t max_iter = 0)
{
    typedef typename Matrix_out::value_type value_type;

    BOOST_ASSERT(A.size1() == A.size2());

    U.resize(A.size1(), A.size2());
    U.assign(A);
    H.resize(A.size1(), A.size2());
    Matrix_out X(A.size1(), A.size2());

    std::size_t count = 0;
    bool close_to_convergence = false;

    while (true)
    {
        X.assign(U);
        detail::invert(X, H);
        value_type gamma = 1.0;

        if (!close_to_convergence)
        {
            value_type alpha = std::sqrt(norm_1(X) * norm_inf(X));
            value_type beta  = std::sqrt(norm_1(H) * norm_inf(H));
            gamma = std::sqrt(beta / alpha);
        }

        U.assign(0.5 * (gamma * X + 1.0 / gamma * trans(H)));

        if (norm_1(X - U) < rtol * norm_1(X))
            break;

        if (max_iter != 0 && ++count == max_iter)
            break;

        if (!close_to_convergence && norm_frobenius(X - U) < 1.0e-2)
            close_to_convergence = true;
    }

    H.assign(0.5 * (prod(trans(U), A) + prod(trans(A), U)));
}

} // namespace polar


/**
 * @file lbfgs_test.cpp
 *
 * Tests the L-BFGS optimizer on a couple test functions.
 *
 * @author Ryan Curtin (gth671b@mail.gatech.edu)
 *
 * 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/optimizers/lbfgs/lbfgs.hpp>

#include <mlpack/core/optimizers/problems/rosenbrock_function.hpp>
#include <mlpack/core/optimizers/problems/rosenbrock_wood_function.hpp>
#include <mlpack/core/optimizers/problems/colville_function.hpp>

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

using namespace mlpack::optimization;
using namespace mlpack::optimization::test;

BOOST_AUTO_TEST_SUITE(LBFGSTest);

/**
 * Tests the L-BFGS optimizer using the Rosenbrock Function.
 */
BOOST_AUTO_TEST_CASE(RosenbrockFunctionTest)
{
  RosenbrockFunction f;
  L_BFGS lbfgs;
  lbfgs.MaxIterations() = 10000;

  arma::vec coords = f.GetInitialPoint();
  if (!lbfgs.Optimize(f, coords))
    BOOST_FAIL("L-BFGS optimization reported failure.");

  double finalValue = f.Evaluate(coords);

  BOOST_REQUIRE_SMALL(finalValue, 1e-5);
  BOOST_REQUIRE_CLOSE(coords[0], 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(coords[1], 1.0, 1e-5);
}

/**
 * Tests the L-BFGS optimizer using the Colville Function.
 */
BOOST_AUTO_TEST_CASE(ColvilleFunctionTest)
{
  ColvilleFunction f;
  L_BFGS lbfgs;
  lbfgs.MaxIterations() = 10000;

  arma::vec coords = f.GetInitialPoint();
  if (!lbfgs.Optimize(f, coords))
    BOOST_FAIL("L-BFGS optimization reported failure.");

  BOOST_REQUIRE_CLOSE(coords[0], 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(coords[1], 1.0, 1e-5);
}

/**
 * Tests the L-BFGS optimizer using the Wood Function.
 */
BOOST_AUTO_TEST_CASE(WoodFunctionTest)
{
  WoodFunction f;
  L_BFGS lbfgs;
  lbfgs.MaxIterations() = 10000;

  arma::vec coords = f.GetInitialPoint();
  if (!lbfgs.Optimize(f, coords))
    BOOST_FAIL("L-BFGS optimization reported failure.");

  double finalValue = f.Evaluate(coords);

  BOOST_REQUIRE_SMALL(finalValue, 1e-5);
  BOOST_REQUIRE_CLOSE(coords[0], 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(coords[1], 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(coords[2], 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(coords[3], 1.0, 1e-5);
}

/**
 * Tests the L-BFGS optimizer using the generalized Rosenbrock function.  This
 * is actually multiple tests, increasing the dimension by powers of 2, from 4
 * dimensions to 1024 dimensions.
 */
BOOST_AUTO_TEST_CASE(GeneralizedRosenbrockFunctionTest)
{
  for (int i = 2; i < 10; i++)
  {
    // Dimension: powers of 2
    int dim = std::pow(2.0, i);

    GeneralizedRosenbrockFunction f(dim);
    L_BFGS lbfgs(20);
    lbfgs.MaxIterations() = 10000;

    arma::vec coords = f.GetInitialPoint();
    if (!lbfgs.Optimize(f, coords))
      BOOST_FAIL("L-BFGS optimization reported failure.");

    double finalValue = f.Evaluate(coords);

    // Test the output to make sure it is correct.
    BOOST_REQUIRE_SMALL(finalValue, 1e-5);
    for (int j = 0; j < dim; j++)
      BOOST_REQUIRE_CLOSE(coords[j], 1.0, 1e-5);
  }
}

/**
 * Tests the L-BFGS optimizer using the Rosenbrock-Wood combined function.  This
 * is a test on optimizing a matrix of coordinates.
 */
BOOST_AUTO_TEST_CASE(RosenbrockWoodFunctionTest)
{
  RosenbrockWoodFunction f;
  L_BFGS lbfgs;
  lbfgs.MaxIterations() = 10000;

  arma::mat coords = f.GetInitialPoint();
  if (!lbfgs.Optimize(f, coords))
    BOOST_FAIL("L-BFGS optimization reported failure.");

  double finalValue = f.Evaluate(coords);

  BOOST_REQUIRE_SMALL(finalValue, 1e-5);
  for (int row = 0; row < 4; row++)
  {
    BOOST_REQUIRE_CLOSE((coords(row, 0)), 1.0, 1e-5);
    BOOST_REQUIRE_CLOSE((coords(row, 1)), 1.0, 1e-5);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <geometry/draw.hpp>
#include <imgproc/gradient_adapter.hpp>
#include <imgproc/derivative_gradient.hpp>
#include <imgproc/susan.hpp>
#include <imgproc/rcmg.hpp>
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/quadratureSF.hpp>
#include <imgproc/quadratureLGF.hpp>
#include <imgproc/pc_sqf.hpp>
#include <imgproc/pc_lgf.hpp>
#include <imgproc/pc_matlab.hpp>
#include <imgproc/image_operator.hpp>
#include <utility/matlab_helpers.hpp>

#include <edge/nms.hpp>
#include <edge/zc.hpp>
#include <edge/spe.hpp>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>

#define WRITE_IMAGE_FILES
//#define SHOW_IMAGES

using namespace lsfm;
using namespace std;

template<class FT, template<class> class PT = cv::Point_>
struct GroundTruth {
    GroundTruth() {
        cv::Mat tmp(32000, 32000, CV_8U);
        tmp.setTo(0);

        Polygon<FT, PT> poly;
        poly.push_back(PT<FT>(5089, 2023));
        poly.push_back(PT<FT>(29947, 2023));
        poly.push_back(PT<FT>(20971, 16007));
        poly.push_back(PT<FT>(29947, 29959));
        poly.push_back(PT<FT>(5089, 29959));
        poly.push_back(PT<FT>(2017, 16007));

        double scale = 0.01;
        poly.fill(tmp, 180);

        //cv::resize(tmp, img, cv::Size(), scale, scale, cv::INTER_AREA); //-> large error, even with sub precision
        //cv::GaussianBlur(tmp, tmp, cv::Size(101, 101),20); // long runtime
        cv::blur(tmp, tmp, cv::Size(100, 100));
        cv::resize(tmp, img, cv::Size(), scale, scale, cv::INTER_NEAREST);
        poly.scale(scale);
        segments = poly.edges();
    }

    LineSegment2Vector<FT,PT> segments;
    cv::Mat img;

    FT error(const PT<FT> &p) const {
        FT e = std::numeric_limits<FT>::max();
        for_each(segments.begin(), segments.end(), [&](const LineSegment2<FT, PT> &seg) {
            if (!seg.inRangeTol(p, 1))
                return;
            e = std::min(e, std::abs(seg.distance(p)));
        });
        return e;
    }

    size_t line(const PT<FT> &p, FT &err = FT()) const {
        size_t ret = 0;
        err = std::numeric_limits<FT>::max();
        for (size_t i = 0; i != segments.size(); ++i) {
            if (!segments[i].inRangeTol(p, 1))
                continue;
            FT dist = std::abs(segments[i].distance(p));
            if (dist < err) {
                err = dist;
                ret = i;
            }
        }
        return ret;
    }

    void draw(cv::Mat &out, const PT<FT> &p, size_t l, FT err) const {
        static cv::Vec3b color[] = { cv::Vec3b(255, 0, 0), cv::Vec3b(0, 255, 0), cv::Vec3b(255, 255, 0), cv::Vec3b(255, 0, 255), cv::Vec3b(0, 255, 255), cv::Vec3b(255, 255, 255), cv::Vec3b(0, 0, 255) };
        if (err > 1 || l > 5)
            lsfm::set(out, cv::Point(getX(p), getY(p)), color[6]);
        else
            lsfm::set(out, cv::Point(getX(p), getY(p)), color[l]);
    }
};

template<class FT, template<class> class PT>
void outputSet(const std::string &name, const GroundTruth<FT, PT> &gt, const std::vector<PT<FT>> &points, bool show = true) {

    cv::Mat out(gt.img.size(), CV_8UC3);
    out.setTo(0);

    int outlier = 0;
    FT err = 0, errSqr = 0;
    for_each(points.begin(), points.end(), [&](const PT<FT> &p) {
        FT e;
        size_t l = gt.line(p, e);
        if (e > 1)
            ++outlier;
        else {
            err += e;
            errSqr += e*e;
        }
        if (getX(p) > -1 && getX(p) < gt.img.cols && getY(p) > -1 && getY(p) < gt.img.rows)
            gt.draw(out, p, l, e);
    });
    err /= (points.size() - outlier);
    errSqr /= (points.size() - outlier);
    std::cout << name << " - points: " << points.size() << ", outlier: " << outlier << ", mean error: " << err << ", std deviation: " << sqrt(errSqr - err*err) << std::endl;
    if (show)
        cv::imshow(name.c_str(), out);
}

int main(int argc, char** argv)
{
    typedef double FT;
    DerivativeGradient<uchar, FT, FT, FT, ScharrDerivative> grad(0,255);
    LaplaceCV<uchar,FT> laplace(5,0,255);
    //LoG<uchar, FT> laplace(5, 1,1,0, 255);
    NonMaximaSuppression<FT, FT, FT, FastNMS8<FT, FT, FT>> nms;
    NonMaximaSuppression<FT, FT, FT, PreciseNMS<FT, FT, false,FT,EMap8, CubicInterpolator,Polar>> pnms;
    ZeroCrossing<FT, FT, FT, FastZC<FT, FT, FT>> zc;
    ZeroCrossing<FT, FT, FT, PreciseZC<FT, FT, FT, NCC_BASIC, EZCMap8, CubicInterpolator, Polar>> pzc;
    PixelEstimator<FT,cv::Point_<FT>> pe;
    // CoGEstimate, LinearEstimate, QuadraticEstimate, SobelEstimate, LinearInterpolator, CubicInterpolator
    PixelEstimator<FT, cv::Point_<FT>, SubPixelEstimator<FT,FT,cv::Point_, QuadraticEstimate, CubicInterpolator>> spe;
    PixelEstimator<FT, cv::Point_<FT>, SubPixelEstimator<FT, FT, cv::Point_, SobelZCEstimate, CubicInterpolator>> spezc;
    GroundTruth<FT, cv::Point_> gt;

    grad.process(gt.img);
    nms.process(grad);
    IndexVector idxs = nms.hysteresis_edgels();
    std::vector<cv::Point_<FT>> points, pointsSp, pointsSpDir;
    pe.convert(idxs, points, grad.magnitude(),nms.directionMap());
    spe.convert(idxs, pointsSp, grad.magnitude(), nms.directionMap());
    spe.convertDir(idxs, pointsSpDir, grad.magnitude(), grad.direction());

    outputSet("fnms_pe", gt, points);
    outputSet("fnms_spe", gt, pointsSp);
    outputSet("fnms_spe_dir", gt, pointsSpDir);

    cv::waitKey();

    pnms.process(grad);
    idxs = pnms.hysteresis_edgels();
    pe.convert(idxs, points, grad.magnitude(), pnms.directionMap());
    spe.convert(idxs, pointsSp, grad.magnitude(), pnms.directionMap());
    spe.convertDir(idxs, pointsSpDir, grad.magnitude(), grad.direction());

    outputSet("pnms_pe", gt, points);
    outputSet("pnms_spe", gt, pointsSp);
    outputSet("pnms_spe_dir", gt, pointsSpDir);

    cv::waitKey();

    laplace.process(gt.img);
    zc.process(laplace);
    idxs = zc.hysteresis_edgels();
    pe.convert(idxs, points, laplace.laplace(), zc.directionMap());
    spezc.convert(idxs, pointsSp, laplace.laplace(), zc.directionMap());
    spezc.convertDir(idxs, pointsSpDir, laplace.laplace(), grad.direction());

    //cv::imshow("gt", gt.img);
    outputSet("fzc_pe", gt, points);
    outputSet("fzc_spe", gt, pointsSp);
    outputSet("fzc_spe_dir", gt, pointsSpDir);

    cv::waitKey();

    zc.processG(laplace,grad);
    idxs = zc.hysteresis_edgels();
    pe.convert(idxs, points, laplace.laplace(), zc.directionMap());
    spezc.convert(idxs, pointsSp, laplace.laplace(), zc.directionMap());
    spezc.convertDir(idxs, pointsSpDir, laplace.laplace(), grad.direction());

    //cv::imshow("gt", gt.img);
    outputSet("fzcg_pe", gt, points);
    outputSet("fzcg_spe", gt, pointsSp);
    outputSet("fzcg_spe_dir", gt, pointsSpDir);
    cv::waitKey();

    pzc.process(laplace);
    idxs = pzc.hysteresis_edgels();
    pe.convert(idxs, points, laplace.laplace(), pzc.directionMap());
    spezc.convert(idxs, pointsSp, laplace.laplace(), pzc.directionMap());
    spezc.convertDir(idxs, pointsSpDir, laplace.laplace(), grad.direction());

    //cv::imshow("gt", gt.img);
    outputSet("pzc_pe", gt, points);
    outputSet("pzc_spe", gt, pointsSp);
    outputSet("pzc_spe_dir", gt, pointsSpDir);

    cv::waitKey();

    pzc.processG(laplace, grad);
    idxs = pzc.hysteresis_edgels();
    pe.convert(idxs, points, laplace.laplace(), pzc.directionMap());
    spezc.convert(idxs, pointsSp, laplace.laplace(), pzc.directionMap());
    spezc.convertDir(idxs, pointsSpDir, laplace.laplace(), grad.direction());

    //cv::imshow("gt", gt.img);
    outputSet("pzcg_pe", gt, points);
    outputSet("pzcg_spe", gt, pointsSp);
    outputSet("pzcg_spe_dir", gt, pointsSpDir);

    cv::imshow("gt", gt.img);
    cv::waitKey();
    return 0;
}


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <math/prim/mat/prob/VectorIntRNGTestRig.hpp>
#include <limits>
#include <vector>

class BinomialTestRig : public VectorIntRNGTestRig {
 public:
  BinomialTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {}, {0, 1, 3, 8},
                            {}, {-1, -5, -7}, {0.0, 0.1, 0.7, 0.99}, {0},
                            {-0.1, 1.2}, {-1, 2}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& N, const T2& theta, const T3&,
                        T_rng& rng) const {
    return stan::math::binomial_rng(N, theta, rng);
  }

  template <typename T1>
  double pmf(int y, T1 N, double theta, double) const {
    if (y <= N) {
      return std::exp(stan::math::binomial_lpmf(y, N, theta));
    } else {
      return 0.0;
    }
  }
};

TEST(ProbDistributionsBinomial, errorCheck) {
  check_dist_throws_int_first_argument(BinomialTestRig());
}

TEST(ProbDistributionsBinomial, distributionCheck) {
  check_counts_int_real(BinomialTestRig());
}


/**
 * @file eigenray_extra_test.cc
 *
 * Perform eigenrays tests that run too slow to be included
 * in the normal suite of regression tests.
 */
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <usml/waveq3d/waveq3d.h>
#include <iostream>
#include <iomanip>
#include <fstream>

BOOST_AUTO_TEST_SUITE(eigenray_extra_test)

using namespace boost::unit_test;
using namespace usml::waveq3d;

static const double time_step = 0.100 ;
static const double f0 = 2000 ;
static const double src_lat = 45.0;        // location = mid-Atlantic
static const double src_lng = -45.0;
static const double c0 = 1500.0;           // constant sound speed
static const double bot_depth = 1e5 ;

/**
 * Tests the model's ability to accurately estimate geometric terms for
 * Lloyd's Mirror eigenrays on a spherical earth.  Performing this test in
 * spherical coordinates eliminates potential sources of error for the
 * proploss_test.cc suite, which compares its results to Cartesian test cases.
 *
 * - Scenario parameters
 *   - Profile: constant 1500 m/s sound speed, no absorption
 *   - Bottom: "infinitely" deep
 *   - Source: 200 meters deep, 2000 Hz
 *   - Target: WOA5 depths from 1-500 meters, range is 5-45 nmi
 *   - Time Step: 100 msec
 *   - Launch D/E: 181 tangent spaced rays from -90 to 90 degrees
 *
 * This test computes travel times and eigenray angles for a combination
 * of direct and surface-reflected paths in an isovelocity ocean on a
 * round earth. It searches for zones of inaccuracies in the the wavefront
 * model by comparing the modeled results to analytic solutions at a
 * variety of depths and ranges.
 *
 * To compute the analytic solution we start with:
 *
 *  - R = earth's radius
 *  - \f$ c_0 \f$ = speed of sound in the ocean
 *  - \f$ d_1 \f$ = source depth
 *  - \f$ d_2 \f$ = target depth
 *  - \f$ \xi \f$ = latitude change from source to receiver
 *
 * The laws of sines and cosines are then used to compute an analytic
 * solution for all direct-path eigenray terms:
 * \f[
 *      L^2 = D_1^2 + D_2^2 - 2 D_1 D_2 cos( \xi )
 * \f]\f[
 *      \mu_{source} = arccos \left( \frac{L^2+D_1^2+D_2^2}{2 L D_1} \right) - 90
 * \f]\f[
 *      \mu_{target} = arccos \left( \frac{L^2+D_2^2+D_1^2}{2 L D_2} \right) - 90
 * \f]\f[
 *      \tau_{direct} = L / c_0
 * \f]
 * where:
 *  - \f$ L \f$ = length of direct-path (meters)
 *  - \f$ D_1 = R - d_1 \f$ = distance from earth center to source (meters)
 *  - \f$ D_2 = R - d_2 \f$ = distance from earth center to target (meters)
 *  - \f$ \mu_{source} \f$ = direct-path D/E angle at source (degrees)
 *  - \f$ \mu_{target} \f$ = direct-path D/E angle at target (degrees)
 *  - \f$ \tau_{direct} \f$ = direct-path travel time from source to target (sec)
 *
 * The surface-reflected path is very complicated in spherical coordinates.
 * One way to find it is to search for the roots to the transcendental equation:
 * \f[
 *      f( \xi_1 ) = D_1 sin( \xi_1 ) - D_2 sin( \xi - \xi_1 ) + \frac{D_1 D_2}{R} sin( \xi - 2 \xi_1 ) = 0
 * \f]
 * where
 *  - \f$ \xi_1 \f$ = latitude change from source to point of reflection
 *  - \f$ \xi_2 = \xi -\xi_1 \f$ = latitude change from reflection point to target
 *
 * This test uses the Newton-Raphson method to iterate over successive values of
 * \f$ \xi_1 \f$ until a solution \f$ f( \xi_1 ) \approx 0 \f$ is found.
 * \f[
 *      f'( \xi_1 ) = D_1 cos( \xi_1 ) + D_2 cos( \xi - \xi_1 ) - 2 \frac{D_1 D_2}{R} cos( \xi - 2 \xi_1 )
 * \f]\f[
 *      \xi_{1 new} = \xi_1 - \frac{ f( \xi_1 ) }{ f'( \xi_1 ) }
 * \f]
 *
 * Plots of the transcendental equation indicate that the solution for
 * \f$ \xi_1 \f$ can have up to three roots, at long ranges, for depths near
 * that of the source.  For the purposes of analytic solution computation,
 * we will limit the range to an area where only one root is supported.
 * For a source at 200 meters, that corresponds to ranges below
 * approximately 0.8 degrees.
 *
 * Once \f$ \xi_1 \f$ and \f$ \xi_2 \f$ are known, the laws of sines and
 * cosines are used to compute an analytic solution for all surface
 * reflected eigenray terms:
 * \f[
 *      a_1^2 = R^2 + D_1^2 - 2 R D_1 cos( \xi_1 )
 * \f]\f[
 *      a_2^2 = R^2 + D_2^2 - 2 R D_2 cos( \xi_2 )
 * \f]\f[
 *      \eta_{source} = arccos \left( \frac{a_1^2+D_1^2-R^2}{2 a_1 D_1} \right) - 90
 * \f]\f[
 *      \eta_{target} = arccos \left( \frac{a_2^2+D_2^2-R^2}{2 a_2 D_2} \right) - 90
 * \f]\f[
 *      \tau_{surface} = ( a_1 + a_2 ) / c_0
 * \f]
 * where:
 *  - \f$ a_1 \f$ = distance from source to point of reflection (meters)
 *  - \f$ a_2 \f$ = distance from point of reflection to target  (meters)
 *  - \f$ \eta_{source} \f$ = surface-reflected D/E angle at source (degrees)
 *  - \f$ \eta_{target} \f$ = surface-reflected D/E angle at target (degrees)
 *  - \f$ \tau_{surface} \f$ = surface-reflected travel time from source to target (sec)
 *
 * Errors are automatically generated if the modeled eigenrays
 * deviate from the analytic results by more than 0.5 millisecs in time or
 * 0.2 degrees in angle.
 *
 * When the wave_queue::compute_offsets() fallback calculation of
 * offset(n) = -gradient(n) / hessian(n,n) is not limited to 1/2 of the
 * beamwidth.  This test has large erorrs in D/E angle.  This illustrates
 * the importance of this limitation.
 *
 * @xref Weisstein, Eric W. "Newton's Method." From MathWorld--A Wolfram
 *       Web Resource. http://mathworld.wolfram.com/NewtonsMethod.html
 */
BOOST_AUTO_TEST_CASE( eigenray_lloyds ) {
    cout << "=== eigenray_test: eigenray_lloyds ===" << endl;
    const char* ncname_wave = USML_STUDIES_DIR "/eigenray_extra/eigenray_lloyds_wave.nc";
    const char* ncname1 = USML_STUDIES_DIR "/eigenray_extra/eigenray_lloyds1.nc";
    const char* ncname2 = USML_STUDIES_DIR "/eigenray_extra/eigenray_lloyds2.nc";
    const char* analytic_name = USML_STUDIES_DIR "/eigenray_extra/eigenray_lloyds_analytic.nc";

    const double src_alt = -200.0;      // source depth = 200 meters
    const double time_max = 120.0 ;     // let rays plots go into region w/ 2 roots

    const double rmax = 45.0/60.0 ;     // limit targets to area where N/R converges
    const double rmin = 1.0/60.0 ;      // 1 nmi min range
    const double rinc = 1.0/60.0 ;      // 1 nmi range inc
    const seq_linear range( rmin, rinc, rmax );   // range in latitude

    static double depth[] = { 0, 10, 100, 1000 } ;
    size_t num_depths = sizeof(depth) / sizeof(double) ;

    //*********************************************************************
    // compute eigenrays for this ocean

    wposition::compute_earth_radius( src_lat );			// init area of ops
    attenuation_model* attn = new attenuation_constant(0.0);    // no absorption
    profile_model* profile = new profile_linear(c0,attn);	// iso-velocity
    boundary_model* surface = new boundary_flat();		// default surface
    boundary_model* bottom = new boundary_flat(bot_depth);	// flat bottom
    ocean_model ocean( surface, bottom, profile );

    seq_log freq( f0, 1.0, 1 );
    wposition1 pos( src_lat, src_lng, src_alt );		// build ray source
    seq_rayfan de ;
//    seq_linear de(0.0, 1.0, 50.0) ;
    seq_linear az( -4.0, 1.0, 4.0 );

    // build a grid of targets at different ranges and depths

    wposition target( range.size(), num_depths, src_lat, src_lng, src_alt );
    for ( size_t t1=0; t1 < range.size(); ++t1 ) {
        for ( size_t t2=0; t2 < num_depths; ++t2 ) {
            target.latitude( t1, t2, src_lat + range(t1) );
            target.altitude( t1, t2, -depth[t2] );
        }
    }

    eigenray_collection loss1(freq, pos, de, az, time_step, &target);

    eigenray_collection loss2(freq, pos, de, az, time_step, &target);

	wave_queue wave( ocean, freq, pos, de, az, time_step, &target ) ;

	// adding eigenray_collection listener 1
	wave.add_eigenray_listener(&loss1);

    //adding eigenray_collection listener 2
	wave.add_eigenray_listener(&loss2);

    // propagate rays & record to log files

    cout << "propagate wavefronts" << endl;
    cout << "writing wavefronts to " << ncname_wave << endl;
    wave.init_netcdf(ncname_wave);  // open a log file for wavefront data
    wave.save_netcdf();             // write ray data to log file
    while (wave.time() < time_max) {
        wave.step();
        wave.save_netcdf();         // write ray data to log file
    }
    wave.close_netcdf();            // close log file for wavefront data

    loss1.sum_eigenrays();
    loss2.sum_eigenrays();

    cout << "writing eigenrays to " << ncname1 << endl;
    loss1.write_netcdf(ncname1);
    cout << "writing eigenrays to " << ncname2 << endl;
    loss2.write_netcdf(ncname2);

    //*********************************************************************
    // compare each target location to analytic results

    cout << "testing eigenrays" << endl;
    for ( size_t t1=0; t1 < range.size(); ++t1 ) {
        for ( size_t t2=0; t2 < num_depths; ++t2 ) {
            double time, sde, tde, phase ;

            // setup analytic equations for this target

            const double R = wposition::earth_radius;
            const double xi = to_radians( target.latitude(t1,t2) - src_lat ) ;
            const double d1 = - src_alt;
            const double d2 = - target.altitude(t1,t2) ;
            const double D1 = R - d1 ;
            const double D2 = R - d2 ;

            eigenray_list *raylist = loss1.eigenrays(t1,t2);
            for ( eigenray_list::iterator iter=raylist->begin();
                  iter != raylist->end(); ++iter )
            {
                eigenray ray = *iter ;

                //*************************************************************
                // compare direct-path model to analytic results

                if ( ray.surface == 0 || depth[t2] < 1e-3 ) {

                    // compute analytic results

                    const double L = sqrt( D1*D1 + D2*D2 - 2.0*D1*D2*cos(xi) );
                    time = L/c0 ;
                    sde = to_degrees( -asin( (L*L+D1*D1-D2*D2) / (2.0*L*D1) ) ) ;
                    tde = to_degrees( asin( (L*L+D2*D2-D1*D1) / (2.0*L*D2) ) ) ;
                    phase = 0.0 ;
                    if ( ray.surface == 1 ) {
                        tde *= -1.0 ;
                        phase = -M_PI ;
                    }

                //*************************************************************
                // compare surface-reflected model to analytic results

                } else {

                    // find reflection point using root of transindental equation
                    // warning: xi2 = 0 for depths < 1e-3, and this makes solution unstable

                    double xi1 = xi ;
                    double xi2 = xi - xi1 ;
                    if ( abs(d2) > 0.5 ) {
                        xi1 = xi / 2.0 ;
                        xi2 = xi - xi1 ;
                        double f,g,delta ;
                        do {
                            f = D1*sin(xi1) - D2*sin(xi2) + D1*D2/R*sin(xi2-xi1) ;
                            g = D1*cos(xi1) + D2*cos(xi2) - 2.0*D1*D2/R*cos(xi2-xi1) ;
                            delta = - f / g ;
                            xi1 += delta ;
                            xi2 = xi - xi1 ;
                        } while ( abs(delta) > 1e-6 ) ;
                    }

                    // compute analytic results

                    const double a1 = sqrt( R*R + D1*D1 - 2.0*R*D1*cos(xi1) );
                    const double a2 = sqrt( R*R + D2*D2 - 2.0*R*D2*cos(xi2) );
                    time = (a1+a2)/c0 ;
                    sde = to_degrees( -asin( (a1*a1+D1*D1-R*R) / (2.0*a1*D1) ) ) ;
                    tde = to_degrees( asin( (a2*a2+D2*D2-R*R) / (2.0*a2*D2) ) ) ;
                    phase = -M_PI ;
                }

                //*************************************************************
                // test the accuracy of the model
                // acknowledge that there will be bigger errors at short range

                if ( range(t1) >= 0.1 ) {
                    BOOST_CHECK_SMALL( ray.time - time, 0.0005 );
                    BOOST_CHECK_SMALL( ray.phase(0)-phase, 1e-6 );
                    BOOST_CHECK_SMALL( ray.source_de - sde, 0.3 );
                    BOOST_CHECK_SMALL( ray.source_az, 1e-6 );
                    BOOST_CHECK_SMALL( ray.target_de - tde, 0.3 );
                    BOOST_CHECK_SMALL( ray.target_az, 1e-6 );
                }

                //*************************************************************
                // replace modeled values with analytic results

                (*iter).time = time ;
                (*iter).source_de = sde ;
                (*iter).source_az = 0.0 ;
                (*iter).target_de = tde ;
                (*iter).target_az = 0.0 ;

//                cout << "lat=" << range[t1] << " depth=" << depth[t2] << " path=" << ray.surface ;
//                cout << " time=" << time << " sd=" << sde << " tde=" << tde << endl ;
            }   // loop through eigenrays for each target
        }   // loop through target depths
    }   // loop through target ranges

    loss1.write_netcdf(analytic_name);

}

/// @}

BOOST_AUTO_TEST_SUITE_END()


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include "_hypre_utilities.h"
#include "HYPRE_krylov.h"
#include "HYPRE.h"
#include "HYPRE_parcsr_ls.h"
#include "general_functions.hpp"
#include "class.hpp"

FRACTIONAL_STEP_1::FRACTIONAL_STEP_1(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, vector<bool> &u_dirichlet_flag1, vector<bool> &v_dirichlet_flag1, vector<bool> &p_dirichlet_flag1, int temporal_order1)
{
    temporal_order = temporal_order1;
    u_dirichlet_flag = u_dirichlet_flag1, v_dirichlet_flag = v_dirichlet_flag1, p_dirichlet_flag = p_dirichlet_flag1;
    check_bc(points, parameters);
    clock_t clock_t1 = clock(), clock_t2 = clock();
    solver_p.init(points, cloud, parameters, p_dirichlet_flag, 0.0, 0.0, 1.0, true);
    parameters.factoring_timer = ((double)(clock() - clock_t1)) / CLOCKS_PER_SEC;

    zero_vector = Eigen::VectorXd::Zero(points.nv);
    zero_vector_1 = Eigen::VectorXd::Zero(points.nv + 1);
    uh = zero_vector, vh = zero_vector;

    p_bc_full_neumann = true;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv] && p_dirichlet_flag[iv])
        { //boundary point found with dirichlet BC
            p_bc_full_neumann = false;
            break;
        }

    if (p_bc_full_neumann)
        p_source = zero_vector_1; //this is full Neumann for pressure
    else
        p_source = zero_vector;
    u_source_old = zero_vector, v_source_old = zero_vector;
    if (temporal_order == 2)
        u_source_old_old = zero_vector, v_source_old_old = zero_vector;
}

void FRACTIONAL_STEP_1::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, int it1)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    calc_vel_hat(points, parameters, u_old, v_old, p_old);
    calc_pressure(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    calc_vel_corr(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old);
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
}

void FRACTIONAL_STEP_1::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y, int it1)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    calc_vel_hat(points, parameters, u_old, v_old, p_old, body_force_x, body_force_y);
    calc_pressure(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    calc_vel_corr(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old);
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
}

void FRACTIONAL_STEP_1::check_bc(POINTS &points, PARAMETERS &parameters)
{
    int u_dirichlet_flag_sum = accumulate(u_dirichlet_flag.begin(), u_dirichlet_flag.end(), 0);
    if (u_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc Setting u_dirichlet_flag to full Neumann BC is not permitted; sum of u_dirichlet_flag: %i\n\n", u_dirichlet_flag_sum);
        throw bad_exception();
    }
    int v_dirichlet_flag_sum = accumulate(v_dirichlet_flag.begin(), v_dirichlet_flag.end(), 0);
    if (v_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc Setting v_dirichlet_flag to full Neumann BC is not permitted; sum of v_dirichlet_flag: %i\n\n", v_dirichlet_flag_sum);
        throw bad_exception();
    }
    if (parameters.rho < 0 || parameters.mu < 0)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc Some parameters are not set; parameters.rho: %g, parameters.mu: %g\n\n", parameters.rho, parameters.mu);
        throw bad_exception();
    }
    if (temporal_order != 1 && temporal_order != 2)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc temporal_order should be either '1' or '2'; current value: %i\n\n", temporal_order);
        throw bad_exception();
    }
}

void FRACTIONAL_STEP_1::calc_vel_hat(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y)
{
    u_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * u_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * u_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * u_old) + body_force_x;
    v_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * v_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * v_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * v_old) + body_force_y;
    if (temporal_order == 1 || it == 0)
    { //Euler method for first timestep of multistep method
        uh = u_old + ((parameters.dt / parameters.rho) * u_source_old);
        vh = v_old + ((parameters.dt / parameters.rho) * v_source_old);
    }
    else
    { //Second order Adam-Bashforth for subsequent timesteps
        uh = u_old + ((parameters.dt / parameters.rho) * (1.5 * u_source_old - 0.5 * u_source_old_old));
        vh = v_old + ((parameters.dt / parameters.rho) * (1.5 * v_source_old - 0.5 * v_source_old_old));
    }
}

void FRACTIONAL_STEP_1::calc_vel_hat(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    u_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * u_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * u_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * u_old);
    v_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * v_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * v_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * v_old);
    if (temporal_order == 1 || it == 0)
    { //Euler method for first timestep of multistep method
        uh = u_old + ((parameters.dt / parameters.rho) * u_source_old);
        vh = v_old + ((parameters.dt / parameters.rho) * v_source_old);
    }
    else
    { //Second order Adam-Bashforth
        uh = u_old + ((parameters.dt / parameters.rho) * (1.5 * u_source_old - 0.5 * u_source_old_old));
        vh = v_old + ((parameters.dt / parameters.rho) * (1.5 * v_source_old - 0.5 * v_source_old_old));
    }
}

void FRACTIONAL_STEP_1::calc_pressure(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    int dim = parameters.dimension;
    p_source.head(points.nv) = ((points.grad_x_matrix_EIGEN_internal * uh) + (points.grad_y_matrix_EIGEN_internal * vh)) * parameters.rho / parameters.dt;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        {
            if (p_dirichlet_flag[iv]) //dirichlet BC
                p_source[iv] = p_old[iv];
            else //normal momentum
                p_source[iv] = -parameters.rho * ((u_old[iv] - uh[iv]) * points.normal[dim * iv] + (v_old[iv] - vh[iv]) * points.normal[dim * iv + 1]) / parameters.dt;
        }
    if (p_source.rows() == points.nv + 1)
        p_source[points.nv] = 0.0;
    solver_p.general_solve(points, parameters, p_new, p_old, p_source);
}

void FRACTIONAL_STEP_1::calc_vel_corr(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old)
{
    u_new = uh - (parameters.dt * (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv)) / parameters.rho);
    v_new = vh - (parameters.dt * (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv)) / parameters.rho);
    double diag_coeff, rhs, off_diag_coeff;
    int ivnb, dim = parameters.dimension;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        {
            if (!p_dirichlet_flag[iv])
            { //for dirichlet pressure, u_new comes from velocity correction above
                if (u_dirichlet_flag[iv])
                    u_new[iv] = u_old[iv]; //dirichlet BC
                else
                { //neumann BC
                    rhs = 0.0;
                    for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                    {
                        ivnb = cloud.nb_points_col[i1];
                        if (iv == ivnb)
                            diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                        else
                        {
                            off_diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                            rhs = rhs - (off_diag_coeff * u_new[ivnb]);
                        }
                    }
                    u_new[iv] = (rhs / diag_coeff);
                }
            }
        }
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        {
            if (!p_dirichlet_flag[iv])
            { //for dirichlet pressure, v_new comes from velocity correction above
                if (v_dirichlet_flag[iv])
                    v_new[iv] = v_old[iv]; //dirichlet BC
                else
                { //neumann BC
                    rhs = 0.0;
                    for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                    {
                        ivnb = cloud.nb_points_col[i1];
                        if (iv == ivnb)
                            diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                        else
                        {
                            off_diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                            rhs = rhs - (off_diag_coeff * v_new[ivnb]);
                        }
                    }
                    v_new[iv] = (rhs / diag_coeff);
                }
            }
        }
}

void FRACTIONAL_STEP_1::extras(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    // double total_steady_err, max_err, l1_err;
    // calc_max_l1_error(u_old, u_new, max_err, l1_err);
    // total_steady_err = l1_err / parameters.dt;
    // calc_max_l1_error(v_old, v_new, max_err, l1_err);
    // total_steady_err += l1_err / parameters.dt;
    if (temporal_order == 2)
        u_source_old_old = u_source_old, v_source_old_old = v_source_old;
    // parameters.steady_error_log.push_back(total_steady_err);
    // return total_steady_err;
}


#define BOOST_TEST_MODULE TestFFTW

#include <fftw3.h>
#include <boost/test/unit_test.hpp>
#include <iostream>

BOOST_AUTO_TEST_CASE( FFT1DSingle, * boost::unit_test::tolerance(0.0001f) )
{
  static const int N = 32;
  fftwf_complex in[N], out[N], in2[N];
  fftwf_plan fwd, bwd;
  int i;
  for (i = 0; i < N; i++) {
    in[i][0] = static_cast<float>(i%7)/6;
    in[i][1] = 0;
  }

  fwd = fftwf_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  fftwf_execute(fwd);
  fftwf_destroy_plan(fwd);

  bwd = fftwf_plan_dft_1d(N, out, in2, FFTW_BACKWARD, FFTW_ESTIMATE);
  fftwf_execute(bwd);

  for (i = 0; i < N; i++) {
    in2[i][0] *= 1.f/N;
    in2[i][1] *= 1.f/N;

    BOOST_TEST( in[i][0] == in2[i][0] );
    BOOST_TEST( in[i][1] == in2[i][1] );
  }

  fftwf_destroy_plan(bwd);
  fftwf_cleanup();
}

BOOST_AUTO_TEST_CASE( FFT1D, * boost::unit_test::tolerance(0.0001) )
{
  static const int N = 32;
  fftw_complex in[N], out[N], in2[N];
  fftw_plan fwd, bwd;
  int i;
  for (i = 0; i < N; i++) {
    in[i][0] = static_cast<double>(i%7)/6;
    in[i][1] = 0;
  }

  fwd = fftw_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(fwd);
  fftw_destroy_plan(fwd);

  bwd = fftw_plan_dft_1d(N, out, in2, FFTW_BACKWARD, FFTW_ESTIMATE);
  fftw_execute(bwd);

  for (i = 0; i < N; i++) {
    in2[i][0] *= 1./N;
    in2[i][1] *= 1./N;

    BOOST_TEST( in[i][0] == in2[i][0] );
    BOOST_TEST( in[i][1] == in2[i][1] );
  }

  fftw_destroy_plan(bwd);
  fftw_cleanup();
}

#if defined(GEARSHIFFT_FFTW_USE_THREADS) && GEARSHIFFT_FFTW_USE_THREADS==1
BOOST_AUTO_TEST_CASE( FFT1D2Threads, * boost::unit_test::tolerance(0.0001) )
{
  static const int N = 32;
  fftw_complex in[N], out[N], in2[N];
  fftw_plan fwd, bwd;
  int i;
  for (i = 0; i < N; i++) {
    in[i][0] = static_cast<double>(i%7)/6;
    in[i][1] = 0;
  }

  int res = fftw_init_threads();
  if(res==0)
    BOOST_ERROR("fftw thread initialization failed.");
  fftw_plan_with_nthreads(2);

  fwd = fftw_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
  if(!fwd)
    BOOST_ERROR("forward plan could not be created.");
  fftw_execute(fwd);
  fftw_destroy_plan(fwd);

  bwd = fftw_plan_dft_1d(N, out, in2, FFTW_BACKWARD, FFTW_ESTIMATE);
  if(!bwd)
    BOOST_ERROR("backward plan could not be created.");
  fftw_execute(bwd);

  for (i = 0; i < N; i++) {
    in2[i][0] *= 1./N;
    in2[i][1] *= 1./N;

    BOOST_TEST( in[i][0] == in2[i][0] );
    BOOST_TEST( in[i][1] == in2[i][1] );
  }

  fftw_destroy_plan(bwd);
  fftw_cleanup_threads();
  fftw_cleanup();
}
#endif


#include <iostream>
#include <fstream>
#include <cmath>
#include <vector>
#include <math.h>
#include "SpaceAdaptiveSolver.hpp"
#include "GeneralizedHeat.hpp"
#include "APDE.hpp"
#include "PDE_Q2.hpp"
#include "originalPDE.hpp"
#include "EllipticPDE_Q1.hpp"
#include "EllipticPDE2.hpp"
#include "Nonanalytic_1.hpp"
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void printTime(SpaceMesh a_smesh, TimeMesh a_tmesh);

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

SpaceMesh smesh;
std::vector<double> mesh = {0, 0.15, 0.25, 0.5, 1};
//smesh.GenerateSpaceMesh(mesh);
smesh.GenerateUniformMesh(1, 5);
//smesh.InsertArray(mesh);
//smesh.PrintSpaceNodes();

TimeMesh tmesh;
tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize(), 2), 1.0);

PDE_Q2 anotherpde;
originalPDE firstpde;
EllipticPDE_Q1 Q1;
EllipticPDE2 elliptic2;
Nonanalytic NA_1;

AdaptiveSolver adapt;

adapt.SetSpaceTimeMesh(smesh, tmesh, firstpde);

adapt.SetSpaceTimeMesh(smesh, tmesh, anotherpde);
adapt.SetTolerances(0.2,0.05);
adapt.AdaptiveSolve();
adapt.PrintSolution();

adapt.SetSpaceTimeMesh(smesh, tmesh, anotherpde);
adapt.SolveWithBCs();
adapt.PrintSolution();
}

void printTime(SpaceMesh a_smesh, TimeMesh a_tmesh)
{
    ofstream myfile2;
    myfile2.open ("Y.csv");
    for(int j=1;j<a_tmesh.NumberOfTimeSteps()+1;j++ )
    {
    for(int i = 0; i<a_smesh.meshsize()+1; i++)
    {
        myfile2 << a_tmesh.ReadTimeStep(j) << ", ";
    }
    myfile2 << "\n";
    }
    myfile2.close();
}

Solver class with methods to adapt in space/Driver.cpp


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE dirMM test
#include <boost/test/unit_test.hpp>

#include <dpMM/dirMM.hpp>
#include <dpMM/niwBaseMeasure.hpp>
#include <dpMM/niwSphere.hpp>
#include <dpMM/dirMMcld.hpp>
#include <dpMM/clTGMMDataGpu.hpp>
#include <dpMM/distribution.hpp>
#include <dpMM/vmfBaseMeasure.hpp>
#include <dpMM/vmfBaseMeasure3D.hpp>

BOOST_AUTO_TEST_CASE(niwBaseMeasure_test)
{
  MatrixXd Delta(3,3);
  Delta << 1.0,0.0,0.0,
        0.0,1.0,0.0,
        0.0,0.0,1.0;
  VectorXd theta(3);
  theta << 1.0,1.0,1.0;
  double nu = 100.0;
  double kappa = 100.0;

  boost::mt19937 rndGen(1);
  NIW<double> niw(Delta,theta,nu,kappa,&rndGen);

  NiwMarginalized<double> niwMargBase(niw);

  VectorXd x(3);
  x << 1.0,1.0,1.0;
  cout<< niwMargBase.logLikelihood(x)<< endl;

  NiwSampled<double> niwSampledBase(niw);

  x << 1.0,1.0,1.0;
  cout<< niwSampledBase.logLikelihood(x)<< endl;
};

BOOST_AUTO_TEST_CASE(dirMM_test)
{

  double nu = 4.0;
  double kappa = 4.0;
  MatrixXd Delta(3,3);
  Delta << .1,0.0,0.0,
        0.0,.1,0.0,
        0.0,0.0,.1;
  VectorXd theta(3);
  theta << 0.0,0.0,0.0;

  boost::mt19937 rndGen(9191);
  NIW<double> niw(Delta,theta,nu,kappa,&rndGen);

  shared_ptr<NiwMarginalized<double> > niwMargBase(
      new NiwMarginalized<double>(niw));
  VectorXd alpha(2);
  alpha << 10.,10.;
  Dir<Cat<double>, double> dir(alpha,&rndGen);
  DirMM<double> dirGMM_marg(dir,niwMargBase,2);

  uint32_t N=20;
  MatrixXd x(3,N);
  for(uint32_t i=0; i<N; ++i)
    if(i<N/2)
      x.col(i) << 0.0,0.0,0.0;
    else
      x.col(i) << 10.0,10.0,10.0;
  dirGMM_marg.initialize(x);
  cout<<"------ sampling -- NIW marginalized"<<endl;
  cout<<dirGMM_marg.labels().transpose()<<endl;
  for(uint32_t t=0; t<30; ++t)
  {
    dirGMM_marg.sampleLabels();
    dirGMM_marg.sampleParameters();
    cout<<dirGMM_marg.labels().transpose()
      <<" logJoint="<<dirGMM_marg.logJoint()<<endl;
  }

  shared_ptr<NiwSampled<double> > niwSampled(
      new NiwSampled<double>(niw));
  DirMM<double> dirGMM_samp(dir,niwSampled,2);

  dirGMM_samp.initialize(x);
  cout<<"------ sampling -- NIW sampled"<<endl;
  cout<<dirGMM_samp.labels().transpose()<<endl;
  for(uint32_t t=0; t<30; ++t)
  {
    dirGMM_samp.sampleLabels();
    dirGMM_samp.sampleParameters();
    cout<<dirGMM_samp.labels().transpose()
      <<" logJoint="<<dirGMM_samp.logJoint()<<endl;
  }
};

BOOST_AUTO_TEST_CASE(dirMM_Sphere_test)
{
  cout<<"------ sampling -- NIW sphere"<<endl;

  double nu = 20.0;
  MatrixXd Delta(2,2);
  Delta << .01,0.0,
        0.0,.01;
  Delta *= nu;

  boost::mt19937 rndGen(9191);
  IW<double> iw(Delta,nu,&rndGen);
  shared_ptr<NiwSphere<double> > niwSp( new NiwSphere<double>(iw,&rndGen));

  VectorXd alpha(2);
  alpha << 10.,10.;
  Dir<Cat<double>, double> dir(alpha,&rndGen);
  DirMM<double> dirGMM_sp(dir,niwSp,2);

  uint32_t N=20;
  uint32_t K=2;
  MatrixXd x(3,N);
  MatrixXd mus = sampleClustersOnSphere<double>(x, K);

  dirGMM_sp.initialize(x);

  cout<<"true means: "<<endl<<mus<<endl;
  cout<<dirGMM_sp.labels().transpose()<<endl;
  for(uint32_t t=0; t<10; ++t)
  {
    dirGMM_sp.sampleParameters();
//    for(uint32_t k=0; k<dirGMM_sp.getK(); ++k)
//    {
//      cout<<"  k: "<<k<<" "<<endl;
//      dirGMM_sp.getTheta(k)->print();
//    }
    dirGMM_sp.sampleLabels();
    cout<<"@t="<<t<<" "<<dirGMM_sp.labels().transpose()
      <<" logJoint="<<dirGMM_sp.logJoint()<<endl;
  }
  MatrixXd logLikes;
  MatrixXu inds = dirGMM_sp.mostLikelyInds(5,logLikes);
  cout<<"most likely indices"<<endl;
  cout<<inds<<endl;
  cout<<"----------------------------------------"<<endl;
};

typedef double myFlt;

BOOST_AUTO_TEST_CASE(dirMMcld_Sphere_test)
{
  cout<<"------ sampling -- NIW sphere using CLD"<<endl;

  uint32_t N=30; //640*480;
  uint32_t K=6;
  uint32_t D=3;
  boost::mt19937 rndGen(9191);
  // sample datapoints
  shared_ptr<Matrix<myFlt,Dynamic,Dynamic> > sx(new
      Matrix<myFlt,Dynamic,Dynamic>(D,N));
  Matrix<myFlt,Dynamic,Dynamic> mus =  sampleClustersOnSphere(*sx, 3);

  // alpha
  Matrix<myFlt,Dynamic,1> alpha(K);
  alpha << 1.,1.,.1,.1,.1,.1;
  alpha *= 1;

  // niw
  double nu = (1.0)+D+N/100.;
  Matrix<myFlt,Dynamic,Dynamic> Delta(2,2);
  Delta << .01,0.0,
        0.0,.01;
  Delta *= nu;

  IW<myFlt> iw(Delta,nu,&rndGen);
  shared_ptr<NiwSphere<myFlt> > niwSp( new NiwSphere<myFlt>(iw,&rndGen));
//  shared_ptr<NiwSphere<double> > niwSp2( new NiwSphere<double>(iw,&rndGen));

  Dir<Cat<myFlt>, myFlt> dir(alpha,&rndGen);
  DirMMcld<NiwSphere<myFlt>,myFlt> dirGMM_sp(dir,niwSp);

//  DirMM<myFlt> dirGMM_cpu(dir,niwSp2);

  shared_ptr<ClTGMMDataGpu<myFlt> > clsp(
      new ClTGMMDataGpu<myFlt>(sx, spVectorXu(new VectorXu(N)),K));

  MatrixXd ps(D,K);
  Sphere<double> sphere(D);
  for(uint32_t k=0; k<K; ++k)
    ps.col(k) = sphere.sampleUnif(&rndGen);
  clsp->init(ps);

  Matrix<myFlt,Dynamic,1> mu(D);
  mu<<0.0,0.0,1.0;
  Matrix<myFlt,Dynamic,Dynamic> Sigma = Matrix<myFlt,Dynamic,Dynamic>::Identity(D,D);

  dirGMM_sp.initialize(clsp);
//  dirGMM_cpu.initialize(*sx);
  cout<<counts<myFlt,uint32_t>(dirGMM_sp.labels(),K).transpose()<<endl;
  Timer t;
  for(uint32_t i=0; i<5; ++i)
  {
//    t.tic();
//    dirGMM_cpu.sampleLabels();
//    dirGMM_cpu.sampleParameters();
//    cout<<dirGMM_cpu.labels().transpose()<<endl;
//    t.toctic(" -----------------CPU------------------- fullIteration");
    t.tic();
    dirGMM_sp.sampleParameters();
    dirGMM_sp.sampleLabels();
    cout<<dirGMM_sp.z().transpose()<<endl;
    cout<<dirGMM_sp.counts().transpose()<<endl;
    cout<<dirGMM_sp.means()<<endl;
    t.toctic(" -----------------GPU------------------- fullIteration");
//        <<" logJoint="<<dirGMM_sp.logJoint()<<endl;
  }
  cout<<"true mus: "<<endl;
  cout <<mus<<endl;
};

BOOST_AUTO_TEST_CASE(dirMM_vMFsampled_test)
{
  cout<<"------ sampling -- Dir-vMF (old, monte carlo estimation for marginalization)"<<endl;

  double a0 = 5.0;
  double b0 = 4.7;
  double t0 = 0.1;
  VectorXd m0(3);
  m0 << 1.0,0.0,0.0;

  boost::mt19937 rndGen(9191);

  vMFpriorFull<double> vMFprior(m0,t0,a0,b0,&rndGen);
  shared_ptr<vMFbase<double> > vMFsampled( new vMFbase<double>(vMFprior));

  VectorXd alpha(2);
  alpha << 10.,10.;
  Dir<Cat<double>, double> dir(alpha,&rndGen);
  DirMM<double> dirvMF_sp(dir,vMFsampled,2);

  uint32_t N=20;
  uint32_t K=2;
  MatrixXd x(3,N);
  MatrixXd mus = sampleClustersOnSphere<double>(x, K);

  dirvMF_sp.initialize(x);

  cout<<"true means: "<<endl<<mus<<endl;
  cout<<dirvMF_sp.labels().transpose()<<endl;
  for(uint32_t t=0; t<10; ++t)
  {
    dirvMF_sp.sampleParameters();
//    for(uint32_t k=0; k<dirvMF_sp.getK(); ++k)
//    {
//      cout<<"  k: "<<k<<" "<<endl;
//      dirvMF_sp.getTheta(k)->print();
//    }
    dirvMF_sp.sampleLabels();
    cout<<"@t="<<t<<" "<<dirvMF_sp.labels().transpose()
      <<" logJoint="<<dirvMF_sp.logJoint()<<endl;
  }
//  MatrixXd logLikes;
//  MatrixXu inds = dirvMF_sp.mostLikelyInds(5,logLikes);
//  cout<<"most likely indices"<<endl;
//  cout<<inds<<endl;
  cout<<"true means: "<<endl<<mus<<endl;
    for(uint32_t k=0; k<dirvMF_sp.getK(); ++k)
    {
      cout<<"  k: "<<k<<endl;
      dirvMF_sp.getTheta(k)->print();
    }
  cout<<"----------------------------------------"<<endl;
};

BOOST_AUTO_TEST_CASE(dirMM_vMF_test)
{
  cout<<"------ sampling -- Dir-vMF (new, analytic marginalization for 3D)"<<endl;

  double a0 = 1.0;
  double b0 = 0.99;
  VectorXd m0(3);
  m0 << 1.0,0.0,0.0;

  boost::mt19937 rndGen(9191);

  vMFprior<double> vMFprior(m0,a0,b0,&rndGen);
  shared_ptr<vMFbase3D<double> > vMFbase( new vMFbase3D<double>(vMFprior));

//  VectorXd alpha(1);
//  alpha << 10.;
//  Dir<Cat<double>, double> dir(alpha,&rndGen);
//  DirMM<double> dirvMF_sp(dir,vMFbase,1);

  VectorXd alpha(2);
  alpha << 10.,10.;
  Dir<Cat<double>, double> dir(alpha,&rndGen);
  DirMM<double> dirvMF_sp(dir,vMFbase,2);

  uint32_t N=200;
  uint32_t K=2;
  MatrixXd x(3,N);
  MatrixXd mus = sampleClustersOnSphere<double>(x, K);

  dirvMF_sp.initialize(x);

  cout<<"true means: "<<endl<<mus<<endl;
  cout<<dirvMF_sp.labels().transpose()<<endl;
  for(uint32_t t=0; t<100; ++t)
  {
    dirvMF_sp.sampleParameters();
//    for(uint32_t k=0; k<dirvMF_sp.getK(); ++k)
//    {
//      cout<<"  k: "<<k<<" "<<endl;
//      dirvMF_sp.getTheta(k)->print();
//    }
    dirvMF_sp.sampleLabels();
    cout<<"@t="<<t<<" "<<dirvMF_sp.labels().transpose()
      <<" logJoint="<<dirvMF_sp.logJoint()<<endl;
  }
//  MatrixXd logLikes;
//  MatrixXu inds = dirvMF_sp.mostLikelyInds(5,logLikes);
//  cout<<"most likely indices"<<endl;
//  cout<<inds<<endl;
  cout<<"true means: "<<endl<<mus<<endl;
    for(uint32_t k=0; k<dirvMF_sp.getK(); ++k)
    {
      cout<<"  k: "<<k<<endl;
      dirvMF_sp.getTheta(k)->print();
    }
  cout<<"----------------------------------------"<<endl;

//  vMF<double> vmf(m0, 100, &rndGen);
//  std::cout << m0.transpose() << std::endl << " -- " << std::endl;
//  for (size_t i=0; i<100; ++i) {
//    Eigen::VectorXd x = vmf.sample();
//    std::cout << x.transpose()  << " || " << x.norm() << std::endl;
//  }
//
//  std::cout << " -- " << std::endl;
//  for (size_t i=0; i<100; ++i) {
//    vmf = vMFprior.sample();
//    vmf.print();
////    std::cout << vmf.mu_.transpose()  << " tau " << vmf.tau_ << std::endl;
//  }

};


#include <memory>
#include <cassert>
#include <limits>
#include <algorithm>
#include <vector>
#include <iostream>

#include <boost/iterator/iterator_facade.hpp>

#include <cs.h>

struct CSparseShim {
    using index_t = CS_INT;     // for options, refer to CS_LONG and CS_COMPLEX in cs.h
    using value_t = CS_ENTRY;

    struct triplet_t {
        index_t row;
        index_t col;
        value_t value;
    };

    // RAII memory management for C data
    struct cs_deleter {
        void operator()(cs *p) {
            cs_spfree(p);
        }
        void operator()(csn *p) {
            cs_nfree(p);
        }
        void operator()(css *p) {
            cs_sfree(p);
        }
        template<typename T>
        void operator()(T *p) {
            cs_free(p);
        }
    };

    template<typename T> using cs_unique_ptr = std::unique_ptr<T, cs_deleter>;
    template<typename T> using cs_shared_ptr = std::shared_ptr<T>;
    template<typename T>
    static cs_shared_ptr<T>
    make_cs_shared_ptr(T* p) { return cs_shared_ptr<T>(p, cs_deleter()); }

    // create a forward iterator for nonzeros within a CSparse matrix
    struct sparse_entry_iterator
        : boost::iterator_facade<sparse_entry_iterator,
                                 triplet_t const,
                                 boost::forward_traversal_tag> {
        sparse_entry_iterator() {}
        sparse_entry_iterator( cs_shared_ptr<cs> mat )
            : mat_(std::move(mat)), entry_{index_t(0), index_t(0), value_t(0)} {
            advance_to_valid();
        }

    private:

        // iterator_facade requirements

        friend class boost::iterator_core_access;

        void increment();

        bool equal( sparse_entry_iterator const& other ) const;

        triplet_t const & dereference() const {
            return entry_;
        }

        // find the next nonzero entry and set the dereference value
        void advance_to_valid();

        cs_shared_ptr<cs> mat_;
        triplet_t entry_;     // for supplying references
    };

    struct sparsemat_t;

    static sparsemat_t
    dense_to_sparse( std::vector<value_t> const& d, index_t rows, index_t cols );

    struct sparsemat_t {
        template<typename Iter>
        sparsemat_t(index_t rows, index_t cols, Iter start, Iter end) {
            // create a triplet matrix
            auto TG = cs_unique_ptr<cs>(cs_spalloc(rows, cols, std::distance(start, end), 1, 1));
            for ( auto it = start; it < end; ++it ) {
                assert(cs_entry(TG.get(), it->row, it->col, it->value));
            }

            // create a "cs" structure from the triplet matrix
            mat_ = make_cs_shared_ptr(cs_compress(TG.get()));
        }

        sparsemat_t( cs_shared_ptr<cs> mat_cs ) : mat_(std::move(mat_cs)) {}

        index_t rows() const { return wrapped()->m; }
        index_t cols() const { return wrapped()->n; }

        sparse_entry_iterator nonzero_begin() const {
            return sparse_entry_iterator(mat_);
        }
        sparse_entry_iterator nonzero_end() const {
            return sparse_entry_iterator();
        }

        friend sparsemat_t operator*(sparsemat_t const& a, sparsemat_t const& b);
        friend std::ostream& operator<<(std::ostream& os, sparsemat_t const & m);

        cs_shared_ptr<const cs> wrapped() const {
            return mat_;
        }

    private:
        cs_shared_ptr<cs> mat_;      // we have to share this structure with LU and QR objects
    };

    struct lu_t {
        lu_t( sparsemat_t const & mat )
            : symbolic_( cs_sqr( 3, mat.wrapped().get(), 0 )),
              numeric_ ( cs_lu ( mat.wrapped().get(), symbolic_.get(),
                                 std::numeric_limits<value_t>::epsilon() )) {}

        sparsemat_t solve(sparsemat_t const& rhs) const;

    private:
        cs_unique_ptr<css> symbolic_;
        cs_unique_ptr<csn> numeric_;

    };

    struct qr_t {
        qr_t( sparsemat_t const & mat )
            : symbolic_( cs_sqr( 3, mat.wrapped().get(), 1 ) ),
              numeric_ ( cs_qr ( mat.wrapped().get(), symbolic_.get() ) ),
              rows_    ( mat.wrapped()->m ),
              cols_    ( mat.wrapped()->n ) {}

        sparsemat_t Q() const;

    private:

        cs_unique_ptr<css> symbolic_;
        cs_unique_ptr<csn> numeric_;

        index_t rows_, cols_;

    };

};


// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <dlib/statistics.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.sammon");

    std::vector<matrix<double,4,1> > make_test_data4(
    )
    {
        std::vector<matrix<double,4,1> > data;

        matrix<double,4,1> m;

        m = 0,0,0, 0; data.push_back(m);
        m = 1,0,0, 0; data.push_back(m);
        m = 0,1,0, 0; data.push_back(m);
        m = 0,0,1, 0; data.push_back(m);

        return data;
    }

    std::vector<matrix<double,3,1> > make_test_data3(
    )
    {
        std::vector<matrix<double,3,1> > data;

        matrix<double,3,1> m;

        m = 0,0,0; data.push_back(m);
        m = 1,0,0; data.push_back(m);
        m = 0,1,0; data.push_back(m);
        m = 0,0,1; data.push_back(m);

        return data;
    }

    std::vector<matrix<double> > make_test_data3d(
    )
    {
        std::vector<matrix<double> > data;

        matrix<double,3,1> m;

        m = 0,0,0; data.push_back(m);
        m = 1,0,0; data.push_back(m);
        m = 0,1,0; data.push_back(m);
        m = 0,0,1; data.push_back(m);

        return data;
    }

    void runtest()
    {
        sammon_projection s;
        std::vector<matrix<double, 0, 1> >  projs = s(make_test_data3(),2);
        running_stats<double> rs1, rs2;

        rs1.add(length(projs[0] - projs[1]));
        rs1.add(length(projs[0] - projs[2]));
        rs1.add(length(projs[0] - projs[3]));

        rs2.add(length(projs[1] - projs[2]));
        rs2.add(length(projs[2] - projs[3]));
        rs2.add(length(projs[3] - projs[1]));

        DLIB_TEST(rs1.stddev()/rs1.mean() < 1e-4);
        DLIB_TEST(rs2.stddev()/rs2.mean() < 1e-4);

        projs = s(make_test_data4(),2);
        rs1.clear();
        rs2.clear();

        rs1.add(length(projs[0] - projs[1]));
        rs1.add(length(projs[0] - projs[2]));
        rs1.add(length(projs[0] - projs[3]));

        rs2.add(length(projs[1] - projs[2]));
        rs2.add(length(projs[2] - projs[3]));
        rs2.add(length(projs[3] - projs[1]));

        DLIB_TEST(rs1.stddev()/rs1.mean() < 1e-4);
        DLIB_TEST(rs2.stddev()/rs2.mean() < 1e-4);

        projs = s(make_test_data3d(),2);
        rs1.clear();
        rs2.clear();

        rs1.add(length(projs[0] - projs[1]));
        rs1.add(length(projs[0] - projs[2]));
        rs1.add(length(projs[0] - projs[3]));

        rs2.add(length(projs[1] - projs[2]));
        rs2.add(length(projs[2] - projs[3]));
        rs2.add(length(projs[3] - projs[1]));

        DLIB_TEST(rs1.stddev()/rs1.mean() < 1e-4);
        DLIB_TEST(rs2.stddev()/rs2.mean() < 1e-4);
    }

    void runtest2()
    {
        sammon_projection s;
        std::vector<matrix<double, 0, 1> >  projs, temp;

        DLIB_TEST(s(projs,3).size() == 0);

        matrix<double,2,1> m;
        m = 1,2;
        projs.push_back(m);
        temp = s(projs,2);
        DLIB_TEST(temp.size() == 1);
        DLIB_TEST(temp[0].size() == 2);

        projs.push_back(m);
        temp = s(projs,1);
        DLIB_TEST(temp.size() == 2);
        DLIB_TEST(temp[0].size() == 1);
        DLIB_TEST(temp[1].size() == 1);
    }

    void runtest3(int num_dims)
    {
        sammon_projection s;
        std::vector<matrix<double, 0, 1> >  projs;
        matrix<double,3,1> m;
        m = 1, 1, 1;
        projs.push_back(m);

        m = 1, 2, 1;
        projs.push_back(m);

        m = 1, 3, 1;
        projs.push_back(m);

        projs = s(projs,num_dims);

        const double d1a = length(projs[0] - projs[1]);
        const double d1b = length(projs[1] - projs[2]);
        const double d2  = length(projs[0] - projs[2]);

        DLIB_TEST(std::abs(d1a-d1b)/d1a < 1e-8);
        DLIB_TEST(std::abs(d2/d1a-2) < 1e-8);
    }

    void runtest4(int num_dims)
    {
        sammon_projection s;
        std::vector<matrix<double, 0, 1> >  projs;
        matrix<double,3,1> m;
        m = 1, 1, 1;
        projs.push_back(m);

        m = 1, 2, 1;
        projs.push_back(m);

        projs = s(projs,num_dims);

        DLIB_TEST(length(projs[0] - projs[1]) > 1e-5);
    }

    class sammon_tester : public tester
    {
    public:
        sammon_tester (
        ) :
            tester ("test_sammon",
                    "Runs tests on the sammon_projection component.")
        {}

        void perform_test (
        )
        {
            print_spinner();
            runtest();
            print_spinner();
            runtest2();
            print_spinner();
            runtest3(2);
            print_spinner();
            runtest4(2);
            runtest3(1);
            print_spinner();
            runtest4(1);
        }
    } a;

}


#include <opencv2/opencv.hpp>
#include <iostream>
#include <boost/filesystem.hpp>
#include "derivatives.h"
#include "labelconnect.hpp"
#include "region.h"
#include "link.h"
#include "utils.hpp"
#include <iostream>
#include <iomanip>
#include "strands.h"
#include <cmath>
#include "version.h"
#include <exception>
#include <time.h>

#include <chrono>

string NowToString()
{
	chrono::system_clock::time_point p = chrono::system_clock::now();
	time_t t = chrono::system_clock::to_time_t(p);
//	char str[26];
//	ctime_s(str, sizeof str, &t);
	return ctime(&t);
}

using namespace cv;
using namespace std;
using namespace boost;
namespace fs = boost::filesystem;
using namespace stegers;
using namespace timer;

void strands::run(){

	assert(labelConnect::test());
	assert(region::test());

	auto polarity = strands::line_polarity::light;
	float sigma = 1.5;
	float low_thr = 1.0;
	float high_thr = 6.0;

	std::string version_string(VERSION);
	version_string = NowToString() + "build " + version_string + "\nimage file: " + m_input_file.stem().string();
	auto filename = m_input_file.stem();
	filename = "histogram_output_" + filename.string() + ".csv";
	filename = m_output_dir / filename;

	Mat image = imread(m_input_file.string(), cv::IMREAD_GRAYSCALE);

	Mat display;
	string screen("display");
	if (show() || graphics()) {
		std::vector<Mat> triple{ image, image, image };
		merge(triple, display);
	}
	// Show our image inside a window.
	if (show())
		namedWindow(screen.c_str(), cv::WINDOW_GUI_EXPANDED);

	std::map<int32_t, int32_t> lm;

	try {

		// Convert to flat vector of doubles
		// @todo have cv::Mat api
		cv::Mat dst;
		image.convertTo(dst, CV_64F);
		std::vector<double> dimg(size_t(image.rows * image.cols));
		assert(dst.isContinuous());
		std::memcpy(dimg.data(), dst.ptr(0), image.total() * sizeof(double));
		convol conv(image.cols, image.rows);
		conv.debug(debug());

		std::vector<std::vector<float>> out;
		conv.get_all_derivatives(dimg, sigma, out);
		std::vector<int32_t> ismax;
		std::vector<std::vector<double>> line_out;
		class link lnk(image.cols, image.rows, 1.5, strands::line_polarity::light);
		lnk.debug(debug());
		lnk.compute_line_points(out, ismax, line_out, low_thr, high_thr);
		const cv::Mat& esp = lnk.eSpace();
		lnk.compute_contours(ismax, line_out[0], line_out[5], line_out[6], line_out[1], line_out[2], line_out[3], line_out[5]);

		float length_total = 0;
		for (auto cc = 0; cc < lnk.results().size(); cc++) {
			const contour& ct = lnk.results()[cc];
			auto length = ct.compute_length();
			int ilength = (int)length;
			auto miter = lm.find(ilength);
			if (miter == lm.end())
				lm[ilength] = 0;
			auto current = lm[ilength];
			lm[ilength] = current + 1;
			length_total += length;
		}

		float avg_node_count = lnk.results().empty() ? 0 : length_total / float(lnk.results().size());
		std::string output = "AvgLength: " + std::to_string(avg_node_count);
		std::string output2 = "Count: " + std::to_string(lnk.results().size());

		if (debug()) {
			std::cout << output << std::endl;
			std::cout << output2 << std::endl;
		}

		string total_string = version_string + "\n" + output + "\n" + output2 + "\n";
		if (lnk.is_crowded())
			total_string = total_string + "is Crowded \n";

		save_csv(lm, filename.string(), total_string);

		if (show() || graphics()) {
			int factor = 8;
			for (auto cc = 0; cc < lnk.results().size(); cc++) {
				const contour& ct = lnk.results()[cc];
				std::vector<Point> pts;
				std::vector<float>::const_iterator rowItr = ct.row.begin();
				std::vector<float>::const_iterator colItr = ct.col.begin();
				for (auto pp = 0; pp < ct.row.size(); pp++, rowItr++, colItr++) {
					pts.emplace_back(static_cast<int>(*colItr * factor), static_cast<int>(*rowItr * factor));
				}
				polylines(display, pts, false, Scalar(0, 0, 255), 2, LINE_AA, int(std::log2(factor)));
			}

			if (lnk.is_crowded())
				putText(display, "Is Crowded ", Point(1000, 900), FONT_HERSHEY_SIMPLEX, 3.0, Scalar(0, 255, 0), 2);

			std::string output = " Average Length " + std::to_string(avg_node_count);
			putText(display, output.c_str(), Point(1000, 1000), FONT_HERSHEY_SIMPLEX, 3.0, Scalar(0, 255, 0), 2);
			std::string output2 = " Count " + std::to_string(lnk.results().size());
			putText(display, output2.c_str(), Point(1000, 1200), FONT_HERSHEY_SIMPLEX, 3.0, Scalar(0, 255, 0), 2);

			int start = 100;
			auto it = lm.begin();
			while (it != lm.end()) {
				std::string hout = "histogram [" + std::to_string(it->first) + "] = " + std::to_string(it->second);
				putText(display, hout.c_str(), Point(500, start), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255, 0, 0), 2);
				start += 30;
				it++;
			}

			if (show()) {
				imshow(screen.c_str(), display);
				// Wait for any keystroke in the window
				waitKey(0);
			}

			if (graphics()) {
				auto filename = m_input_file.filename();
				filename = "output_" + filename.string();
				filename = m_output_dir / filename;
				cv::imwrite(filename.string(), display);
			}

		}

	}

	catch (std::exception& e) {
		version_string = version_string + " error " + e.what();
		lm.clear();
		save_csv(lm, filename.string(), version_string);
	}

}


#include "utils/data_generator.cpp"
#include "../src/numerical/evolutionary_algorithm/ea_solver.h"
#include "../src/numerical/evolutionary_algorithm/robust_ea_solver.h"
#include "../src/logging/easylogging++.h"
#include <armadillo>

using arma::mat;

INITIALIZE_EASYLOGGINGPP

mat WEIGHTS = {100, -20, 50, -0.5};

arma::mat quadratic_model(arma::mat x, arma::mat L)
{
    return x * arma::pow(L, 2);
}

int main(int argc, char *argv[])
{
    el::Configurations conf("./logging-config.conf");
    el::Loggers::reconfigureLogger("default", conf);

    auto data_generator = DataGenerator();
    auto L = data_generator.generate_library();
    auto s = data_generator.generate_signal(WEIGHTS);
    std::vector<int> outlier_channels = {5, 10};
    auto s_with_outliers = data_generator.generate_signal(WEIGHTS, outlier_channels);

    LOG(INFO) << "True: " << WEIGHTS;

    EASolver ea_solver = EASolver(L);
    ea_solver.set_initial_guess(arma::mat{50, -10, 5, 0});
    mat result1 = ea_solver.solve(s);
    LOG(INFO) << "EA fit: " << result1;

    RobustEASolver robust_ea_solver = RobustEASolver(L);
    mat result2 = robust_ea_solver.solve(s_with_outliers);
    LOG(INFO) << "Robust EA fit: " << result2;

    return 0;
}


#include <util_coo_matrix.h>
#include <util_coo_pcg.h>
#include <util_coo_bicg.h>
#include <util_coo_cr.h>
#include <util_coo_bicgstab.h>
#include <util_coo_gmres.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <boost/random.hpp>
#include <boost/cast.hpp>

template<typename T>
class Random
{
public:

  typedef boost::minstd_rand  generator_type;

protected:

  static generator_type &  generator()
  {
    static generator_type tmp(static_cast<unsigned int>(std::time(0)));
    return tmp;
  }

public:

  typedef boost::uniform_real<T>                                         distribution_type;
  typedef boost::variate_generator<generator_type&, distribution_type >  random_type;

  distribution_type       m_distribution;
  random_type             m_random;

public:

  Random()
  : m_distribution(  boost::numeric_cast<T>(0.0)
                   , boost::numeric_cast<T>(1.0)
                   )
  , m_random(generator(), m_distribution)
  {}

  Random(T const & lower,T const & upper)
  : m_distribution(lower,upper)
  , m_random(generator(), m_distribution)
  {}

private:

  Random(Random const & rnd){}

  Random & operator=(Random const & rnd){return *this;}

public:

  T operator()() { return m_random();  }

  bool operator==(Random const & rnd) const { return m_distribution == rnd.m_distribution; }

};

template<typename T>
inline util::COOMatrix<T>  make_random_matrix( unsigned int const & m, unsigned int const & n )
{
  typedef util::COOMatrix<T>   matrix_type;

  matrix_type A;

  Random<T>  value( T(0.0), T(1.0));

  A.init(m,n,n);

  for(unsigned int i=0u;i<m;++i)
  {
    for(unsigned int j=0u;j<n;++j)
    {
      T const val = value();
      A.insert(i,j,val);
    }
  }
  return A;

}

template<typename T>
inline std::vector<T> make_random_vector( unsigned int const & n)
{
  Random<T>  value( T(0.0), T(1.0));

  std::vector<T> v(n, T(0.0));

  for(unsigned int i=0u; i<n; ++i)
  {
    v[i] = value();
  }

  return v;
}

template<typename T>
inline util::COOMatrix<T>  make_hilbert_matrix( unsigned int const & n )
{
  util::COOMatrix<T> A;

  A.init(n,n,n);

  for(unsigned int i=0u;i<n;++i)
  {
    for(unsigned int j=0u;j<n;++j)
    {
      T const val = T(1.0) / (   T(i+j+1u) );
      A.insert(i,j,val);
    }
  }
  return A;

}

/**
 * Factory function for creating a tri diagonal matrix (special case of a banded matrix).
 *
 * @param n       The size of the matrix to make. The resulting matrix will be of size (n X n)
 * @param a       The value to store on the diagonal of the matrix
 * @param b       The value to store above the diagonal.
 * @param c       The value to store below the diagonal.
 *
 */
template<typename T>
inline util::COOMatrix<T>  make_triadiagonal_matrix( unsigned int const & n
                                                    , T const & a = T(4.0)
                                                    , T const & b = T(1.0)
                                                    , T const & c = T(1.0)
                                                    )
{
  util::COOMatrix<T> A;

  A.init(n,n,3u);

  for(unsigned int i=0u;i<n;++i)
  {
    for(unsigned int j=0u;j<n;++j)
    {
      T val = 0.0;

      if( i==j)
      {
        val = a;
      }
      if( i==j+1u)
      {
        val = b;
      }
      if( i==j-1u)
      {
        val = c;
      }
      if (fabs(val)> T(0.0))
      {
        A.insert(i,j,val);
      }

    }
  }
  return A;
}

template<typename T>
inline util::COOMatrix<T>  make_diagonal_dominant_matrix( unsigned int const & n )
{
  util::COOMatrix<T> A;

  A.init(n,n,n);

  for(unsigned int i=0u;i<n;++i)
  {
    for(unsigned int j=0u;j<n;++j)
    {
      if(i==j)
      {
        A.insert(i,j,T(n*n) );
      }
      else if (fabs( T(j) - T(i) ) <= 2.0 )
      {
        T const val = fabs(  T(j) - T(i) );
        A.insert(i,j,val);
      }
    }
  }
  return A;
}

template<typename T>
class Problem
{
public:

  util::COOMatrix<T>    m_A;   ///< System matrix (assumed non-singular at least)
  std::vector<T>        m_x;   ///< The known solution
  std::vector<T>        m_b;   ///< Right hand side vector

};

template<typename T>
inline Problem<T> make_problem(unsigned int const & n)
{
  Problem<T> problem;

  // problem.m_A = make_hilbert_matrix<T>(n); // Hilbert matrices seems to be badly conditioned as n goes up
  problem.m_A = make_triadiagonal_matrix<T>(n);
  // problem.m_A = make_diagonal_dominant_matrix<T>(n);
  problem.m_x = make_random_vector<T>(n);
  problem.m_b = util::mul(problem.m_A, problem.m_x);

  return problem;
}

BOOST_AUTO_TEST_SUITE(coo_solve_equation);

BOOST_AUTO_TEST_CASE(pcg_testing)
{
  typedef double T;

  util::COOMatrix<T> A  = util::make_identity<T>(4u);
  util::COOMatrix<T> M  = util::make_identity<T>(4u);

  unsigned int    const   max_iterations = 10u;
  unsigned int    const   restart        = 5u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  std::vector<T> b(4u);

  b[0u] = 1.0;
  b[1u] = 2.0;
  b[2u] = 3.0;
  b[3u] = 4.0;

  std::vector<T> x(4u, 0.0);

  bool success = util::pcg(M, A, x, b, max_iterations, restart, tolerance, verbose, &residual);

  BOOST_CHECK(success);

  BOOST_CHECK_CLOSE(x[0u], 1.0, 0.01);
  BOOST_CHECK_CLOSE(x[1u], 2.0, 0.01);
  BOOST_CHECK_CLOSE(x[2u], 3.0, 0.01);
  BOOST_CHECK_CLOSE(x[3u], 4.0, 0.01);
}

BOOST_AUTO_TEST_CASE(pcg_random_testing)
{
  typedef double T;

  unsigned int    const   max_samples    = 10u;      ///< Maximum number of random problems to solve.
  unsigned int    const   n              = 10u;      ///< Problem size
  unsigned int    const   max_iterations = 100u;
  unsigned int    const   restart        = 1u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  for (unsigned int sample=0u; sample < max_samples; ++sample)
  {
    Problem<T> problem = make_problem<T>(n);

    util::COOMatrix<T> M  = util::make_identity<T>(n);

    std::vector<T> x(n, 0.0);

    bool success = util::pcg(
                             M
                             , problem.m_A
                             , x
                             , problem.m_b
                             , max_iterations
                             , restart
                             , tolerance
                             , verbose
                             , &residual
                             );

    BOOST_CHECK(success);

    if(success)
    {
      for (unsigned int i=0u; i < n; ++i)
      {
        BOOST_CHECK_CLOSE(x[i], problem.m_x[i], 0.1);
      }
    }

  }
}

BOOST_AUTO_TEST_CASE(bicg_testing)
{
  typedef double T;

  util::COOMatrix<T> A  = util::make_identity<T>(4u);
  util::COOMatrix<T> At = util::transpose(A);

  util::COOMatrix<T> M  = util::make_identity<T>(4u);
  util::COOMatrix<T> Mt = util::transpose(M);

  unsigned int    const   max_iterations = 10u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  std::vector<T> b(4u);

  b[0u] = 1.0;
  b[1u] = 2.0;
  b[2u] = 3.0;
  b[3u] = 4.0;

  std::vector<T> x(4u, 0.0);

  bool success = util::bicg(A, At, x, b, M, Mt, max_iterations, tolerance, verbose, &residual);

  BOOST_CHECK(success);

  BOOST_CHECK_CLOSE(x[0u], 1.0, 0.01);
  BOOST_CHECK_CLOSE(x[1u], 2.0, 0.01);
  BOOST_CHECK_CLOSE(x[2u], 3.0, 0.01);
  BOOST_CHECK_CLOSE(x[3u], 4.0, 0.01);
}

BOOST_AUTO_TEST_CASE(bicg_random_testing)
{
  typedef double T;

  unsigned int    const   max_samples    = 10u;   ///< Maximum number of random problems to solve.
  unsigned int    const   n              = 10u;   ///< Problem size
  unsigned int    const   max_iterations = 100u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  for (unsigned int sample=0u; sample < max_samples; ++sample)
  {
    Problem<T> problem = make_problem<T>(n);

    util::COOMatrix<T> At = util::transpose(problem.m_A);
    util::COOMatrix<T> M  = util::make_identity<T>(n);
    util::COOMatrix<T> Mt = util::transpose(M);

    std::vector<T> x(n, 0.0);

    bool success = util::bicg(
                              problem.m_A
                              , At
                              , x
                              , problem.m_b
                              , M
                              , Mt
                              , max_iterations
                              , tolerance
                              , verbose
                              , &residual
                              );

    BOOST_CHECK(success);

    if(success)
    {
      for (unsigned int i=0u; i < n; ++i)
      {
        BOOST_CHECK_CLOSE(x[i], problem.m_x[i], 0.1);
      }
    }

  }
}

BOOST_AUTO_TEST_CASE(cr_testing)
{
  typedef double T;

  util::COOMatrix<T> A  = util::make_identity<T>(4u);
  util::COOMatrix<T> M  = util::make_identity<T>(4u);

  unsigned int    const   max_iterations = 10u;
  unsigned int    const   restart       = 5u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  std::vector<T> b(4u);

  b[0u] = 1.0;
  b[1u] = 2.0;
  b[2u] = 3.0;
  b[3u] = 4.0;

  std::vector<T> x(4u, 0.0);

  bool success = util::cr(M, A, x, b, max_iterations, restart, tolerance, verbose, &residual);

  BOOST_CHECK(success);

  BOOST_CHECK_CLOSE(x[0u], 1.0, 0.01);
  BOOST_CHECK_CLOSE(x[1u], 2.0, 0.01);
  BOOST_CHECK_CLOSE(x[2u], 3.0, 0.01);
  BOOST_CHECK_CLOSE(x[3u], 4.0, 0.01);
}

BOOST_AUTO_TEST_CASE(cr_random_testing)
{
  typedef double T;

  unsigned int    const   max_samples    = 10u;    ///< Maximum number of random problems to solve.
  unsigned int    const   n              = 10u;    ///< Problem size
  unsigned int    const   max_iterations = 100u;
  unsigned int    const   restart        = 1u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  for (unsigned int sample=0u; sample < max_samples; ++sample)
  {
    Problem<T> problem = make_problem<T>(n);

    util::COOMatrix<T> M  = util::make_identity<T>(n);

    std::vector<T> x(n, 0.0);

    bool success = util::cr(
                             M
                             , problem.m_A
                             , x
                             , problem.m_b
                             , max_iterations
                             , restart
                             , tolerance
                             , verbose
                             , &residual
                             );

    BOOST_CHECK(success);

    if(success)
    {
      for (unsigned int i=0u; i < n; ++i)
      {
        BOOST_CHECK_CLOSE(x[i], problem.m_x[i], 0.1);
      }
    }

  }
}

BOOST_AUTO_TEST_CASE(bicgstab_testing)
{
  typedef double T;

  util::COOMatrix<T> A  = util::make_identity<T>(4u);
  util::COOMatrix<T> M  = util::make_identity<T>(4u);

  unsigned int    const   max_iterations = 10u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  std::vector<T> b(4u);

  b[0u] = 1.0;
  b[1u] = 2.0;
  b[2u] = 3.0;
  b[3u] = 4.0;

  std::vector<T> x(4u, 0.0);

  bool success = util::bicgstab(M, A, x, b, max_iterations, tolerance, verbose, &residual);

  BOOST_CHECK(success);

  BOOST_CHECK_CLOSE(x[0u], 1.0, 0.01);
  BOOST_CHECK_CLOSE(x[1u], 2.0, 0.01);
  BOOST_CHECK_CLOSE(x[2u], 3.0, 0.01);
  BOOST_CHECK_CLOSE(x[3u], 4.0, 0.01);
}

BOOST_AUTO_TEST_CASE(bicgstab_random_testing)
{
  typedef double T;

  unsigned int    const   max_samples    = 10u;      ///< Maximum number of random problems to solve.
  unsigned int    const   n              = 10u;      ///< Problem size
  unsigned int    const   max_iterations = 100u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  for (unsigned int sample=0u; sample < max_samples; ++sample)
  {
    Problem<T> problem = make_problem<T>(n);

    util::COOMatrix<T> M  = util::make_identity<T>(n);

    std::vector<T> x(n, 0.0);

    bool success = util::bicgstab(
                            M
                            , problem.m_A
                            , x
                            , problem.m_b
                            , max_iterations
                            , tolerance
                            , verbose
                            , &residual
                            );

    BOOST_CHECK(success);

    if(success)
    {
      for (unsigned int i=0u; i < n; ++i)
      {
        BOOST_CHECK_CLOSE(x[i], problem.m_x[i], 0.1);
      }
    }

  }
}

BOOST_AUTO_TEST_CASE(gmres_testing)
{
  typedef double T;

  util::COOMatrix<T> A  = util::make_identity<T>(4u);
  util::COOMatrix<T> M  = util::make_identity<T>(4u);

  unsigned int    const   max_iterations = 10u;
  unsigned int    const   restart        = 5u;
  T               const   tolerance      = 1e-10;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  std::vector<T> b(4u);

  b[0u] = 1.0;
  b[1u] = 2.0;
  b[2u] = 3.0;
  b[3u] = 4.0;

  std::vector<T> x(4u, 0.0);

  bool success = util::gmres(M, A, x, b, max_iterations, restart, tolerance, verbose, &residual);

  BOOST_CHECK(success);

  BOOST_CHECK_CLOSE(x[0u], 1.0, 0.01);
  BOOST_CHECK_CLOSE(x[1u], 2.0, 0.01);
  BOOST_CHECK_CLOSE(x[2u], 3.0, 0.01);
  BOOST_CHECK_CLOSE(x[3u], 4.0, 0.01);
}

BOOST_AUTO_TEST_CASE(gmres_random_testing)
{
  typedef double T;

  unsigned int    const   max_samples    = 10u;      ///< Maximum number of random problems to solve.
  unsigned int    const   n              = 10u;      ///< Problem size
  unsigned int    const   max_iterations = 100u;
  unsigned int    const   restart        = 1u;

  // 2015-05-08 Marek: the tolerance 1e-10 was probably a bit too much for gmres.
  //                   It seems to have a tendency to get "stuck" at a certain residual value.
  T               const   tolerance      = 1e-7;
  bool            const   verbose        = true;
  std::vector<T>          residual;

  for (unsigned int sample=0u; sample < max_samples; ++sample)
  {
    Problem<T> problem = make_problem<T>(n);

    util::COOMatrix<T> M  = util::make_identity<T>(n);

    std::vector<T> x(n, 0.0);

    bool success = util::gmres(
                             M
                             , problem.m_A
                             , x
                             , problem.m_b
                             , max_iterations
                             , restart
                             , tolerance
                             , verbose
                             , &residual
                             );

    BOOST_CHECK(success);

    if(success)
    {
      for (unsigned int i=0u; i < n; ++i)
      {
        BOOST_CHECK_CLOSE(x[i], problem.m_x[i], 0.1);
      }
    }

  }
}

BOOST_AUTO_TEST_SUITE_END();


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/**
 * @file fft.cpp - computing the canonical embedding and related norms
 **/
#include <complex>
#include <cmath>
#include <numeric> // std::accumulate
#include <NTL/BasicThreadPool.h>
#include "NumbTh.h"
#include "timing.h"
#include "norms.h"
#include "PAlgebra.h"
NTL_CLIENT

const double pi = 4 * std::atan(1);

#ifdef FFT_ARMA
#warning "canonicalEmbedding implemented via Armadillo"
#include <armadillo>
void convert(zzX& to, const arma::vec& from)
{
  to.SetLength(from.size());
  NTL_EXEC_RANGE(to.length(), first, last)
  for (long i=first; i<last; i++)
    to[i] = std::round(from[i]);
  NTL_EXEC_RANGE_END
}

void convert(arma::vec& to, const zzX& from)
{
  to.resize(from.length());
  NTL_EXEC_RANGE(from.length(), first, last)
  for (long i=first; i<last; i++)
    to[i] = from[i];
  NTL_EXEC_RANGE_END
}

void convert(arma::vec& to, const ZZX& from)
{
  to.resize(from.rep.length());
  NTL_EXEC_RANGE(from.rep.length(), first, last)
  for (long i=first; i<last; i++) {
    double x = conv<double>(from[i]);
    to[i] = x;
  }
  NTL_EXEC_RANGE_END
}

#if 0

//======================

namespace arma {

  template<>
  struct is_supported_elem_type<RR> {
    enum {value = 1};
  };

  template<>
  struct is_supported_elem_type<cx_RR> {
    enum {value = 1};
  };

  template<>
  struct is_real<RR> {
    enum {value = 1};
  };

}

void convert(zzX& to, const arma::Col<RR>& from)
{
  to.SetLength(from.size());
  for (long i: range(to.length()))
    to[i] = conv<long>(RoundToZZ(from[i]));
}

void convert(arma::Col<RR>& to, const zzX& from)
{
  to.resize(from.length());
  for (long i: range(from.length()))
    to[i] = from[i];
}

void convert(arma::Col<RR>& to, const ZZX& from)
{
  to.resize(from.rep.length());
  for (long i: range(from.rep.length())) {
    RR x = conv<RR>(from[i]);
    to[i] = x;
  }
}

#endif

// Computing the canonical embedding. This function returns in v only
// the first half of the entries, the others are v[phi(m)-i]=conj(v[i])
void canonicalEmbedding(std::vector<cx_double>& v,
                        const zzX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::vec av; // convert to vector of doubles
  convert(av, f);
  arma::cx_vec avv = arma::fft(av,m); // compute the full FFT

  v.resize(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      v[phimBy2-i-1] = avv[palg.ith_rep(i)];
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) v[idx++] = avv[i];
}

void canonicalEmbedding(std::vector<cx_double>& v,
                        const ZZX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::vec av; // convert to vector of doubles
  convert(av, f);
  arma::cx_vec avv = arma::fft(av,m); // compute the full FFT

  v.resize(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      v[phimBy2-i-1] = avv[palg.ith_rep(i)];
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) v[idx++] = avv[i];
}

#if 0
void canonicalEmbedding(std::vector<cx_RR>& v,
                        const ZZX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::Col<RR> av; // convert to vector of doubles
  convert(av, f);
  arma::Col<cx_RR> avv = arma::fft(av,m); // compute the full FFT

  v.resize(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      v[phimBy2-i-1] = avv[palg.ith_rep(i)];
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) v[idx++] = avv[i];
}
#endif

// Roughly the inverse of canonicalEmbedding, except for scaling and
// rounding issues. Calling embedInSlots(f,v,palg,1.0,strictInverse=true)
// after setting canonicalEmbedding(v, f, palg), is sure to recover the
// same f, but embedInSlots(f,v,palg,1.0,strictInverse=false) may return
// a different "nearby" f.
void embedInSlots(zzX& f, const std::vector<cx_double>& v,
                  const PAlgebra& palg, double scaling, bool strictInverse)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::cx_vec avv(m);
  for (auto& x: avv) x = 0.0;

  if (palg.getNSlots()==phimBy2) // roots ordered by the palg order
    for (long i=0; i<palg.getNSlots(); i++) {
      long j = palg.ith_rep(i);
      long ii = palg.getNSlots()-i-1;
      if (ii < lsize(v)) {
        avv[j] = scaling*v[ii];
        avv[m-j] = std::conj(avv[j]);
      }
    }
  else                           // roots ordered sequentially
    for (long i=1, idx=0; i<=m/2 && idx<lsize(v); i++) {
      if (palg.inZmStar(i)) {
        avv[i] = scaling*v[idx++];
        avv[m-i] = std::conj(avv[i]);
      }
    }
  arma::vec av = arma::real(arma::ifft(avv,m)); // compute the inverse FFT

  // If v was obtained by canonicalEmbedding(v,f,palg,1.0) then we have
  // the guarantee that m*av is an integral polynomial, and moreover
  // m*av mod Phi_m(x) is in m*Z[X].
  if (strictInverse) av *= m; // scale up by m
  convert(f, av);    // round to an integer polynomial
  reduceModPhimX(f, palg);
  if (strictInverse) f /= m;  // scale down by m
  normalize(f);
}
#else
#ifdef FFT_NATIVE
#warning "canonicalEmbedding implemented via slow DFT, expect very slow key-generation"
// An extremely lame implementation of the canonical embedding

// evaluate poly(x) using Horner's rule
cx_double complexEvalPoly(const zzX& poly, const cx_double& x)
{
  if (lsize(poly)<=0) return cx_double(0.0,0.0);
  cx_double res(double(poly[0]), 0.0);
  for (long i=1; i<lsize(poly); i++) {
    res *= x;
    res += cx_double(double(poly[i]));
  }
  return res;
}

void canonicalEmbedding(std::vector<cx_double>& v, const zzX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  vector<long> zmstar(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      zmstar[phimBy2-i-1] = palg.ith_rep(i);
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) zmstar[idx++] = i;

  v.resize(phimBy2);
  NTL_EXEC_RANGE(phimBy2, first, last)
  for (long i=first; i < last; ++i) {
    auto rou = std::polar<double>(1.0, -(2*pi*zmstar[i])/m); // root of unity
    v[i] = complexEvalPoly(f,rou);
  }
  NTL_EXEC_RANGE_END
  FHE_TIMER_STOP;
}

// evaluate poly(x) using Horner's rule
// FIXME: this is actually evaluating the reverse polynomial
cx_double complexEvalPoly(const Vec<double>& poly, const cx_double& x)
{
  if (poly.length()<=0) return cx_double(0.0,0.0);
  cx_double res(poly[0], 0.0);
  for (long i: range(1, poly.length())) {
    res *= x;
    res += cx_double(poly[i]);
  }
  return res;
}

void canonicalEmbedding(std::vector<cx_double>& v, const ZZX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  vector<long> zmstar(phimBy2); // the first half of Zm*

  Vec<double> ff;
  conv(ff, f.rep);

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      zmstar[phimBy2-i-1] = palg.ith_rep(i);
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) zmstar[idx++] = i;

  v.resize(phimBy2);
  NTL_EXEC_RANGE(phimBy2, first, last)
  for (long i=first; i < last; ++i) {
    auto rou = std::polar<double>(1.0, -(2*pi*zmstar[i])/m); // root of unity
    v[i] = complexEvalPoly(ff,rou);
  }
  NTL_EXEC_RANGE_END
  FHE_TIMER_STOP;
}

void embedInSlots(zzX& f, const std::vector<cx_double>& v,
                  const PAlgebra& palg, double scaling, bool strictInverse)
{
  NTL::Error("embedInSlots not implemented\n");
}
#endif // ifdef FFT_NATIVE
#endif // ifdef FFT_ARMA


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>                           math_types;
typedef math_types::vector_type                                                       vector_type;
typedef std::vector<double>                                                           knot_container;
typedef std::vector<vector_type>                                                      point_container;
typedef OpenTissue::spline::NUBSpline<knot_container, point_container>                NUBSpline;

knot_container init_knots(int const & k, int const & M)
{
  int m = M - 1;
  int const n = m-k;

  knot_container U;

  double knot_value = 0.0;
  int i = 0;
  for(;i<k;++i)
   U.push_back(knot_value);

  for(;i<=n;++i)
  {
    knot_value += 1.0;
     U.push_back(knot_value);
  }
  knot_value += 1.0;
  for(;i<M;++i)
   U.push_back(knot_value);
  return U;
}

BOOST_AUTO_TEST_SUITE(opentissue_spline_compute_knot_span);

BOOST_AUTO_TEST_CASE(test_compute_knot_span)
{
  // Create knot vector: U = 0 0 0 1 2 3 4 5 5 5
  knot_container U = init_knots(3,10);
  {
    double u = -0.5;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 2 );
  }
  {
    double u = 0.0;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 2 );
  }
  {
    double u = 0.5;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 2 );
  }
  {
    double u = 1.0;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 3 );
  }
  {
    double u = 1.5;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 3 );
  }
  {
    double u = 2.0;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 4 );
  }
  {
    double u = 2.5;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 4 );
  }
  {
    double u = 3.0;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 5 );
  }
  {
    double u = 3.5;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 5 );
  }
  {
    double u = 4.0;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 6 );
  }
  {
    double u = 4.5;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 6 );
  }
  {
    double u = 5.0;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 6 );
  }
  {
    double u = 5.5;
    int idx = OpenTissue::spline::detail::compute_knot_span(u,3,U);
    BOOST_CHECK( idx == 6 );
  }
  // Illegal order
  {
    double u = 2.5;
    BOOST_CHECK_THROW( OpenTissue::spline::detail::compute_knot_span(u,0,U), std::invalid_argument );
  }
  // Too few element in U
  {
    double u = 2.5;
    BOOST_CHECK_THROW( OpenTissue::spline::detail::compute_knot_span(u,6,U), std::invalid_argument );
  }
}

BOOST_AUTO_TEST_SUITE_END();