/**
 * @file pedersen_test.cc
 *
 * Analyze the performance of the model against the extreme downward refraction
 * N^2 linear test case developed by Pedersen and Gordon.  This is also the
 * test case that Weinberg and Keenan used to calibrate the minimum beam width
 * in GRAB. This test serves several important functions
 *
 *  - Compares the ray path locations, travel time, and D/E angles
 *    to analytic results.
 *  - Compares the total propagation loss to analytic results.
 *  - Analyzes the contribution of the phase change at the caustic.
 *
 * The Pedersen/Gordon paper focuses two test cases.
 *
 * - The shallow source portion analyzed an acoustic field for a downward
 *   refracting profile that had been fit to experimental velocity data.
 *   Propagation loss was studied in both the image interference zone
 *   and the shadow zone.
 * - The deep source test cases extended this analysis to a non-physical
 *   extreme that emphasizes the effects of the caustic.
 *
 * Pedersen/Gordon concludes that the pi/2 phase change should only occur
 * when the ray path touches the caustic and not when the ray passes through
 * a horizontal vertex.  This is consistent with the current implementation
 * of GRAB, but different than the original Weinberg/Keenan paper.
 *
 * Uses the MKS version of the parameters defined in Jensen et. al.
 * instead of the original yd/sec version defined by Pedersen.
 *
 *  - c0 = 1550 m/s, g0 = 1.2 1/s, f = 2000 Hz
 *  - shallow:
 *      - source: depth = 75 m, time inc=0.025, cs = 1467.149925 m/s
 *      - target: depth = 75 m, range = 500-1000 m
 *  - deep:
 *      - source: depth = 1000 m, time inc=0.1, cs = 970.954770 m/s
 *      - target: depth = 800 m, range = 3000-3100 m
 *
 * @xref M. A. Pedersen, D. F. Gordon, "Normal-Mode and Ray Theory Applied
 * to Underwater Acoustic conditions of Extreme Downward Refraction",
 * J. Acoust. Soc. Am. 51 (1B), 323-368 (June 1972).
 * @xref H. Weinberg, R. E. Keenan, “Gaussian ray bundles for modeling
 * high-frequency propagation loss under shallow-water conditions”,
 * J. Acoust. Soc. Am. 100 (3), 1421-1431, (Sept 1996).
 * @xref L. M. Brekhovskikh, Waves in Layered Media, 2nd Edition,
 * Academic Press Inc., 1980, Section 54.
 * @xref F. B. Jensen, W. A. Kuperman, M. B. Porter, and H. Schmidt,
 * Computational Ocean Acoustics (American Institute of Physics Press,
 * New York, 1994) pp. 162-166.
 */
#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(pedersen_test)

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

// Define analysis parameter constants.

static const double FREQ = 2000 ;   // Hz
static const double C0 = 1550.0 ;   // sound speed at surface (m/s)
static const double G0 = 1.2 ;      // speed gradient at surface (1/s)
static const double LAT_SOURCE = 45.0;  // source latitude (deg)
static const double LNG_SOURCE = -45.0; // source longitude (deg)

/**
 * Construct the ocean model from its constituant parts.
 *
 * - N^2 Linear Profile on a Flat Earth
 * - No Attenuation
 * - Default Sea Surface
 * - "Infinitely" deep bottom
 *
 * @return  Dynamically allocated ocean model.
 *          Calling routine responsible for cleanup.
 */
ocean_model* build_ocean() {
    wposition::compute_earth_radius(LAT_SOURCE);

    attenuation_model* attn = new attenuation_constant(0.0);
    profile_model* profile = new profile_n2( C0, G0, attn );
    profile->flat_earth(true);

    boundary_model* surface = new boundary_flat();
    boundary_model* bottom = new boundary_flat(30000.0);

    return new ocean_model(surface, bottom, profile);
}

/**
 * Tests the accuracy of the raytrace model against the analytic
 * solutions for N^2 linear test case developed by Pedersen/Gordon.
 * Limits this comparison to the portion of the propagation after
 * all the rays are heading down.
 *
 * @param   source_depth    Source depth (m)
 * @param   de              Source D/E angles (deg)
 * @param   time_threshold  Error tolerance for travel time (%)
 * @param   range_threshold Error tolerance for target range (%)
 * @param   de_threshold    Error tolerance for target D/E angle (%)
 * @param   ncfile          File used to store wavefront history
 * @param   csvfile         File used to store model/theory comparisons
 */
void analyze_raytrace(
    double source_depth, const seq_vector& de,
    double time_min, double time_inc, double time_max,
    double time_threshold, double range_threshold, double de_threshold,
    const char* ncfile, const char* csvfile )
{
    // initialize source information

    ocean_model* ocean = build_ocean() ;
    seq_log freq( FREQ, 1.0, 1 ) ;
    wposition1 pos( LAT_SOURCE, LNG_SOURCE, source_depth );
    seq_linear az(0.0,1.0,1);       // only 1 ray needed for ray trace

    // compute the ray trace and store wavefronts to disk

    wave_queue wave( *ocean, freq, pos, de, az, time_inc );
    cout << "writing wavefronts to " << ncfile << endl;
    wave.init_netcdf(ncfile); // open a log file for wavefront data
    wave.save_netcdf();     // write initial ray data to log file

    cout << "writing spreadsheets to " << csvfile << endl;
    std::ofstream os(csvfile);
    os << "DElaunch,Tmodel,Ttheory,Rmodel,Rtheory,DEmodel,DEtheory" << endl;
    os << std::setprecision(18);
    const double Cs = wave.curr()->sound_speed(0,0) ; // sound speed at source
    const double b = G0 * 2 / (C0*C0*C0) ;

    double time_max_error = 0.0 ;
    double range_max_error = 0.0 ;
    double de_max_error = 0.0 ;

    while ( wave.time() < time_max ) {
        wave.step();        // propagate by one time step
        wave.save_netcdf(); // write ray data to log file

        // compare to the analytic results once all the rays are heading down

        if ( wave.time() >= time_min  ) {
            for ( size_t d=0 ; d < wave.num_de() ; ++d ) {

                // find Cm = speed at which each ray becomes horizonal

                const double As = -to_radians(wave.source_de(d)); // angle at source
                const double Cm = Cs / cos(As) ;    // Pedersen's ray parameter

                // find A0 = the angle at which this ray will hit the surface
                // Pedersen eq. 9: cos(A0) = C0/Cm

                const int surface = wave.curr()->surface(d,0) ;
                const double A0 = (surface==0) ? NAN : acos(min(1.0,C0/Cm)) ;

                // find Ah = the angle at the target location
                // Pedersen eq. 9: cos(Ah) = Ch/Cm

                const double Ch = wave.curr()->sound_speed(d,0) ;
                const double Ah = acos(Ch/Cm) ;

                wvector1 ndir( wave.curr()->ndirection, d, 0 ) ;
                double DEmodel, AZmodel ;
                ndir.direction( &DEmodel, &AZmodel ) ;

                // find R = horizontal range to the ray at the target location
                // Pedersen eq. 11: R=2/(b Cm^2)[tan(Ah)-tan(As)-2 tan(A0)]

                const double tanAh = tan(Ah);
                const double tanAs = tan(As);
                const double tanA0 = (surface==0) ? 0.0 : tan(A0);

                const double R = (2/(b*Cm*Cm)) * ( tanAh - tanAs - 2*tanA0 ) ;

                const double Rmodel = wposition::earth_radius * to_radians(
                    wave.curr()->position.latitude(d,0) - LAT_SOURCE ) ;

                // find T = travel time to the ray at the target location
                // Pedersen eq. 12:
                // R = 2/(3b Cm^3)[tan^3(Ah)-tan^3(As)-2tan^3(A0)] + R/Cm

                const double T = (2/(3*b*Cm*Cm*Cm))
                    * ( tanAh*tanAh*tanAh - tanAs*tanAs*tanAs
                      - 2*tanA0*tanA0*tanA0 ) + R / Cm ;

                // write the results to a spreadsheet

                os << -to_degrees(As)
                   << "," << wave.time() << "," << T
                   << "," << Rmodel << "," << R
                   << "," << DEmodel << "," << -to_degrees(Ah)
                   << endl ;

                const double time_error = wave.time()-T ;
                const double range_error = Rmodel-R ;
                const double de_error = DEmodel+to_degrees(Ah) ;
                time_max_error = max( time_max_error, abs(time_error) ) ;
                range_max_error = max( range_max_error, abs(range_error) ) ;
                de_max_error = max( de_max_error, abs(de_error) ) ;
                BOOST_CHECK_SMALL( time_error, time_threshold ) ;
                BOOST_CHECK_SMALL( range_error, range_threshold ) ;
                BOOST_CHECK_SMALL( de_error, de_threshold ) ;
            }
        }
    }

    // clean up and exit

    wave.close_netcdf(); // close log file for wavefront data
    delete ocean ;
    cout << "max errors: time=" << time_max_error
         << " range=" << range_max_error
         << " de=" << de_max_error << endl ;
}

/**
 * Tests the accuracy of the raytrace model against the analytic
 * solution for N^2 linear, shallow source, test case used by Pedersen/Gordon.
 * Generates linearly spaced beams in a 0:0.5:25 degree fan
 * to match Figure 3.6 in Jensen et. al.  The critical ray for a source
 * at 75.0 meters is 18.82 degrees.
 *
 * Errors are automatically generated if the ray paths deviate from the
 * analytic results by more than 3 millisecs in time, 5.0 meters in range,
 * or 0.2 degrees in angle.
 */
BOOST_AUTO_TEST_CASE( pedersen_shallow_raytrace ) {
    cout << "=== pedersen_shallow_raytrace ===" << endl ;
    seq_linear de( 0.0, 0.02, 25.00 ) ;
    analyze_raytrace( -75.0, de,
        0.30, 0.01, 0.85,
        0.003, 5.0, 0.2,
        USML_STUDIES_DIR "/pedersen/pedersen_shallow_raytrace.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_shallow_raytrace.csv" ) ;
}

/**
 * Tests the accuracy of the raytrace model against the analytic
 * solution for N^2 linear test case used by Pedersen/Gordon.
 * Generates linearly spaced beams in a 20:1:60 degrees fan
 * to match Figure 3.8 in Jensen et. al.  The critical ray for a source
 * at 1000.0 meters is 51.21 degrees.
 *
 * Errors are automatically generated if the ray paths deviate from the
 * analytic results by more than 8 millisecs in time, 10 meters in range,
 * or 0.02 degrees in angle.
 */
BOOST_AUTO_TEST_CASE( pedersen_deep_raytrace ) {
    cout << "=== pedersen_deep_raytrace ===" << endl ;
    seq_linear de( 20.00, 0.2, 60.00 ) ;
    analyze_raytrace( -1000.0, de,
        2.0, 0.01, 3.5,
        0.008, 10.0, 0.03,
        USML_STUDIES_DIR "/pedersen/pedersen_deep_raytrace.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_deep_raytrace.csv" ) ;
}

/**
 * Tests the accuracy of the eigenray_collection model against the analytic
 * solution for N^2 linear test case developed by Pedersen/Gordon.
 *
 * @param   time_step       Size of the time step to used in model (sec)
 * @param   de              Source D/E angles (deg)
 * @param   frequency       Source frequency (Hz)
 * @param   source_depth    Source depth (m)
 * @param   target_depth    Target depth (m)
 * @param   target_range    List of target ranges (m)
 * @param   ncfile          File used to store eigenrays
 * @param   csvfile         File used to store model/theory comparisons
 */
void analyze_proploss(
    const seq_vector& de, double source_depth, double target_depth,
    const seq_vector& target_range, double time_inc, double time_max,

    const char* ncfile, const char* csvfile )
{
    // initialize source information

    ocean_model* ocean = build_ocean() ;
    seq_log freq( FREQ, 1.0, 1 ) ;
    wposition1 pos( LAT_SOURCE, LNG_SOURCE, source_depth );
    seq_linear az(-4.0,1.0,4.0);

    // build a series of targets at different ranges

    wposition target(target_range.size(),1,LAT_SOURCE,LNG_SOURCE,target_depth);
    for (size_t n = 0; n < target.size1(); ++n) {
        const double angle = target_range[n] / wposition::earth_radius ;
        target.latitude( n, 0, LAT_SOURCE + to_degrees(angle) );
    }

    eigenray_collection loss(freq, pos, de, az, time_inc, &target);

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

    // adding eigenray_collection listener
    wave.add_eigenray_listener(&loss);

    // compute the eigenray_collection and store eigenrays to disk

    cout << "propagate wavefronts" << endl;
    while ( wave.time() < time_max ) {
        wave.step();        // propagate by one time step
    }
    loss.sum_eigenrays();
    cout << "writing eigenrays to " << ncfile << endl;
    loss.write_netcdf(ncfile);

    // save results to spreadsheet for post-test analysis

    cout << "writing spreadsheets to " << csvfile << endl;
    std::ofstream os(csvfile);
    os << "range,model,first,second" << endl;
    os << std::setprecision(18);

    vector<double> tl_model(target.size1());
    for (size_t n = 0; n < target.size1(); ++n) {
        double tl_model = -loss.total(n,0)->intensity(0);
        os << target_range[n] << "," << tl_model ;

        const eigenray_list* raylist = loss.eigenrays(n, 0);
        for (eigenray_list::const_iterator iter = raylist->begin();
                iter != raylist->end(); ++iter) {
            const eigenray &ray = *iter;
            os << "," << (-ray.intensity(0));
        }
        os << endl;
    }

    // clean up and exit

    delete ocean ;
}

/**
 * Tests the accuracy of the eigenray_collection model against the analytic solution for
 * the shallow source, N^2 linear test case developed by Pedersen and Gordon.
 * The source is located at a depth of 75 meters. Receivers have a depth of
 * 75 meters and ranges from 500 to 1000 yds. Uses rays from 0 to 30 degrees at
 * a very a tight 0.025 deg spacing.  Such tight spacing is needed to capture
 * the portion of the surface reflected path just inside of the critical ray.
 *
 * This configuration tests the model's ability to produce accurate propagation
 * loss, phase, travel times, and eigneray angles at the edge of a shadow zone.
 * Eigenrays are written out in both netCDF and CSV format so that they can be
 * compared to other models off-line.
 */
BOOST_AUTO_TEST_CASE( pedersen_shallow_proploss ) {
    cout << "=== pedersen_shallow_proploss ===" << endl ;
    seq_linear ranges(500.0,1.0,1000.0) ;
    seq_linear de( 0.0, 0.025, 25.0 ) ;
    analyze_proploss( de, -75.0, -75.0, ranges,
        0.01, 0.85,
        USML_STUDIES_DIR "/pedersen/pedersen_shallow_proploss.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_shallow_proploss.csv" ) ;
}

/**
 * Tests the accuracy of the eigenray_collection model against the analytic solution for
 * the deep source, N^2 linear test case developed by Pedersen and Gordon.
 * The source is located at a depth of 1000 meters. Receivers have a depth of
 * 800 meters and ranges from 3000 to 3120 meters. Uses rays from 20
 * to 60 degrees at a 0.25 deg spacing.
 *
 * We found that tangent spaced beams did not work very well for this
 * scenario.  Several combinations of parameters lead to artifacts in the
 * shadow zone from an imperfect cancellation of the direct and caustic paths.
 * We believe that this is caused the accumulation of small cell width
 * and target range errors over hundreds of contributing beams.
 *
 * This configuration tests the model's ability to produce accurate propagation
 * loss, phase, travel times, and eigneray angles in an area where a caustic
 * causes rays to cross at the edge of a shadow zone.  Eigenrays are written
 * out in both netCDF and CSV format so that they can be  compared to other
 * models off-line.
 */
BOOST_AUTO_TEST_CASE( pedersen_deep_proploss ) {
    cout << "=== pedersen_deep_proploss ===" << endl ;
    seq_linear ranges(3000.0,0.25,3120.0) ;
    seq_linear de( 20.0, 0.25, 60.0 ) ;
    analyze_proploss( de, -1000.0, -800.0, ranges,
        0.01, 3.5,
        USML_STUDIES_DIR "/pedersen/pedersen_deep_proploss.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_deep_proploss.csv" ) ;
}

/**
 * Tests the sensitivity of the eigenray_collection model the D/E angular resolution
 * near the caustic. The source is located at a depth of 1000 yds. Receivers
 * have a depth of 800 yds and ranges from 3100 to 3180 yds. Uses a ray fan
 * from +40 to +51 degrees with increments of 0.025, 0.05, 0.10, and 0.20
 * degrees. This configuration test the models sensitivity to ray spacing
 * near the caustic.
 *
 * The N^2 linear test case developed by Pedersen and Gordon was specifically
 * chosen because we expect it to be very sensitive to the ray geometry near
 * the caustic.  This sensitivity is especially true for the deep source
 * varient, because the profile below 200 meters is more extreme than those
 * found in the real world.
 */
BOOST_AUTO_TEST_CASE( pedersen_deep_sensitivity ) {
    cout << "=== pedersen_deep_sensitivity ===" << endl ;
    seq_linear ranges(3000.0,0.25,3120.0) ;

    seq_rayfan de( -90.0, 90.0, 181, 51.21 ) ;
    analyze_proploss( de, -1000, -800.0, ranges, 0.01, 3.5,
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_tan.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_tan.csv" ) ;

    seq_linear de1000( 40.00, 0.100, 60.00 ) ;
    analyze_proploss( de1000, -1000, -800.0, ranges, 0.01, 3.5,
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_1000.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_1000.csv" ) ;

    seq_linear de0500( 40.00, 0.05000, 60.00 ) ;
    analyze_proploss( de0500, -1000, -800.0, ranges, 0.01, 3.5,
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_0500.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_0500.csv" ) ;

    seq_linear de0250( 40.00, 0.0250, 60.00 ) ;
    analyze_proploss( de0250, -1000, -800.0, ranges, 0.01, 3.5,
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_0250.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_0250.csv" ) ;

    seq_linear de0125( 40.00, 0.0125, 60.00 ) ;
    analyze_proploss( de0125, -1000, -800.0, ranges, 0.01, 3.5,
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_0125.nc",
        USML_STUDIES_DIR "/pedersen/pedersen_deep_sensitivity_0125.csv" ) ;
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


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

#include <boost/math/ccmath/scalbln.hpp>
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<float>(boost::math::ccmath::scalbln(1.0f, 1l));
   check_result<double>(boost::math::ccmath::scalbln(1.0, 1l));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::ccmath::scalbln(1.0l, 1l));
#endif
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <vector>

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

template <typename Vector>
void test(const char* name, const Vector& v)
{
    std::cout << name << ": v is " << v << ", trans(v) is " << trans(v) << ", trans(v) * v is "
	      << trans(v) * v << "\n";
    MTL_THROW_IF(std::abs(std::abs(trans(v) * v) - 61.0) > 0.1, mtl::runtime_error("Wrong result"));
}

template <typename Vector>
void shape(const Vector&)
{
    std::cout << typeid(typename mtl::ashape::ashape<Vector>::type).name() << "\n";

}

template <typename Op1, typename Op2>
void shape(const Op1&, const Op2&)
{
    using namespace mtl;

    typedef typename ashape::mult_op<typename ashape::ashape<Op1>::type, typename ashape::ashape<Op2>::type>::type op_type;
    std::cout << "Op type is " << typeid(op_type).name() << "\n";

    typedef boost::is_same<op_type, ashape::rvec_cvec_mult>                same;
    typedef typename same::type                                            same_type;
    std::cout << "same type is " << typeid(same_type).name() << "\n";

    typedef boost::enable_if<same, float>          enable;
    typedef typename enable::type                  enable_type;
    std::cout << "enable type is " << typeid(enable_type).name() << "\n";

    typedef typename vec::detail::dot_result<Op1, Op2>::type res_type;
    std::cout << "res is " << typeid(res_type).name() << "\n";

}

template <typename Vector>
void test2(const char* name, const Vector& v)
{
    // shape(v); shape(trans(v));
    // shape(v, trans(v));
#if 1
    std::cout << name << ": v is " << v << ", trans(v) is " << trans(v) << ", v * trans(v) is "
	      << v * trans(v) << "\n";
    MTL_THROW_IF(std::abs(std::abs(v * trans(v)) - 61.0) > 0.1, mtl::runtime_error("Wrong result"));
#endif
}

int main(int, char**)
{

    typedef mtl::vec::fixed::dimension<3> fsize;
    mtl::dense_vector<float, mtl::vec::parameters<mtl::row_major, fsize, true> >     rf; rf= 3., 4., 6.;
    mtl::dense_vector<float, mtl::vec::parameters<mtl::col_major, fsize, true> >     cf; cf= 3., 4., 6.;

    mtl::dense_vector<float, mtl::vec::parameters<mtl::row_major> >                  rd(3); rd= 3., 4., 6.;
    mtl::dense_vector<float>                                                            cd(3); cd= 3., 4., 6.;

    mtl::dense_vector<std::complex<double> >                                            rdc(3); rdc= 3., 4., 6.;

    mtl::dense2D<float>  A(3, 3);
    A= 2, 3, 4,
       3, 4, 6,
       7, 6, 9;

    test2("Row vector fixed size", rf);

    test("Column vector fixed size", cf);
    test2("Row vector", rd);
    test("Column vector", cd);
    //test2("Row vector complex", rdc);
#if 0
    test2("Matrix row", A[mtl::iall][1]);
    test("Matrix column", A[1][mtl::iall]);
#endif
    return 0;
}


#include <boost/lexical_cast.hpp>
#include <iostream>

using namespace std;
using namespace boost;

int main()
{
  std::string x = "3.14159265358979";

  double a;
  std::istringstream is(x);
  is >> a;

  double b;
  b = lexical_cast<double>(x);

  cout << "a = " << a << ", b = " << b << endl;

  return 0;
}


// http://people.cs.clemson.edu/~bcdean/dp_practice/
//
// "Two Ways of Thinking about Dynamic Programming... "
//
// Тренироваться только на решенных, иначе не ясно. Нужно сверяться
//
#include "visuality/view.h"

#include <gtest/gtest.h>

#include <boost/unordered_set.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <cassert>
#include <string>
#include <vector>
#include <algorithm>
#include <numeric>

using namespace std;
using namespace boost;
using namespace view;

TEST(Dyn, MaxGrow) {
  // http://e-maxx.ru/algo/longest_increasing_subseq_log
  int arr[] = {5, 2, 8, 6, 3, 6, 9, 7};
  vector<int> v(arr, arr+8);

  //list<int>
  vector<int> d(v.size(), 0);
  for (int i=0; i<v.size(); ++i) {
    d[i] = 1;

    // поиск со стиранием значения
    for (int j = 0; j < i; ++j)
      if (v[j] < v[i]) // включаем в ответ
        d[i] = max (d[i], 1 + d[j]);
  }
  cout << v;
  cout << d;
  //
}

TEST(Dyn, MaxCont) {
  // http://e-maxx.ru/algo/maximum_average_segment
  //
  // Если на каком-то этапе не возрастает, то не обязательно она потом не возрастет. Это не как в жадном
  //
  // vn in Opt
  //
  // vn not in Opt
  //
  // Решение херня!
  //
  int arr[] = {5, 15, -30, 10, -5, 40, 10, -10, -11};//, 10, -5, 40, 11};
  vector<int> v(arr, arr+sizeof arr / sizeof arr[0]);
  cout << v;

  vector<int> s(v.size(), 0);  // +1
  vector<int> d(v.size()+1, 0);
  partial_sum(v.begin(), v.end(), /*++*/s.begin());  // постоянно считаем их - сейчас считаем за раз
  cout << s;

  // s[r] - s[l-1] -> max -> нужно вычесть минимум -> min(s[0, r-1])
  // Solve
  // значение суммы может потерятья, а оно нужно для поиска мимимуму - не все берется в решение
  // sum += v[r];  // s[r] int sum = 0;
  d[0] = v[0];
  for (int r=0; r < v.size(); ++r) {
    int max_sum_r_elem = s[r] - *min_element(s.begin(), s.begin()+r);  // как считать - все равно. можно и локальными переменными
    d[r+1] = max(d[r], max_sum_r_elem);
  }

  // Reconstruct - только по dr не вышло
  int ans_l = 0, ans_r = 0;
  int value = d.back();  // не проверяю не пустоту
  for (int r=0; r < d.size() - 1; ++r) {
    // ищем последний перепад - если активности нет, до конца, то последовательность найдена
    if (d[r+1] != d[r]) {
      ans_r = r+1;  // не вкл. )
    }
  }

  ans_l = ans_r-1;
  while (true) {
    value -= v[ans_l];
    if (!value)
      break;
    --ans_l;
  }

  // Result
  cout << "Result: " << d.back() << " : ";
  assert(ans_l <= ans_r);
  cout << vector<int>(v.begin()+ans_l, v.begin()+ans_r);

  cout << d;
  //
}

// cout << x[i] << " " << delta_i << " " << delta_ni << endl;
// FIXME: А корректен ли он? Тут наврное правильнее оптимален
TEST(Dyn, Way) {
  int arr[] = {0, 5, 5, 5, 5, 50, 50, 50, 200, 300};
  vector<int> v(arr, arr+sizeof arr / sizeof arr[0]);
  cout << v;

  vector<int> x(v.size(), 0);
  vector<int> cost(v.size(), 0);

  // на жадный похож - смотрим вперед
  for (int i = 1; i < v.size(); ++i) {
    int tmp_ni = (200 - (x[i-1]+v[i]));
    int tmp_i =  (200 - (     v[i]));
    int delta_ni = tmp_ni * tmp_ni;
    int delta_i =  tmp_i  * tmp_i;

    // это мы ищем локальный оптимум?
    cost[i] = std::min(cost[i-1] + delta_i, cost[i-1] + delta_ni);

    // разбираемся с путем - включаем ли предыдущую точку в решение или нет
    if (cost[i] == cost[i-1] + delta_i) {
      // текущая точка включена
      x[i] = v[i];
    } else {
      x[i] = x[i-1] + v[i];
    }
  }

  cout << endl;
  cout << cost;
  cout << x;

  // brutforce
  //for ()
}

// http://habrahabr.ru/post/191498/
// 1D, 2D ...
//
// http://comp-science.narod.ru/WebPage/lesson2.htm
//
// LCS: http://en.wikipedia.org/wiki/Longest_common_subsequence_problem
// Наибольшая общая подпоследовательность
//
// Лекция 7. Динамическое программирование с одним параметром Михаил Густокашин
// http://neerc.ifmo.ru/wiki/index.php?title=%D0%94%D0%B8%D0%BD%D0%B0%D0%BC%D0%B8%D1%87%D0%B5%D1%81%D0%BA%D0%BE%D0%B5_%D0%BF%D1%80%D0%BE%D0%B3%D1%80%D0%B0%D0%BC%D0%BC%D0%B8%D1%80%D0%BE%D0%B2%D0%B0%D0%BD%D0%B8%D0%B5
// create
// reconstr

// Coin change
// http://www.algorithmist.com/index.php/Coin_Change


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <cmath>
#include <functional>
#include <iostream>

#include <Eigen/Dense>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

#include "examples_common.h"

/**
 * @example cartesian_impedance_control.cpp
 * An example showing a simple cartesian impedance controller without inertia shaping
 * that renders a spring damper system where the equilibrium is the initial configuration.
 * After starting the controller try to push the robot around and try different stiffness levels.
 *
 * @warning collision thresholds are set to high values. Make sure you have the user stop at hand!
 */

int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

  // Compliance parameters
  const double translational_stiffness{150.0};
  const double rotational_stiffness{10.0};
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
                                     Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
                                         Eigen::MatrixXd::Identity(3, 3);

  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();

    franka::RobotState initial_state = robot.readOnce();

    // equilibrium point is the initial position
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
    Eigen::Vector3d position_d(initial_transform.translation());
    Eigen::Quaterniond orientation_d(initial_transform.linear());

    // set collision behavior
    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

    // define callback for the torque control loop
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                         franka::Duration /*duration*/) -> franka::Torques {
      // get state variables
      std::array<double, 7> coriolis_array = model.coriolis(robot_state);
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

      // convert to Eigen
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
      Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
      Eigen::Vector3d position(transform.translation());
      Eigen::Quaterniond orientation(transform.linear());

      // compute error to desired equilibrium pose
      // position error
      Eigen::Matrix<double, 6, 1> error;
      error.head(3) << position - position_d;

      // orientation error
      // "difference" quaternion
      if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
        orientation.coeffs() << -orientation.coeffs();
      }
      // "difference" quaternion
      Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
      error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
      // Transform to base frame
      error.tail(3) << -transform.linear() * error.tail(3);

      // compute control
      Eigen::VectorXd tau_task(7), tau_d(7);

      // Spring damper system with damping ratio=1
      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
      tau_d << tau_task + coriolis;

      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
      return tau_d_array;
    };

    // start real-time control loop
    std::cout << "WARNING: Collision thresholds are set to high values. "
              << "Make sure you have the user stop at hand!" << std::endl
              << "After starting try to push the robot and see how it reacts." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(impedance_control_callback);

  } catch (const franka::Exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }

  return 0;
}


#ifndef EXPSUM_REDUCTION_CHOLESKY_CAUCHY_HPP
#define EXPSUM_REDUCTION_CHOLESKY_CAUCHY_HPP

#include <armadillo>
#include <cassert>

namespace expsum
{
//
// Rank-revealing Cholesky decomposition for a positive-definite quasi-Cauchy
// matrix.
//
// This class computes the Cholesky decomposition of ``$n \times n$``
// quasi-Cauchy matrix defined as
//
// ``` math
//  C_{ij} = \frac{a_{i}^{} b_{j}^{}}{x_{i}^{} + y_{i}^{}}.
// ```
//
template <typename T>
struct cholesky_quasi_cauchy
{
public:
    using value_type = T;
    using real_type  = typename arma::get_pod_type<T>::result;
    using size_type  = arma::uword;

    using vector_type      = arma::Col<value_type>;
    using matrix_type      = arma::Mat<value_type>;
    using real_vector_type = arma::Col<real_type>;
    //
    // Pre-compute pivot order for the Cholesky factorization of Cauchy matrix.
    //
    static size_type pivot_order(vector_type& a, vector_type& b, vector_type& x,
                                 vector_type& y, real_type delta,
                                 arma::uvec& ipiv, vector_type& g);
    //
    // Compute Cholesky factors (`X` and diagonal part of `D`).
    //
    // The arrays `a,b,x,y` must be properly reordered by calling `pivot_order`
    // beforehand, so that the diagonal part of Cholesky factors appear in
    // decesnding order.
    //
    // @a vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @b vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @x vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @y vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @L Cholesky factor (lower triangular matrix)
    // @d diagonal elements of Cholesky factor ``$D$``
    // @alpha vector of length ``$n$`` as working space
    // @beta  vector of length ``$n$`` as working space
    //
    static void factorize(const vector_type& a, const vector_type& b,
                          const vector_type& x, const vector_type& y,
                          matrix_type& L, real_vector_type& d,
                          vector_type& alpha, vector_type& beta);
    //
    // Apply permutation matrix generated by previous decomposition *in-place.*
    //
    // @X $n \times k$ matrix that the row-permutation matrix is applied.
    // @ipiv vector of index with size $n$. Permutation index obtained by
    //       `pivot_order` or `pivot_order_sym`.
    // @work working space of size $n$.
    //
    template <typename MatX>
    static void apply_row_permutation(MatX& X, const arma::uvec& ipiv,
                                      vector_type& work)
    {
        const size_type n = ipiv.size();
        assert(X.n_rows == n && work.n_elem == n);

        for (size_type j = 0; j < X.n_cols; ++j)
        {
            for (size_type i = 0; i < n; ++i)
            {
                work(ipiv(i)) = X(i, j);
            }
            X.col(j) = work;
        }
    }
    //
    // Reconstruct matrix from Cholesky factor
    //
    // @X Cholesky factor computed by `cholesky_quasi_cauchy::run`.
    // @d Cholesky factor computed by `cholesky_quasi_cauchy::run`.
    //
    static matrix_type reconstruct(const matrix_type& X, const arma::uvec& ipiv,
                                   const real_vector_type& d)
    {
        matrix_type PX(X);
        vector_type work(X.n_rows);
        apply_row_permutation(PX, ipiv, work);

        return matrix_type(PX * arma::diagmat(arma::square(d)) * PX.t());
    }
};

//------------------------------------------------------------------------------
// Implementation of member functions
//------------------------------------------------------------------------------
template <typename T>
typename cholesky_quasi_cauchy<T>::size_type
cholesky_quasi_cauchy<T>::pivot_order(vector_type& a, vector_type& b,
                                      vector_type& x, vector_type& y,
                                      real_type delta, arma::uvec& ipiv,
                                      vector_type& g)
{
    const size_type n = a.size();
    assert(b.size() == n);
    assert(x.size() == n);
    assert(y.size() == n);
    assert(ipiv.size() == n);
    assert(g.size() == n);
    //
    // Set cutoff for GECP termination
    //
    const auto eta = arma::Datum<real_type>::eps * delta * delta;
    //
    // Form vector g(i) = a(i) * b(i) / (x(i) + y(i))
    //
    g = (a % b) / (x + y);
    //
    // Initialize permutation matrix
    //
    ipiv = arma::linspace<arma::uvec>(0, n - 1, n);

    size_type m = 0;
    while (m < n)
    {
        //
        // Find m <= l < n such that |g(l)| = max_{m<=k<n}|g(k)|
        //
        const auto l    = arma::abs(g.tail(n - m)).index_max() + m;
        const auto gmax = std::abs(g(l));

        if (gmax < eta)
        {
            break;
        }

        if (l != m)
        {
            // Swap elements
            std::swap(g(l), g(m));
            std::swap(a(l), a(m));
            std::swap(b(l), b(m));
            std::swap(x(l), x(m));
            std::swap(y(l), y(m));
            // Swap _rows_ of permutation matrix
            std::swap(ipiv(l), ipiv(m));
        }

        // Update diagonal of Schur complement
        const auto xm = x(m);
        const auto ym = y(m);
        for (size_type k = m + 1; k < n; ++k)
        {
            g(k) *= (x(k) - xm) * (y(k) - ym) / ((x(k) + ym) * (y(k) + xm));
        }
        ++m;
    }
    //
    // Returns the truncation size
    //
    return m;
}

template <typename T>
void cholesky_quasi_cauchy<T>::factorize(const vector_type& a,
                                         const vector_type& b,
                                         const vector_type& x,
                                         const vector_type& y, matrix_type& L,
                                         real_vector_type& d,
                                         vector_type& alpha, vector_type& beta)
{
    const auto n = L.n_rows;
    const auto m = L.n_cols;
    assert(a.size() == n);
    assert(b.size() == n);
    assert(x.size() == n);
    assert(y.size() == n);
    assert(d.size() == m);
    assert(alpha.size() == n);
    assert(beta.size() == n);

    alpha = a;
    beta  = b;

    L.zeros();
    for (size_type l = 0; l < n; ++l)
    {
        L(l, 0) = alpha(l) * beta(0) / (x(l) + y(0));
    }

    for (size_type k = 1; k < m; ++k)
    {
        // Upgrade generators
        const auto xkm1 = x(k - 1);
        const auto ykm1 = y(k - 1);
        for (size_type l = k; l < n; ++l)
        {
            alpha(l) *= (x(l) - xkm1) / (x(l) + ykm1);
            beta(l) *= (y(l) - ykm1) / (y(l) + xkm1);
        }
        // Extract k-th column for Cholesky factors
        for (size_type l = k; l < n; ++l)
        {
            L(l, k) = alpha(l) * beta(k) / (x(l) + y(k));
        }
    }
    //
    // Scale strictly lower triangular part of G
    //   - diagonal part of G contains D**2
    //   - L = tril(G) * D^{-2} + I
    //
    for (size_type j = 0; j < m; ++j)
    {
        const auto djj   = std::real(L(j, j));
        const auto scale = real_type(1) / djj;
        d(j)             = std::sqrt(djj);
        L(j, j) = real_type(1);
        for (size_type i = j + 1; i < n; ++i)
        {
            L(i, j) *= scale;
        }
    }

    return;
}

} // namespace: expsum

#endif /* EXPSUM_REDUCTION_CHOLESKY_CAUCHY_HPP */


#include <gtest/gtest.h>
#include <vector>
#include "unit_tests.h"
#include <boost/test/unit_test.hpp>
#include "host.h"

BOOST_AUTO_TEST_CASE(circular_convolution_test)
{
	vector<float> h = {2, 2};
	vector<float> x = {1, -1, 1, 0};
	vector<float> y_linear;
	vector<float> y_circular;
	int N = x.size();
	int Nh = h.size();
	y_linear.resize(N * 2);
	y_circular.resize(N);

	vector<float> y = {2, 0, 0, 2};

	circular_convolution(x, h, y_linear, y_circular, N);

	BOOST_CHECK(y_circular == y);
}

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


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
//
#include <iostream>

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

#include "multicontact-api/geometry/linear-cone.hpp"
#include "multicontact-api/geometry/second-order-cone.hpp"

using namespace multicontact_api::geometry;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(WrenchConeTest) {
  using std::fabs;
  typedef ForceCone::Matrix3x Matrix3x;
  // typedef WrenchCone::Matrix6x Matrix6x;
  typedef ForceCone::Index Index;
  typedef ForceCone::SE3 SE3;
  // typedef ForceCone::Scalar Scalar;

  const Index size = 10;
  const Matrix3x rays(Matrix3x::Random(3, size));

  ForceCone C3(rays);
  BOOST_CHECK(C3.size() == size);
  BOOST_CHECK(C3.isApprox(C3));

  // Transform a ForceCone to a Wrench Cone
  const SE3 M1(SE3::Identity());
  WrenchCone C6 = C3.SE3ActOn(M1);
  BOOST_CHECK(C6.linear().isApprox(C3.rays()));

  const SE3 M2(SE3::Random());
  C6 = C3.SE3ActOn(M2);
  WrenchCone C6_inv = C6.SE3ActOn(M2.inverse());

  BOOST_CHECK(C6_inv.linear().isApprox(C3.rays()));

  // Export force cone to txt file
  const std::string txt_filename = "C3_in.txt";
  C3.saveAsText(txt_filename);

  ForceCone C3_from_txt(0);
  C3_from_txt.loadFromText(txt_filename);

  BOOST_CHECK(C3_from_txt.isApprox(C3));

  // Export gmm to xml file
  const std::string xml_filename = "C3_in.xml";
  C3.saveAsXML(xml_filename, "ForceCone");

  ForceCone C3_from_xml(0);
  C3_from_xml.loadFromXML(xml_filename, "ForceCone");

  BOOST_CHECK(C3_from_xml.isApprox(C3));

  // Export gmm to binary file
  const std::string bin_filename = "gmm_in.bin";
  C3.saveAsBinary(bin_filename);

  ForceCone C3_from_bin(0);
  C3_from_bin.loadFromBinary(bin_filename);

  BOOST_CHECK(C3_from_bin.isApprox(C3));
}

BOOST_AUTO_TEST_CASE(SecondOrderConeTest) {
  const double mu = 0.3;
  const SOC3d::VectorD direction(0., 0., 1.);

  SOC3d cone3 = SOC3d::RegularCone(mu, direction);

  BOOST_CHECK(cone3.check(direction));

  SOC3d::VectorD x(direction);
  x[0] = mu;
  BOOST_CHECK(cone3.check(x));

  x[0] = 0.;
  x[1] = mu;
  BOOST_CHECK(cone3.check(x));
}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file k_median_long_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-08-01
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_orlib_km.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/data_structures/facility_location/fl_algo.hpp"
#include "paal/local_search/k_median/k_median.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/irange.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/test/unit_test.hpp>

#include <iterator>
#include <iostream>
#include <fstream>
#include <iomanip>

using namespace paal::local_search;
using namespace paal;

BOOST_AUTO_TEST_CASE(KMedianLong) {
    std::string test_dir = paal::system::get_test_data_dir("KM_ORLIB");
    using paal::system::build_path;

    parse(build_path(test_dir, "capopt.txt"),
          [&](const std::string & fname, std::istream & is_test_cases) {
        double opt;
        is_test_cases >> opt;

        LOGLN("TEST " << fname);
        LOGLN(std::setprecision(20) << "OPT " << opt);

        std::ifstream ifs(build_path(test_dir, "/cases/" + fname + ".txt"));
        assert(ifs.good());

        boost::integer_range<int> fac(0, 0);
        boost::integer_range<int> clients(0, 0);
        auto metric = paal::read_orlib_KM(ifs, fac, clients);

        typedef paal::data_structures::voronoi<decltype(metric)> VorType;
        typedef paal::data_structures::k_median_solution<VorType> Sol;
        typedef paal::data_structures::voronoi_traits<VorType> VT;
        typedef typename VorType::GeneratorsSet GSet;
        typedef typename VT::VerticesSet VSet;
        typedef typename Sol::UnchosenFacilitiesSet USet;
        VorType voronoi(GSet{ fac.begin(), fac.end() },
                        VSet(fac.begin(), clients.end()), metric);
        Sol sol(std::move(voronoi), USet(clients.begin(), clients.end()),
                fac.size());
        paal::local_search::default_k_median_components swap;

        facility_location_first_improving(sol, swap);

        double c = simple_algo::get_km_cost(metric, sol);
        LOGLN("chosen (" << (sol.get_chosen_facilities()).size() << "):");
        VSet chosen = sol.get_chosen_facilities();
        LOG_COPY_RANGE_DEL(chosen, " ");
        check_result(c, opt, 5., paal::utils::less_equal(), 0.01);
    });
}


/**
 * units test
 * @author Tobias Weber <tweber@ill.fr>
 * @date jun-20
 * @license GPLv3, see 'LICENSE' file
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE La1
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/units.h"

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_equals, t_real, t_types)
{
	std::cout << "kB = " << tl2::kB<t_real> << std::endl;
	std::cout << "muB = " << tl2::muB<t_real> << std::endl;

	std::cout << "kB = "
		<< static_cast<t_real>(tl2::kB<t_real>/tl2::meV<t_real>*tl2::kelvin<t_real>)
		<< std::endl;
	std::cout << "muB = "
		<< static_cast<t_real>(tl2::muB<t_real>/tl2::meV<t_real>*tl2::tesla<t_real>)
		<< std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/geometry/strategies/transform/inverse_transformer.hpp>
#include <boost/geometry/strategies/transform/map_transformer.hpp>
#include <boost/geometry/strategies/transform/matrix_transformers.hpp>

#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/transform.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P, typename T>
void check_inverse(P const& p, T const& trans)
{
    bg::strategy::transform::inverse_transformer<P, P> inverse(trans);

    P i;
    bg::transform(p, i, inverse);

    BOOST_CHECK_CLOSE(double(bg::get<0>(i)), 1.0, 0.001);
    BOOST_CHECK_CLOSE(double(bg::get<1>(i)), 1.0, 0.001);
}

template <typename P>
void test_all()
{
    P p;
    bg::assign_values(p, 1, 1);

    {
        bg::strategy::transform::translate_transformer<P, P> trans(1, 1);
        P tp;
        bg::transform(p, tp, trans);

        BOOST_CHECK_CLOSE(double(bg::get<0>(tp)), 2.0, 0.001);
        BOOST_CHECK_CLOSE(double(bg::get<1>(tp)), 2.0, 0.001);

        check_inverse(tp, trans);
    }

    {
        bg::strategy::transform::scale_transformer<P, P> trans(10, 10);
        P tp;
        bg::transform(p, tp, trans);

        BOOST_CHECK_CLOSE(double(bg::get<0>(tp)), 10.0, 0.001);
        BOOST_CHECK_CLOSE(double(bg::get<1>(tp)), 10.0, 0.001);

        check_inverse(tp, trans);
    }

    {
        bg::strategy::transform::rotate_transformer<P, P, bg::degree> trans(90.0);
        P tp;
        bg::transform(p, tp, trans);

        BOOST_CHECK_CLOSE(double(bg::get<0>(tp)), 1.0, 0.001);
        BOOST_CHECK_CLOSE(double(bg::get<1>(tp)), -1.0, 0.001);
        check_inverse(tp, trans);
    }

    {
        // Map from 0,0,2,2 to 0,0,500,500
        bg::strategy::transform::map_transformer<P, P, false> trans
            (
                0.0, 0.0, 2.0, 2.0, 500, 500
            );
        P tp;
        bg::transform(p, tp, trans);

        BOOST_CHECK_CLOSE(double(bg::get<0>(tp)), 250.0, 0.001);
        BOOST_CHECK_CLOSE(double(bg::get<1>(tp)), 250.0, 0.001);

        check_inverse(tp, trans);
    }
}

int test_main(int, char* [])
{
    //test_all<int[2]>();
    //test_all<float[2]>();
    //test_all<double[2]>();

    test_all<boost::tuple<float, float> >();

    //test_all<point<int, 2, cs::cartesian> >();
    test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, Eigen::Dynamic, Eigen::Dynamic> test_long_D_D(const Eigen::Matrix<boost::int64_t, Eigen::Dynamic, Eigen::Dynamic> & M)
{
	return M;
}
void export_long_D_D()
{
	boost::python::def("test_long_D_D",test_long_D_D);
}


/* ****************************** NOT DONE ****************************** */
// view/functor.cc

#include <stapl/containers/array/array.hpp>
#include <stapl/views/array_view.hpp>
#include <stapl/views/functor_view.hpp>
#include <stapl/algorithms/algorithm.hpp>
#include <stapl/algorithms/functional.hpp>

#include <boost/random.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include "viewhelp.hpp"
using namespace std;

template <typename T>
struct const_gener
{
private:
  T m_value;
public:
  typedef std::size_t index_tp;
  const_gener(T value)
    : m_value(value)
  { }

  typedef T result_type;
  result_type operator()(index_tp const& idx) const
  { return m_value; }

  void define_type(stapl::typer& t)
  { t.member(m_value); }
};

template <typename T>
struct rand_gener
{
private:
  T m_value;
  std::size_t m_seed;
public:
  typedef std::size_t index_tp;
  rand_gener(T seed)
    : m_seed(seed)
  { }

  typedef T result_type;
  result_type operator()(index_tp const& idx) const
  {
    typedef boost::mt19937 engine_tp;
    typedef boost::random::uniform_real_distribution<double> dist_tp;
    typedef boost::variate_generator<engine_tp, dist_tp> gener_tp;

    engine_tp eng(idx*m_seed + idx);
    gener_tp pos_gen(eng, dist_tp(0,1));
    return pos_gen();
  }

  void define_type(stapl::typer& t)
  { t.member(m_seed); }
};

typedef int val_tp;
typedef stapl::array<val_tp> ary_int_tp;
typedef stapl::array_view<ary_int_tp> ary_int_vw_tp;

typedef stapl::plus<val_tp> add_int_wf;

stapl::exit_code stapl_main(int argc, char **argv) {

  ulong count = 40;
  // construct container and view over container
  ary_int_tp a_ct(count), b_ct(count), c_ct(count);
  ary_int_vw_tp a_vw(a_ct), b_vw(b_ct), c_vw(c_ct);

  // initialize container
  stapl::iota(a_vw, 0);

  // construct functor views
  val_tp param = 28128;
#if 0
  auto fn_con = stapl::functor_view(count, const_gener<ulong>(param));
  auto fn_rand = stapl::functor_view(count, rand_gener<ulong>(16807));
#endif

  // process elements through view
#if 0
  stapl::map_func( id_int_wf(), add_int_wf(), fn_con, a_vw, b_vw );
  stapl::map_func( id_int_wf(), add_int_wf(), rand_con, a_vw, c_vw );
#endif

  // print container elements
  stapl::stream<ofstream> zout;
  zout.open("refman_funcvw.txt");

  return EXIT_SUCCESS;
}
/* ****************************** NOT DONE ****************************** */


#include <boost/test/unit_test.hpp>

#include <k52/dsp/transform/fourier_transform.h>
#include <k52/dsp/transform/fast_fourier_transform.h>

#include  <k52/common/constants.h>

#include <cmath>

#include "../boost_test_tools_extensions.h"

using std::vector;
using std::complex;
using std::invalid_argument;
using k52::common::Constants;
using k52::dsp::IFourierTransform;
using k52::dsp::FourierTransform;
using k52::dsp::FastFourierTransform;

/**
 * Definition of test methods
 */

void test_zero(const IFourierTransform* ft)
{
    //Prepare
    size_t N = 11;

    vector< complex <double> > samples(N);

    //Test
    vector< complex <double> > result = ft->Transform(samples);

    //Check
    BOOST_REQUIRE_EQUAL(result.size(), N);

    for (size_t k = 0; k < N; ++k)
    {
        CheckComplexEqual(result[k], 0);
    }
}

void test_simple_impulse(const IFourierTransform* ft)
{
    //Prepare
    size_t N = 4;

    vector< complex <double> > samples(N);
    samples[0] = 1;

    //Test
    vector< complex <double> > result = ft->Transform(samples);

    //Check
    BOOST_REQUIRE_EQUAL(result.size(), N);

    for (size_t k = 0; k < N; ++k)
    {
        CheckComplexEqual(result[k], 1);
    }
}

void test_impulse(const IFourierTransform* ft)
{
    //Prepare
    size_t N = 11;
    size_t n0 = 2;

    vector< complex <double> > samples(N);
    samples[n0] = 1;

    //Test
    vector< complex <double> > result = ft->Transform(samples);

    //Check
    BOOST_REQUIRE_EQUAL(result.size(), N);

    for (size_t k = 0; k < N; ++k)
    {
        complex< double > w = exp( -2 * Constants::Pi * Constants::ImaginaryUnit * (double)k * (double)n0 / (double)N);

        CheckComplexEqual(result[k], w);
    }
}

void test_complex_harmonic(const IFourierTransform* ft)
{
    //Prepare
    size_t N = 128;
    size_t k0 = 5;

    vector< complex <double> > samples(N);

    for (size_t n = 0; n < N; ++n)
    {
        samples[n] = exp( 2 * Constants::Pi * Constants::ImaginaryUnit * (double)k0 * (double)n / (double)N);
    }

    //Test
    vector< complex <double> > result = ft->Transform(samples);

    //Check
    BOOST_REQUIRE_EQUAL(result.size(), N);

    for (size_t k = 0; k < N; ++k)
    {
        CheckComplexEqual(result[k], k == k0 ? N : 0);
    }
}

/**
 * Actual tests are below
 */

struct FourierTransformTestFixture
{
    FourierTransform ft;
};

BOOST_AUTO_TEST_SUITE(fourier_transform_tests);

BOOST_FIXTURE_TEST_SUITE(fourier_transform, FourierTransformTestFixture);

BOOST_AUTO_TEST_CASE(zero)
{
    test_zero(&ft);
}

BOOST_AUTO_TEST_CASE(simple_impulse)
{
    test_simple_impulse(&ft);
}

BOOST_AUTO_TEST_CASE(impulse)
{
    test_impulse(&ft);
}

BOOST_AUTO_TEST_CASE(complex_harmonic)
{
    test_complex_harmonic(&ft);
}

BOOST_AUTO_TEST_SUITE_END();

BOOST_AUTO_TEST_SUITE(fft);

BOOST_AUTO_TEST_CASE(zero_size)
{
    BOOST_REQUIRE_THROW(FastFourierTransform fft(0), invalid_argument);
}

BOOST_AUTO_TEST_CASE(not_equal_size)
{
    //Prepare
    FastFourierTransform fft(10);
    vector<complex<double> > data(12);

    //Test
    //Check
    BOOST_REQUIRE_THROW(fft.Transform(data), invalid_argument);
}

BOOST_AUTO_TEST_CASE(zero)
{
    FastFourierTransform fft(11);
    test_zero(&fft);
}

BOOST_AUTO_TEST_CASE(simple_impulse)
{
    FastFourierTransform fft(4);
    test_simple_impulse(&fft);
}

BOOST_AUTO_TEST_CASE(impulse)
{
    FastFourierTransform fft(11);
    test_impulse(&fft);
}

BOOST_AUTO_TEST_CASE(complex_harmonic)
{
    FastFourierTransform fft(128);
    test_complex_harmonic(&fft);
}

BOOST_AUTO_TEST_SUITE_END();

BOOST_AUTO_TEST_SUITE_END();


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SCHUR_SOLVER_HH
#define SCHUR_SOLVER_HH

/**
 * @file
 * @brief  Routines for the solution of (sparse) linear systems
 * @author Anton Schiela
 */

#include <iostream>
#include <memory> // std::unique_ptr

#include <boost/timer/timer.hpp>

#include "dune/common/fmatrix.hh"
#include "dune/istl/matrix.hh"

#include "linalg/umfpack_solve.hh"
#include "linalg/linearsystem.hh"
#include "linalg/simpleLAPmatrix.hh"
#include "algorithm/opt_interface.hh"
#include "algorithm/newton_bridge.hh"

namespace Kaskade
{

/**
 * \ingroup linalg
 * \brief Adapter class for DUNE::IterativeSolver
 */

void printVec(std::vector<double> const&v, int vend=1000000);

template<class Factorization=UMFFactorization<double> >
class BlockLUFactorization
{
public:
/// needs a matrix
  static const bool needMatrix = true;
template<class Sys>
BlockLUFactorization(Sys const& lin, int start2, int end2, int start3, int end3)
  {
    MatrixAsTriplet<double> matL;
    lin.getMatrixBlocks(matL,start3,end3,start2,end2);
    matA.resize(0);
    lin.getMatrixBlocks(matA,start2,end2,start2,end2);
    boost::timer::cpu_timer timer;
    factoredL.reset(new Factorization(matL.nrows(),
                                      2,
                                      matL.ridx,
                                      matL.cidx,
                                      matL.data,
                                      MatrixProperties::GENERAL));
  }

  void resetBlock22(MatrixAsTriplet<double>const & matA_) { matA = matA_; };

  void solve(std::vector<double>const& rhs, std::vector<double>& sol, int nr=1);
  MatrixAsTriplet<double> matA;
  std::unique_ptr<Factorization> factoredL;
};

void printVec(std::vector<double> const&v, int vend)
{
  int endv=v.size();
  if(vend < v.size()) endv=vend;
  for(int i=0; i< endv; ++i)
  {
    if(i%5 == 0)  std::cout << ".  " << v[i] << std::endl;
    else std::cout << "   " << v[i] << std::endl;

  }
}

template<class Factorization>
void BlockLUFactorization<Factorization>::solve(std::vector<double>const& rhs, std::vector<double>& sol, int nr)
{
  int r=rhs.size()/2/nr;
  std::vector<double> rhs1(r*nr);
  std::vector<double> rhs2(r*nr);
  std::vector<double> sol1(r*nr);
  std::vector<double> sol2(r*nr);
  for(int i=0; i<nr; ++i)
  for(int j=0; j<r; ++j)
  {
    rhs1[i*r+j]=rhs[i*2*r+j];
    rhs2[i*r+j]=-rhs[i*2*r+r+j];
  }
  factoredL->solve(rhs2,sol2,false);  // solve system
  //  factoredL->solve(rhs2,sol2,nr,false);  // solve system
  matA.axpy(rhs1,sol2,1.0,nr);
  factoredL->solve(rhs1,sol1,true);  //solve transposed system
  // factoredL->solve(rhs1,sol1,nr,true);  //solve transposed system
  sol.resize(2*r*nr);
  for(int i=0; i<nr; ++i)
  for(int j=0; j<r; ++j)
  {
    sol[i*2*r+j]=-sol2[i*r+j];
    sol[i*2*r+r+j]=sol1[i*r+j];
  }
};

struct BlockSchurParameters
{
  BlockSchurParameters(bool reg_)
    : refactorizeInner(true), refactorizeOuter(true)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  BlockSchurParameters(bool reg_, bool innerf_, bool outerf_)
    : refactorizeInner(innerf_), refactorizeOuter(outerf_)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  bool refactorizeInner, refactorizeOuter;
  typedef enum { None=0, AddId=1, IterateType::CG = 2} RegularizationMethod;

  RegularizationMethod regularizationMethod;
};

template<class Factorization, class VariableSet>
class DirectBlockSchurSolver : public AbstractNormalDirection, public Dune::Preconditioner<typename VariableSet::Descriptions::template CoefficientVectorRepresentation<>::type,
 typename VariableSet::Descriptions::template CoefficientVectorRepresentation<>::type>
{
public:
  typedef typename VariableSet::Descriptions::template CoefficientVectorRepresentation<>::type Domain;
  typedef Domain Range;

  int start1, end1, start2, end2, start3, end3;

/// needs a matrix
  static const bool needMatrix = true;

  DirectBlockSchurSolver(bool doregularize = false) :
    start1(0), end1(1),
    start2(1), end2(2),
    start3(2), end3(3),
    report(false), paras(doregularize)
  {}

  virtual void pre(Domain &x, Range &b) {}

  virtual void apply (Domain &v, const Range &d) {

    std::vector<double> r(rows1),s(rows2),t(rows3), sol1(rows1+rows2+rows3);

    d.write(sol1.begin());

    for(int i=0; i<rows1; ++i) r[i]=sol1[i];
    for(int i=0; i<rows2; ++i) s[i]=sol1[i+rows1];
    for(int i=0; i<rows3; ++i) t[i]=sol1[i+rows1+rows2];

    resolveN(sol1,r,s,t);

    v.read(sol1.begin());
  }
  virtual void post (Domain &x) {}

  void onChangedLinearization() {flushFactorization(); }

  void flushFactorization()
  {
    if(factorization.get() && paras.refactorizeInner) factorization.reset();
    mC.setSize(0,0);
    B.resize(0);
    AinvB.resize(0);
    matANormal.flush();
  }

  bool report;

  void resetParameters(BlockSchurParameters const& p_) { paras=p_; }

  virtual void computeCorrectionAndAdjointCorrection(AbstractVector& correction, AbstractVector& adjointCorrection, AbstractLinearization& linearization)
  {
    SparseLinearSystem& lins=dynamic_cast<SparseLinearSystem &>(linearization);
    flushFactorization();
    buildNewSchurComplement(lins);

    std::vector<double> r,s,t, sol1, sol2;

    lins.getRHSBlocks(r,start1,end1);
    lins.getRHSBlocks(s,start2,end2);
    lins.getRHSBlocks(t,start3,end3);

    std::vector<double> r0(r.size(),0.0),s0(s.size(),0.0),t0(t.size(),0.0);
    resolveN(sol1,r0,s0,t);
    resolveN(sol2,r,s,t0);

    dynamic_cast<Bridge::Vector<VariableSet>& >(correction).read(sol1);
    dynamic_cast<Bridge::Vector<VariableSet>& >(adjointCorrection).read(sol2);
    correction *= -1.0;
    adjointCorrection *= -1.0;
  }

  virtual void computeSimplifiedCorrection(AbstractVector& correction, AbstractLinearization const& lin) const
  {
    SparseLinearSystem const& lins=dynamic_cast<SparseLinearSystem const&>(lin);

    std::vector<double> t, sol1;
    lins.getRHSBlocks(t,start3,end3);
    std::vector<double> r0(rows1,0.0),s0(rows2,0.0);
    resolveN(sol1,r0,s0,t);
    dynamic_cast<Bridge::Vector<VariableSet>& >(correction).read(sol1);
    correction *= -1.0;
  }

private:

  void resolveN(std::vector<double>& sol, std::vector<double>const &rhs0,std::vector<double>const &rhs1, std::vector<double>const &rhs2) const;

  void fwd(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;
  void bwd(std::vector<double>& sol, std::vector<double>const &x2,std::vector<double>const &s,std::vector<double>const &t) const;

  void buildNewSchurComplement(SparseLinearSystem const& lin,int task);

  std::unique_ptr<BlockLUFactorization<Factorization> > factorization; //
  MatrixAsTriplet<double> matANormal;
  Dune::Matrix<Dune::FieldMatrix<double,1,1> > mC,mCNormal;
  std::vector<double> B,AinvB;

  int rowsB, colsBC, rowsC;
  int rows1, rows2, rows3;

  BlockSchurParameters paras;

};

//  UU UY BU* ...  C B* B*
//  YU YY AY* ...  B A  A
//  BU AY 00  ...  B A  A
//  .. .. ..  ...
template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization, VariableSet>::buildNewSchurComplement(SparseLinearSystem const& lin,int task=0)
{
    boost::timer::cpu_timer timer;
    if(report) std::cout << "Schur Complement: " << std::flush;

    MatrixAsTriplet<double> matB, matC;
    rowsB=lin.rows(start2,end3);
    colsBC=lin.cols(start1,end1);
    rowsC=lin.rows(start1,end1);
    if(task==0)
    {
      rows1=lin.rows(start1,end1);
      rows2=lin.rows(start2,end2);
      rows3=lin.rows(start3,end3);
    }

    if(paras.refactorizeOuter || !factorization.get() || B.size()==0)
    {
      if(report) std::cout << "Outer, " << std::flush;
      if(task==0)
      {
        flushFactorization();
      }

      if((task==0 && paras.refactorizeInner) || !factorization.get())
      {
        factorization.reset(new BlockLUFactorization<Factorization>(lin,start2,end2,start3,end3));
        lin.getMatrixBlocks(matANormal,start2,end2, start2, end2);
      }
      else
      {
        MatrixAsTriplet<double> matA;
        lin.getMatrixBlocks(matA,start2,end2, start2, end2);
        factorization->resetBlock22(matA);
      }

// Matrix = [C B^T; B A]; rhs=[r2, r1]; sol=[x_2,x_1]
// Compute sol via the Schur complement
// B is stored in column-first formal in a vector B(i,j)=B[i+j*rowsB]
      if(task==0 || B.size()==0)
      {
        matB.resize(0);
        lin.getMatrixBlocks(matB,start2,end3, start1, end1);
        B.resize(0);
        matB.toVector(B);
      }

// Compute A^{-1}B
      AinvB.resize(0);
      factorization->solve(B,AinvB,colsBC);

    }

// Compute C as a full matrix
    matC.resize(0);
    lin.getMatrixBlocks(matC,start1,end1, start1, end1);
    mC.setSize(rowsC, colsBC);
    mC=0.0;
    matC.addToMatrix(mC);

// Compute S := B^T A^{-1}B-C
    for(int i=0; i<rowsC; ++i)
      for(int j=0; j<colsBC; ++j)
      {
        mC[i][j] *= -1.0;
        for(int k=0; k<rowsB; ++k) mC[i][j] += B[i*rowsB+k]*AinvB[j*rowsB+k];
      }
    if(task==0)
    {
      mCNormal.setSize(rowsC, colsBC);
      mCNormal = mC;
    }
    if(report) std::cout << "Finished: " << (double)(timer.elapsed().user)/1e9 << " sec." << std::endl;
}

template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization,VariableSet>::resolveN(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const
{
  std::vector<double> xx2;

  fwd(xx2,r,s,t);

  std::vector<double> x2(xx2.size(),0.0);

  // Compute x2 := S^{-1}xx_2

  LeastSquares(SLAPMatrix<double>(mCNormal), xx2, x2);

  bwd(sol,x2,s,t);
}

template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization,VariableSet>::fwd(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const
{
    std::vector<double> xx1, r1;

    r1.reserve(s.size()+t.size());

    for(int i=0; i<s.size();++i) r1.push_back(s[i]);
    for(int i=0; i<t.size();++i) r1.push_back(t[i]);
// Compute xx1=A^{-1}r1

    xx1.resize(r1.size());
    factorization->solve(r1,xx1);

// Compute sol := -r2+B^T xx1

    sol.resize(r.size());
    for(int i=0; i<r.size(); ++i)
    {
      sol[i]=-r[i];
      for(int k=0; k<xx1.size(); ++k)
        sol[i]+=B[i*rowsB+k]*xx1[k];
    }
}

template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization,VariableSet>::bwd
(std::vector<double>& sol, std::vector<double>const &x2,std::vector<double>const &s,std::vector<double>const &t) const
{
  std::vector<double> xx1,x1;

   xx1.reserve(s.size()+t.size());

    for(int i=0; i<s.size();++i) xx1.push_back(s[i]);
    for(int i=0; i<t.size();++i) xx1.push_back(t[i]);

// Compute xx1 := r1-B x2
    for(int i=0; i<xx1.size(); ++i)
      for(int k=0; k<x2.size(); ++k)
        xx1[i]-=B[k*rowsB+i]*x2[k];

// Compute x1 = A^{-1}xx1
    factorization->solve(xx1,x1);

// sol=[x2;x1]
    sol.reserve(x2.size()+x1.size());//x2.size()+x1.size());
    sol.resize(x2.size()+x1.size(),0.0);
    int k=0;
    for(int i=0; i< x2.size(); ++i)
    {
      sol[k]=x2[i];
      ++k;
    }
    for(int i=0; i< x1.size(); ++i)
    {
      sol[k]=x1[i];
      ++k;
    }
}

}  // namespace Kaskade
#endif


#include <boost/test/unit_test.hpp>

#include <fc/crypto/dh.hpp>
#include <fc/exception/exception.hpp>

BOOST_AUTO_TEST_SUITE(fc_crypto)

BOOST_AUTO_TEST_CASE(dh_test)
{
    fc::diffie_hellman alice;
    BOOST_CHECK( alice.generate_params( 128, 5 ) );
    BOOST_CHECK( alice.generate_pub_key() );

    fc::diffie_hellman bob;
    bob.p = alice.p;
    BOOST_CHECK( bob.validate() );
    BOOST_CHECK( bob.generate_pub_key() );

    fc::diffie_hellman charlie;
    BOOST_CHECK( !charlie.validate() );
    BOOST_CHECK( !charlie.generate_pub_key() );
    charlie.p = alice.p;
    BOOST_CHECK( charlie.validate() );
    BOOST_CHECK( charlie.generate_pub_key() );

    BOOST_CHECK( alice.compute_shared_key( bob.pub_key ) );
    BOOST_CHECK( bob.compute_shared_key( alice.pub_key ) );
    BOOST_CHECK_EQUAL( alice.shared_key.size(), bob.shared_key.size() );
    BOOST_CHECK( !memcmp( alice.shared_key.data(), bob.shared_key.data(), alice.shared_key.size() ) );
    std::vector<char> alice_bob = alice.shared_key;

    BOOST_CHECK( alice.compute_shared_key( charlie.pub_key ) );
    BOOST_CHECK( charlie.compute_shared_key( alice.pub_key ) );
    BOOST_CHECK_EQUAL( alice.shared_key.size(), charlie.shared_key.size() );
    BOOST_CHECK( !memcmp( alice.shared_key.data(), charlie.shared_key.data(), alice.shared_key.size() ) );
    std::vector<char> alice_charlie = alice.shared_key;

    BOOST_CHECK( charlie.compute_shared_key( bob.pub_key ) );
    BOOST_CHECK( bob.compute_shared_key( charlie.pub_key ) );
    BOOST_CHECK_EQUAL( charlie.shared_key.size(), bob.shared_key.size() );
    BOOST_CHECK( !memcmp( charlie.shared_key.data(), bob.shared_key.data(), bob.shared_key.size() ) );
    std::vector<char> bob_charlie = charlie.shared_key;

    BOOST_CHECK_EQUAL( alice_bob.size(), alice_charlie.size() );
    BOOST_CHECK( memcmp( alice_bob.data(), alice_charlie.data(), alice_bob.size() ) );

    BOOST_CHECK_EQUAL( alice_bob.size(), bob_charlie.size() );
    BOOST_CHECK( memcmp( alice_bob.data(), bob_charlie.data(), alice_bob.size() ) );

    BOOST_CHECK_EQUAL( alice_charlie.size(), bob_charlie.size() );
    BOOST_CHECK( memcmp( alice_charlie.data(), bob_charlie.data(), alice_charlie.size() ) );

    alice.p.clear(); alice.p.push_back(100); alice.p.push_back(2);
    BOOST_CHECK( !alice.validate() );
    alice.p = bob.p;
    alice.g = 9;
    BOOST_CHECK( !alice.validate() );

// It ain't easy...
//    alice.g = 2;
//    BOOST_CHECK( alice.validate() );
//    BOOST_CHECK( alice.generate_pub_key() );
//    BOOST_CHECK( alice.compute_shared_key( bob.pub_key ) );
//    BOOST_CHECK( bob.compute_shared_key( alice.pub_key ) );
//    BOOST_CHECK_EQUAL( alice.shared_key.size(), bob.shared_key.size() );
//    BOOST_CHECK( memcmp( alice.shared_key.data(), bob.shared_key.data(), alice.shared_key.size() ) );
}

BOOST_AUTO_TEST_SUITE_END()


#include "SODSOSystem.h"

#include "IOWrapper/ImageDisplay.h"
#include "stdio.h"
#include "util/globalCalib.h"
#include "util/globalFuncs.h"
#include <Eigen/LU>
#include <algorithm>

#include "FullSystem/ImmaturePoint.h"
#include "math.h"
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>

namespace dso {

PointHessian *
SODSOSystem::optimizeImmaturePoint(ImmaturePoint *point, int minObs,
                                   ImmaturePointTemporaryResidual *residuals) {
  int nres = 0;
  for (FrameHessian *fh : frameHessians) {
    if (fh != point->host) {
      residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0;
      residuals[nres].state_NewState = ResState::OUTLIER;
      residuals[nres].state_state = ResState::IN;
      residuals[nres].target = fh;
      nres++;
    }
  }
  assert(nres == ((int)frameHessians.size()) - 1);

  bool print = false; // rand()%50==0;

  float lastEnergy = 0;
  float lastHdd = 0;
  float lastbd = 0;
  float currentIdepth = (point->idepth_max + point->idepth_min) * 0.5f;

  for (int i = 0; i < nres; i++) {
    lastEnergy += point->linearizeResidual(&Hcalib, 1000, residuals + i,
                                           lastHdd, lastbd, currentIdepth);
    residuals[i].state_state = residuals[i].state_NewState;
    residuals[i].state_energy = residuals[i].state_NewEnergy;
  }

  if (!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act) {
    if (print)
      printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
             nres, lastHdd, lastEnergy);
    return 0;
  }

  if (print)
    printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial "
           "Id=%f\n",
           nres, lastHdd, lastEnergy, currentIdepth);

  float lambda = 0.1;
  for (int iteration = 0; iteration < setting_GNItsOnPointActivation;
       iteration++) {
    float H = lastHdd;
    H *= 1 + lambda;
    float step = (1.0 / H) * lastbd;
    float newIdepth = currentIdepth - step;

    float newHdd = 0;
    float newbd = 0;
    float newEnergy = 0;
    for (int i = 0; i < nres; i++)
      newEnergy += point->linearizeResidual(&Hcalib, 1, residuals + i, newHdd,
                                            newbd, newIdepth);

    if (!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act) {
      if (print)
        printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
               nres, newHdd, lastEnergy);
      return 0;
    }

    if (print)
      printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n",
             (true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT", iteration,
             log10(lambda), "", lastEnergy, newEnergy, newIdepth);

    if (newEnergy < lastEnergy) {
      currentIdepth = newIdepth;
      lastHdd = newHdd;
      lastbd = newbd;
      lastEnergy = newEnergy;
      for (int i = 0; i < nres; i++) {
        residuals[i].state_state = residuals[i].state_NewState;
        residuals[i].state_energy = residuals[i].state_NewEnergy;
      }

      lambda *= 0.5;
    } else {
      lambda *= 5;
    }

    if (fabsf(step) < 0.0001 * currentIdepth)
      break;
  }

  if (!std::isfinite(currentIdepth)) {
    printf("MAJOR ERROR! point idepth is nan after initialization (%f).\n",
           currentIdepth);
    return (PointHessian *)((
        long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems.
  }

  int numGoodRes = 0;
  for (int i = 0; i < nres; i++)
    if (residuals[i].state_state == ResState::IN)
      numGoodRes++;

  if (numGoodRes < minObs) {
    if (print)
      printf("OptPoint: OUTLIER!\n");
    return (PointHessian *)((
        long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems.
  }

  PointHessian *p = new PointHessian(point, &Hcalib);
  if (!std::isfinite(p->energyTH)) {
    delete p;
    return (PointHessian *)((long)(-1));
  }

  p->lastResiduals[0].first = 0;
  p->lastResiduals[0].second = ResState::OOB;
  p->lastResiduals[1].first = 0;
  p->lastResiduals[1].second = ResState::OOB;
  p->setIdepthZero(currentIdepth);
  p->setIdepth(currentIdepth);
  p->setPointStatus(PointHessian::ACTIVE);

  for (int i = 0; i < nres; i++)
    if (residuals[i].state_state == ResState::IN) {
      PointFrameResidual *r =
          new PointFrameResidual(p, p->host, residuals[i].target);
      r->state_NewEnergy = r->state_energy = 0;
      r->state_NewState = ResState::OUTLIER;
      r->setState(ResState::IN);
      p->residuals.push_back(r);

      if (r->target == frameHessians.back()) {
        p->lastResiduals[0].first = r;
        p->lastResiduals[0].second = ResState::IN;
      } else if (r->target == (frameHessians.size() < 2
                                   ? 0
                                   : frameHessians[frameHessians.size() - 2])) {
        p->lastResiduals[1].first = r;
        p->lastResiduals[1].second = ResState::IN;
      }
    }

  if (print)
    printf("point activated!\n");

  statistics_numActivatedPoints++;
  return p;
}

} // namespace dso


//
//  main_keyframeCalib.cpp
//  VideoCalibVXL
//
//  Created by jimmy on 7/23/17.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#if 1

#include <stdlib.h>
#include <time.h>
#include "cvxImage_310.hpp"
#include "eigenVLFeatSIFT.h"
#include "eigenFlann.h"
#include "cvxImgMatch.h"
#include "perspective_camera.h"
#include <Eigen/Geometry>
#include "rotation_3d.h"
#include "util.h"

static void help()
{
    printf("usage:   ./program             refFrames images \n");
    printf("example: ./referenceFrameCalib /*.txt    /*.jpg      \n");
    printf("refFrames: reference frame location and ground truth camera\n");
    printf("images: \n");
    printf("Using vlfeat sift with fixed parameter\n");
    printf("keyframeFolder: ./abc/*.txt\n");
    printf("save to ./result/*.txt folder\n");
    printf("Assumption: reference frames and images are from the same PTZ camera\n");
}

using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Matrix3d;
using Eigen::AngleAxisd;

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

    if (argc != 3)
    {
        help();
        printf("parameter number is %d, should be 2.\n", argc);
        return -1;
    }

    // get parameters
    const char *referenceFrameFolder = argv[1];
    const char *testImageFolder = argv[2];

    /*
     //debug
    const char *referenceFrameFolder = "./reference_frames/*.txt";
    const char *testImageFolder = "./images/*.jpg";
	*/

    // Step 1: load reference frames and testing images
    vector<string> refFrameNames;
    readFilenames(referenceFrameFolder, refFrameNames);

    // find most similar keyframe
    vector<cv::Mat> refFrames;
    vector<cvx::perspective_camera> refCameras;
    for (int i = 0; i<refFrameNames.size(); i++) {
        string imagename;
        //vpgl_perspective_camera<double> camera;
        cvx::perspective_camera camera;
        readCamera(refFrameNames[i].c_str(), imagename, camera);
       refCameras.push_back(camera);
        cv::Mat image = cv::imread(imagename.c_str());
        assert(!image.empty());
        refFrames.push_back(image);
    }

    printf("load %lu key frames\n", refFrames.size());

    vector<string> test_image_names;
    readFilenames(testImageFolder, test_image_names);
    printf("test image number %lu\n", test_image_names.size());

    // Step 2: detect SIFT feature from reference frames
    // SIFT feature for keyframes
    vl_feat_sift_parameter sift_para;
    sift_para.edge_thresh = 20;
    sift_para.peak_thresh = 1.0;
    sift_para.magnif      = 3.0;
    sift_para.dim = 128;

    vector< vector< std::shared_ptr<sift_keypoint> > > refFrameFeaturesVec(refFrames.size());
    for (int i = 0; i<refFrames.size(); i++) {
        EigenVLFeatSIFT::extractSIFTKeypoint(refFrames[i], sift_para, refFrameFeaturesVec[i]);
    }

    // Step 3: calibrate testing images

    for (int i = 0; i<test_image_names.size(); i += 1) {
        // Step 3.1: detect sift feature in the testing image
        string cur_image_name = test_image_names[i].c_str();
        cv::Mat queryImage = cv::imread(cur_image_name.c_str());
        assert(!queryImage.empty());
        vector< std::shared_ptr<sift_keypoint> > queryImageFeatures;
        EigenVLFeatSIFT::extractSIFTKeypoint(queryImage, sift_para, queryImageFeatures);

        // loop all reference frames, choose one with the largest matching numbers
		double tt = clock();
        int max_inlier = 0;
        cvx::perspective_camera finalCamera;
        for (int j = 0; j<refFrameFeaturesVec.size(); j++) {
            vector<std::shared_ptr<sift_keypoint> > src_keypoints = refFrameFeaturesVec[j];
            vector<std::shared_ptr<sift_keypoint> > dst_keypoints = queryImageFeatures;

            // SIFT matching and ransac filter
            vector<cv::Point2d> src_pts;
            vector<cv::Point2d> dst_pts;
            cvx::SIFTMatchingParameter matching_param;
            cvx::SIFTMatching(src_keypoints, dst_keypoints, matching_param, src_pts, dst_pts);

            Mat mask;
            Mat H = cv::findHomography(src_pts, dst_pts, mask, CV_RANSAC, 3.0);
            assert(mask.rows == src_pts.size());

            vector<Vector2d > inlier_src_pts;
            vector<Vector2d > inlier_dst_pts;
            int n_inlier = 0;
            for (int k = 0; k<src_pts.size(); k++) {
                if ((int)(mask.at<uchar>(k, 0)) == 1) {
                    inlier_src_pts.push_back(Vector2d(src_pts[k].x, src_pts[k].y));
                    inlier_dst_pts.push_back(Vector2d(dst_pts[k].x, dst_pts[k].y));
                    n_inlier++;
                }
            }

            printf("inlier number %d\n", n_inlier);
            if (n_inlier > max_inlier && n_inlier >= 4) {
                // assume the cameras has the same location (only rotate)
                cvx::perspective_camera curCamera;
                bool isCalib = calibratePureRotateCamera(inlier_src_pts, inlier_dst_pts, refCameras[j], curCamera);
                if (isCalib) {
                    max_inlier = n_inlier;
                    finalCamera = curCamera;
                }
            }
        }
		printf("Camera pose estimation cost time: %f seconds.\n", (clock() - tt)/CLOCKS_PER_SEC);
        if (max_inlier >= 4) {
            char save_name[1024] = {NULL};
            sprintf(save_name, "./result/%08d.txt", i);
            writeCamera(save_name, cur_image_name.c_str(), finalCamera);
        }
    }

    return 0;
}

#endif


/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/termstructures/blackvolsurfacedelta.hpp>

using namespace boost::unit_test_framework;
using namespace QuantLib;
using namespace QuantExt;
using namespace std;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(BlackVolSurfaceDeltaTest)

BOOST_AUTO_TEST_CASE(testBlackVolSurfaceDeltaConstantVol) {

    BOOST_TEST_MESSAGE("Testing QuantExt::BlackVolatilitySurfaceDelta...");

    Volatility constVol = 0.10; // 10%

    Date refDate(1, Jan, 2010);
    Settings::instance().evaluationDate() = refDate;

    // Setup a 2x2
    vector<Date> dates = { Date(1, Jan, 2011), Date(1, Jan, 2012) };
    vector<Real> putDeltas = { -0.25 };
    vector<Real> callDeltas = { 0.25 };
    bool hasAtm = false;
    Matrix blackVolMatrix(2, 2, constVol);

    // dummy spot and zero yield curve
    Handle<Quote> spot(boost::make_shared<SimpleQuote>(1.0));
    Handle<YieldTermStructure> dts(boost::make_shared<FlatForward>(0, TARGET(), 0.011, ActualActual()));
    Handle<YieldTermStructure> fts(boost::make_shared<FlatForward>(0, TARGET(), 0.012, ActualActual()));

    // build a vol surface
    BOOST_TEST_MESSAGE("Build Surface");
    BlackVolatilitySurfaceDelta surface(refDate, dates, putDeltas, callDeltas, hasAtm, blackVolMatrix, ActualActual(),
                                        TARGET(), spot, dts, fts);

    // ask for volatility at lots of points, should be constVol at every point
    // make sure we ask for vols outside 25D and 2Y
    for (Time t : { 0.25, 0.5, 1.0, 1.5, 2.0, 2.5, 10.0 }) {
        for (Real k = 0.5; k < 2.0; k += 0.05) {
            Volatility vol = surface.blackVol(t, k);
            BOOST_CHECK_EQUAL(vol, constVol);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-bitwise
    This function object returns the integer part of the base 2
    logarithm of the input.

    @par Header <boost/simd/function/ilog2.hpp>

    @par Note
    This function asserts for negative or null values.

    @see  twopower, exponent

    @par Example:

      @snippet ilog2.cpp ilog2

    @par Possible output:

      @snippet ilog2.txt ilog2

  **/
  as_integer_t<Value> ilog2(Value const& x);
} }
#endif

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

#endif


/**
 * \ file StereoUniversalFixedDelayLineFilter.cpp
 */

#include <ATK/Delay/StereoUniversalFixedDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

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

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_linel100_delay50_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::StereoUniversalFixedDelayLineFilter<float> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay_ch1(50);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(int64_t i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_linel25_delay24_blend_1_feedforward_1_feedback_0_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::StereoUniversalFixedDelayLineFilter<float> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay_ch1(24);
  filter.set_blend_ch1(1);
  filter.set_feedback_ch1_ch1(0);
  filter.set_feedforward_ch1_ch1(1);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_linel25_delay24_blend_0_feedforward_0_feedback_1_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::StereoUniversalFixedDelayLineFilter<float> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay_ch1(24);
  filter.set_blend_ch1(0);
  filter.set_feedback_ch1_ch1(1);
  filter.set_feedforward_ch1_ch1(0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_liner100_delay50_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::StereoUniversalFixedDelayLineFilter<float> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay_ch2(50);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 1);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(int64_t i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_liner25_delay24_blend_1_feedforward_1_feedback_0_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::StereoUniversalFixedDelayLineFilter<float> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay_ch2(24);
  filter.set_blend_ch2(1);
  filter.set_feedback_ch2_ch2(0);
  filter.set_feedforward_ch2_ch2(1);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 1);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}

BOOST_AUTO_TEST_CASE( StereoUniversalFixedDelayLineFilter_sinus_liner25_delay24_blend_0_feedforward_0_feedback_1_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::StereoUniversalFixedDelayLineFilter<float> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  filter.set_delay_ch2(24);
  filter.set_blend_ch2(0);
  filter.set_feedback_ch2_ch2(1);
  filter.set_feedforward_ch2_ch2(0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 1);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 0.0001f);
  }
}


#ifndef BAYES_CLASSIFIER_HPP
#define BAYES_CLASSIFIER_HPP

#include <cmath>

#include <utility>
#include <vector>

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

namespace osrm
{
namespace engine
{
namespace map_matching
{

struct NormalDistribution
{
    NormalDistribution(const double mean, const double standard_deviation)
        : mean(mean), standard_deviation(standard_deviation)
    {
    }

    // FIXME implement log-probability version since it's faster
    double Density(const double val) const
    {
        using namespace boost::math::constants;

        const double x = val - mean;
        return 1.0 / (std::sqrt(two_pi<double>()) * standard_deviation) *
               std::exp(-x * x / (standard_deviation * standard_deviation));
    }

    double mean;
    double standard_deviation;
};

struct LaplaceDistribution
{
    LaplaceDistribution(const double location, const double scale)
        : location(location), scale(scale)
    {
    }

    // FIXME implement log-probability version since it's faster
    double Density(const double val) const
    {
        const double x = std::abs(val - location);
        return 1.0 / (2. * scale) * std::exp(-x / scale);
    }

    double location;
    double scale;
};

template <typename PositiveDistributionT, typename NegativeDistributionT, typename ValueT>
class BayesClassifier
{
  public:
    enum class ClassLabel : unsigned
    {
        NEGATIVE,
        POSITIVE
    };
    using ClassificationT = std::pair<ClassLabel, double>;

    BayesClassifier(PositiveDistributionT positive_distribution,
                    NegativeDistributionT negative_distribution,
                    const double positive_apriori_probability)
        : positive_distribution(std::move(positive_distribution)),
          negative_distribution(std::move(negative_distribution)),
          positive_apriori_probability(positive_apriori_probability),
          negative_apriori_probability(1. - positive_apriori_probability)
    {
    }

    // Returns label and the probability of the label.
    ClassificationT classify(const ValueT &v) const
    {
        const double positive_postpriori =
            positive_apriori_probability * positive_distribution.Density(v);
        const double negative_postpriori =
            negative_apriori_probability * negative_distribution.Density(v);
        const double norm = positive_postpriori + negative_postpriori;

        if (positive_postpriori > negative_postpriori)
        {
            return std::make_pair(ClassLabel::POSITIVE, positive_postpriori / norm);
        }

        return std::make_pair(ClassLabel::NEGATIVE, negative_postpriori / norm);
    }

  private:
    PositiveDistributionT positive_distribution;
    NegativeDistributionT negative_distribution;
    double positive_apriori_probability;
    double negative_apriori_probability;
};
}
}
}

#endif // BAYES_CLASSIFIER_HPP


//
// Created by Alex Beccaro on 24/01/2019.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/131/problem131.hpp"

BOOST_AUTO_TEST_SUITE( Problem131 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem131::solve(100);
        BOOST_CHECK_EQUAL(res, 4);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem131::solve();
        BOOST_CHECK_EQUAL(res, 173);
    }

BOOST_AUTO_TEST_SUITE_END()