#include <iostream>
#include <cassert>
#include <time.h>
#include <cstdint>
#include <stdexcept>
#include <chrono>
#include <limits.h>

#include <NTL/ZZX.h>

#include "FINAL.h"

using namespace std;
using namespace NTL;

void test_params()
{
    {
        Param param(LWE);
        cout << "Ciphertext modulus of the base scheme (LWE): " << param.q_base << endl;
        cout << "Dimension of the base scheme (LWE): " << param.n << endl;
        cout << "Ciphertext modulus for bootstrapping (LWE): " << q_boot << endl;
        cout << "Polynomial modulus (LWE): " << Param::get_def_poly() << endl;
        assert(param.l_ksk == int(ceil(log(double(param.q_base))/log(double(Param::B_ksk)))));
        cout << "Decomposition length for key-switching (LWE): " << param.l_ksk << endl;
        cout << "Decomposition bases for key-switching (LWE): " << Param::B_ksk << endl;
        cout << "Dimension for bootstrapping (LWE): " << Param::N << endl;
        cout << "Decomposition bases for bootstrapping (LWE): ";
        for (const auto &v: param.B_bsk) cout << v << ' ';
        cout << endl;
        cout << "Delta (LWE): " << param.delta_base << endl;
        cout << "Half Delta (LWE): " << param.half_delta_base << endl;
    }
    {
        Param param(NTRU);
        cout << "Ciphertext modulus of the base scheme (MNTRU): " << param.q_base << endl;
        cout << "Dimension of the base scheme (NTRU): " << param.n << endl;
        cout << "Ciphertext modulus for bootstrapping (NTRU): " << q_boot << endl;
        cout << "Polynomial modulus (NTRU): " << Param::get_def_poly() << endl;
        assert(param.l_ksk == int(ceil(log(double(param.q_base))/log(double(Param::B_ksk)))));
        cout << "Decomposition length for key-switching (MNTRU): " << param.l_ksk << endl;
        cout << "Decomposition bases for key-switching (MNTRU): " << Param::B_ksk << endl;
        cout << "Dimension for bootstrapping (MNTRU): " << Param::N << endl;
        cout << "Decomposition bases for bootstrapping (MNTRU): ";
        for (const auto &v: param.B_bsk) cout << v << ' ';
        cout << endl;
        cout << "Decomposition lengths for bootstrapping (MNTRU): ";
        for (int i = 0; i < Param::B_bsk_size; i++)
        {
            assert(param.l_bsk[i] == int(ceil(log(double(q_boot))/log(double(param.B_bsk[i])))));
            cout << param.l_bsk[i] << ' ';
        }
        cout << endl;
        cout << "Decomposition lengths for bootstrapping (MNTRU): ";
        for (int i = 0; i < Param::B_bsk_size; i++)
        {
            assert(param.l_bsk[i] == int(ceil(log(double(q_boot))/log(double(param.B_bsk[i])))));
            cout << param.l_bsk[i] << ' ';
        }
        cout << endl;
        cout << "Delta (MNTRU): " << param.delta_base << endl;
        cout << "Half Delta (MNTRU): " << param.half_delta_base << endl;

        {
            assert(0L == mod_q_boot(0L));
            assert(1L == mod_q_boot(1L));
            assert(0L == mod_q_boot(q_boot));
            assert(half_q_boot == mod_q_boot(half_q_boot));
            assert(-half_q_boot == mod_q_boot(-half_q_boot));
            cout << "MODULO REDUCTION IS OK" << endl;
        }
    }

    cout << "Plaintext modulus: " << Param::t << endl;
    cout << endl;
    cout << "PARAMS ARE OK" << endl;

    {
        vector<int> res;
        decompose(res, 0, 2, 3);
        assert(res.size() == 3);
        for (auto iter=res.begin(); iter < res.end(); iter++)
            assert(0L == *iter);
    }
    {
        vector<int> res;
        decompose(res, 1, 2, 3);
        assert(res.size() == 3);
        assert(res[0] == 1);
        for (auto iter=res.begin()+1; iter < res.end(); iter++)
            assert(0L == *iter);
    }
    {
        vector<int> res;
        decompose(res, 2, 3, 3);
        assert(res.size() == 3);
        assert(res[0] == -1 && res[1] == 1 && res[2] == 0);
    }
    {
        vector<int> res;
        decompose(res, 2, 4, 3);
        assert(res.size() == 3);
        assert(res[0] == 2 && res[1] == 0 && res[2] == 0);
        decompose(res, 3, 4, 3);
        assert(res.size() == 3);
        assert(res[0] == -1 && res[1] == 1 && res[2] == 0);
    }
    {
        vector<int> res;
        try
        {
            decompose(res, 14, 3, 3);
            assert(false);
        }
        catch (overflow_error)
        {
            assert(true);
        }
    }
    {
        vector<int> res;
        try
        {
            decompose(res, -14, 3, 3);
            assert(false);
        }
        catch (overflow_error)
        {
            assert(true);
        }
    }
    {
        vector<int> res;
        decompose(res, 13, 3, 3);
        assert(res.size() == 3);
        assert(res[0] == 1 && res[1] == 1 && res[2] == 1);
        decompose(res, -13, 3, 3);
        assert(res.size() == 3);
        assert(res[0] == -1 && res[1] == -1 && res[2] == -1);
    }

    cout << "DECOMPOSITION IS OK" << endl;

}

void test_sampler()
{
    int N = Param::N;

    Param pLWE(LWE);
    Param pNTRU(NTRU);
    for (int run = 0; run < 1; run++)
    {
        //cout << "Run: " << run+1 << endl;
        {
            vector<int> vec(pNTRU.n, 0L);
            Sampler::get_ternary_vector(vec);

            assert(vec.size() == pNTRU.n);
            for (int i = 0; i < pNTRU.n; i++)
            {
                assert((vec[i]==0) || (vec[i]==-1) || (vec[i]==1) );
            }
        }

        {
            vector<int> vec(N,0L);
            Sampler::get_ternary_vector(vec);

            assert(vec.size() == N);
            for (int i = 0; i < N; i++)
            {
                assert((vec[i]==0) || (vec[i]==-1) || (vec[i]==1) );
            }
        }

        {
            vector<int> vec(N,0L);
            Sampler::get_binary_vector(vec);

            assert(vec.size() == N);
            for (int i = 0; i < N; i++)
            {
                assert((vec[i]==0) || (vec[i]==1) );
            }
        }

        {
            int n = pLWE.n;
            vector<vector<int>> mat(n, vector<int>(N,0L));
            Sampler::get_ternary_matrix(mat);

            assert(mat.size() == n && mat[0].size() == N);
            for (int i = 0; i < n; i++)
            {
                vector<int>& row = mat[i];
                for (int j = 0; j < N; j++)
                    assert((row[j]==0) || (row[j]==-1) || (row[j]==1) );
            }
        }

        {
            int n = pLWE.n;
            vector<int> vec(n, 0L);
            double st_dev = 4.0;
            Sampler::get_gaussian_vector(vec, st_dev);

            assert(vec.size() == n);
            for (int i = 0; i < n; i++)
            {
                assert(conv<double>(abs(vec[i])) < 6*st_dev);
            }
        }

        {
            int n = pNTRU.n;
            vector<vector<int>> mat(n, vector<int>(N,0L));
            double st_dev = 4.0;
            Sampler::get_gaussian_matrix(mat, st_dev);

            assert(mat.size() == n && mat[0].size() == N);
            for (int i = 0; i < n; i++)
            {
                vector<int>& row = mat[i];
                for (int j = 0; j < N; j++)
                    assert(conv<double>(abs(row[j])) < 6*st_dev);
            }
        }

        {
            vector<int> vec_inv(N,0L);
            vector<int> vec(N,0L);
            Sampler s(pNTRU);
            s.get_invertible_vector(vec, vec_inv, 4, 1);

            assert(vec.size() == N && vec_inv.size() == N);
            assert((vec[0]==1) || (vec[0]==-3) || (vec[0]==5) );
            for (int i = 1; i < N; i++)
            {
                assert((vec[i]==0) || (vec[i]==-4) || (vec[i]==4));
            }
        }

        {
            int n = pLWE.n;
            vector<vector<int>> mat_inv(n, vector<int>(n,0L));
            vector<vector<int>> mat(n, vector<int>(n,0L));
            Sampler s(pLWE);
            s.get_invertible_matrix(mat, mat_inv, 5, 1);

            assert(mat.size() == n && mat[0].size() == n
                && mat_inv.size() == n && mat_inv[0].size() == n);
            for (int i = 0; i < n; i++)
                assert((mat[i][i]==1) || (mat[i][i]==-4) || (mat[i][i]==6) );

            for (int i = 0; i < n; i++)
                for (int j = 0; (j < n) && (j != i); j++)
                {
                    assert((mat[i][j]==0) || (mat[i][j]==-5) || (mat[i][j]==5) );
                }
        }
    }
    cout << "SAMPLER IS OK" << endl;
}

enum GateType {NAND, AND, OR, XOR, NOT};

void test_ntruhe_gate_helper(int in1, int in2, const SchemeNTRU& s, GateType g)
{
    float avg_time = 0.0;
    int N_TESTS = (g == NOT ? 30 : 100);
    for (int i = 0; i < N_TESTS; i++)
    {
        Ctxt_NTRU ct_res, ct1, ct2;
        s.encrypt(ct1, in1);
        s.encrypt(ct2, in2);

        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "NAND output: " << output << endl;
            assert(output == !(in1 & in2));
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "AND output: " << output << endl;
            assert(output == (in1 & in2));
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "OR output: " << output << endl;
            assert(output == (in1 | in2));
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "XOR output: " << output << endl;
            assert(output == (in1 ^ in2));
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct1);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

//            cout << ".... NOT output: " << output << endl;
            assert(output == (1 - in1));
        }

    }
    cout << "Avg. time: " << avg_time/N_TESTS << endl;
}

void test_ntru_gate(SchemeNTRU& s, GateType g)
{
    test_ntruhe_gate_helper(0, 0, s, g);
    test_ntruhe_gate_helper(0, 1, s, g);
    test_ntruhe_gate_helper(1, 0, s, g);
    test_ntruhe_gate_helper(1, 1, s, g);
}

void test_ntruhe_nand(SchemeNTRU& s)
{
    GateType g = NAND;

    test_ntru_gate(s, g);

    cout << "NAND IS OK" << endl;
}

void test_ntruhe_and(SchemeNTRU& s)
{
    GateType g = AND;
    test_ntru_gate(s, g);
    cout << "AND IS OK" << endl;
}

void test_ntruhe_or(SchemeNTRU& s)
{
    GateType g = OR;
    test_ntru_gate(s, g);
    cout << "OR IS OK" << endl;
}

void test_ntruhe_xor(SchemeNTRU& s)
{
    GateType g = XOR;
    test_ntru_gate(s, g);
    cout << "XOR IS OK" << endl;
}

void test_ntruhe_not(SchemeNTRU& s)
{
    GateType g = NOT;
    for(int i = 0; i < 5; i++){
        int bit = binary_sampler(rand_engine);
        test_ntruhe_gate_helper(bit, 0, s, g);
    }
    cout << "NOT GATE IS OK" << endl;
}

void test_ntruhe_gate_composition_helper(SchemeNTRU& s, GateType g)
{
    float avg_time = 0.0;
    int N_TESTS = 100;

    int in1, in2, exp_out;
    in1 = binary_sampler(rand_engine);
    exp_out = in1;

    Ctxt_NTRU ct_res, ct;
    s.encrypt(ct_res, in1);
    for (int i = 0; i < N_TESTS; i++)
    {
        in2 = binary_sampler(rand_engine);
        s.encrypt(ct, in2);
        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct_res, ct);// ct_res should encrypt NAND(exp_out, in2)
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = !(exp_out & in2); // exp_out = NAND(exp_out, in2)
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out & in2); // exp_out = AND(exp_out, in2)
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out | in2); // exp_out = OR(exp_out, in2)
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out ^ in2); // exp_out = XOR(exp_out, in2)
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct_res);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (1 - exp_out); // exp_out = NOT(exp_out)
        }

        int output = s.decrypt(ct_res);
        assert(output == exp_out);
    }
    cout << "Avg. time: " << avg_time/N_TESTS << endl;
}

void test_ntruhe_composition_of_gates(SchemeNTRU& s)
{
    test_ntruhe_gate_composition_helper(s, NAND);
    cout << "COMPOSING NAND IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, AND);
    cout << "COMPOSING AND IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, OR);
    cout << "COMPOSING OR IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, XOR);
    cout << "COMPOSING XOR IS OK" << endl;

    test_ntruhe_gate_composition_helper(s, NOT);
    cout << "COMPOSING NOT GATE IS OK" << endl;
}

// ----- LWE tests

void test_lwehe_gate_helper(int in1, int in2, SchemeLWE& s, GateType g)
{
    float avg_time = 0.0;
    for (int i = 0; i < 100; i++)
    {
        Ctxt_LWE ct_res, ct1, ct2;
        s.encrypt(ct1, in1);
        s.encrypt(ct2, in2);

        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "NAND output: " << output << endl;
            assert(output == !(in1 & in2));
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "AND output: " << output << endl;
            assert(output == (in1 & in2));
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "OR output: " << output << endl;
            assert(output == (in1 | in2));
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct1, ct2);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

            //cout << "OR output: " << output << endl;
            assert(output == (in1 ^ in2));
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct1);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;

            int output = s.decrypt(ct_res);

//            cout << "NOT output: " << output << endl;
            assert(output == (1 - in1));
        }

    }
    cout << "Avg. time: " << avg_time/100.0 << endl;
}

void test_lwe_gate(SchemeLWE& s, GateType g)
{
    test_lwehe_gate_helper(0, 0, s, g);
    test_lwehe_gate_helper(0, 1, s, g);
    test_lwehe_gate_helper(1, 0, s, g);
    test_lwehe_gate_helper(1, 1, s, g);
}

void test_lwehe_nand(SchemeLWE& s)
{
    GateType g = NAND;
    test_lwe_gate(s, g);
    cout << "NAND IS OK" << endl;
}

void test_lwehe_and(SchemeLWE& s)
{
    GateType g = AND;
    test_lwe_gate(s, g);
    cout << "AND IS OK" << endl;
}

void test_lwehe_or(SchemeLWE& s)
{
    GateType g = OR;
    test_lwe_gate(s, g);
    cout << "OR IS OK" << endl;
}

void test_lwehe_xor(SchemeLWE& s)
{
    GateType g = XOR;
    test_lwe_gate(s, g);
    cout << "XOR IS OK" << endl;
}

void test_lwehe_not(SchemeLWE& s)
{
    GateType g = NOT;
    for(int i = 0; i < 4; i++){
        int bit = binary_sampler(rand_engine);
        test_lwehe_gate_helper(bit, 0, s, g);
    }

    cout << "NOT GATE IS OK" << endl;
}

void test_lwehe_gate_composition_helper(SchemeLWE& s, GateType g)
{
    float avg_time = 0.0;
    int N_TESTS = 110;

    int in1, in2, exp_out;
    in1 = binary_sampler(rand_engine);
    exp_out = in1;

    Ctxt_LWE ct_res, ct;
    s.encrypt(ct_res, in1);
    for (int i = 0; i < N_TESTS; i++)
    {
        in2 = binary_sampler(rand_engine);
        s.encrypt(ct, in2);
        if (g == NAND)
        {
            auto start = clock();
            s.nand_gate(ct_res, ct_res, ct);// ct_res should encrypt NAND(exp_out, in2)
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = !(exp_out & in2); // exp_out = NAND(exp_out, in2)
            //cout << "NAND output: " << output << endl;
        }
        else if (g == AND) {
            auto start = clock();
            s.and_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out & in2); // exp_out = AND(exp_out, in2)
            //cout << "AND output: " << output << endl;
        }
        else if (g == OR) {
            auto start = clock();
            s.or_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out | in2); // exp_out = OR(exp_out, in2)
            //cout << "OR output: " << output << endl;
        }
        else if (g == XOR) {
            auto start = clock();
            s.xor_gate(ct_res, ct_res, ct);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (exp_out ^ in2); // exp_out = XOR(exp_out, in2)
            //cout << "XOR output: " << output << endl;
        }
        else if (g == NOT) {
            auto start = clock();
            s.not_gate(ct_res, ct_res);
            avg_time += float(clock()-start)/CLOCKS_PER_SEC;
            exp_out = (1 - exp_out); // exp_out = NOT(exp_out)
        }

        int output = s.decrypt(ct_res);
        assert(output == exp_out);
    }
    cout << "Avg. time: " << avg_time/N_TESTS << endl;
}

void test_lwehe_composition_of_gates(SchemeLWE& s)
{
    test_lwehe_gate_composition_helper(s, NAND);
    cout << "COMPOSING NAND IS OK" << endl;

    test_lwehe_gate_composition_helper(s, AND);
    cout << "COMPOSING AND IS OK" << endl;

    test_lwehe_gate_composition_helper(s, OR);
    cout << "COMPOSING OR IS OK" << endl;

    test_lwehe_gate_composition_helper(s, XOR);
    cout << "COMPOSING XOR IS OK" << endl;

    test_lwehe_gate_composition_helper(s, NOT);
    cout << "COMPOSING NOT GATE IS OK" << endl;
}

int main()
{
    test_params();
    test_sampler();

    cout << endl;
    cout << "-------------------------" << endl;
    cout << "NTRU tests" << endl;
    SchemeNTRU s_ntru;
    test_ntruhe_nand(s_ntru);
    test_ntruhe_and(s_ntru);
    test_ntruhe_or(s_ntru);
    test_ntruhe_xor(s_ntru);
    test_ntruhe_not(s_ntru);
    test_ntruhe_composition_of_gates(s_ntru);
    cout << "NTRU tests PASSED" << endl;

    cout << endl;
    cout << "-------------------------" << endl;
    cout << "LWE tests" << endl;
    SchemeLWE s_lwe;
    test_lwehe_not(s_lwe);
    test_lwehe_nand(s_lwe);
    test_lwehe_and(s_lwe);
    test_lwehe_or(s_lwe);
    test_lwehe_xor(s_lwe);
    test_lwehe_not(s_lwe);
    test_lwehe_composition_of_gates(s_lwe);
    cout << "LWE tests PASSED" << endl;

    return 0;
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2019 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// 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/formulas/karney_direct.hpp>
#include <boost/geometry/srs/srs.hpp>

#ifdef BOOST_GEOEMTRY_TEST_WITH_GEOGRAPHICLIB
#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/Constants.hpp>
#endif // BOOST_GEOEMTRY_TEST_WITH_GEOGRAPHICLIB

int test_main(int, char*[])
{

#ifdef BOOST_GEOEMTRY_TEST_WITH_GEOGRAPHICLIB
    // accuracy test from https://github.com/boostorg/geometry/issues/560
    using namespace GeographicLib;

    const long double wgs84_a = 6378137.0L;
    const long double wgs84_f = 1.0L / 298.257223563L;
    const long double one_minus_f = 1.0L - wgs84_f;
    const long double wgs84_b = wgs84_a * one_minus_f;

    const boost::geometry::srs::spheroid<long double> BoostWGS84(wgs84_a, wgs84_b);

    // boost karney_direct function class with azimuth output and SeriesOrder = 6
    typedef boost::geometry::formula::karney_direct <double, true, true, false, false, 6u>
            BoostKarneyDirect_6;

    // boost karney_direct function class with azimuth output and SeriesOrder = 8
    typedef boost::geometry::formula::karney_direct <double, true, true, false, false, 8u>
            BoostKarneyDirect_8;

    // boost test BOOST_CHECK_CLOSE macro takes a percentage accuracy parameter
    const double EPSILON = std::numeric_limits<double>::epsilon();
    const double CALCULATION_TOLERANCE = 100 * EPSILON;

    const Geodesic GeographicLibWGS84(Geodesic::WGS84());

    // Loop around latitudes: 0 to 89 degrees
    for (int i=0; i < 90; ++i)
    {
        // The latitude in degrees.
        double latitude(1.0 * i);

        // Loop around longitudes: 1 to 179 degrees
        for (int j=1; j < 180; ++j)
        {
            // The longitude in degrees.
            double longitude(1.0 * j);

            // The Geodesic: distance in metres, start azimuth and finish azimuth in degrees.
            double distance_m, azimuth, azi2;
            GeographicLibWGS84.Inverse(0.0, 0.0, latitude, longitude, distance_m, azimuth, azi2);

            // The GeographicLib position and azimuth at the distance in metres
            double lat2k, lon2k, azi2k;
            GeographicLibWGS84.Direct(0.0, 0.0, azimuth, distance_m, lat2k, lon2k, azi2k);
            BOOST_CHECK_CLOSE(latitude, lat2k, 140 * CALCULATION_TOLERANCE);
            BOOST_CHECK_CLOSE(longitude, lon2k, 120 * CALCULATION_TOLERANCE);

            // The boost karney_direct order 6 position at the azimuth and distance in metres.
            boost::geometry::formula::result_direct<double> results_6
                    = BoostKarneyDirect_6::apply(0.0, 0.0, distance_m, azimuth, BoostWGS84);
            BOOST_CHECK_CLOSE(azi2, results_6.reverse_azimuth, 140 * CALCULATION_TOLERANCE);
            BOOST_CHECK_CLOSE(latitude, results_6.lat2, 220 * CALCULATION_TOLERANCE);

            /******** Test below only passes with >= 10172000 * CALCULATION_TOLERANCE !! ********/
            BOOST_CHECK_CLOSE(longitude, results_6.lon2, 10171000 * CALCULATION_TOLERANCE);
            /*****************************************************************************/

            // The boost karney_direct order 8 position at the azimuth and distance in metres.
            boost::geometry::formula::result_direct<double> results_8
                    = BoostKarneyDirect_8::apply(0.0, 0.0, distance_m, azimuth, BoostWGS84);
            BOOST_CHECK_CLOSE(azi2, results_8.reverse_azimuth, 140 * CALCULATION_TOLERANCE);
            BOOST_CHECK_CLOSE(latitude, results_8.lat2, 220 * CALCULATION_TOLERANCE);

            /******** Test below only passes with >= 10174000 * CALCULATION_TOLERANCE !! ********/
            BOOST_CHECK_CLOSE(longitude, results_8.lon2, 10173000 * CALCULATION_TOLERANCE);
            /*****************************************************************************/
        }
    }
#endif // BOOST_GEOEMTRY_TEST_WITH_GEOGRAPHICLIB

    return 0;
}


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

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

 * See LICENSE for the license information

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

/**
 * @file    cholesky.cpp
 * @brief   Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky
 * @author  Richard Roberts
 * @author  Frank Dellaert
 * @date    Nov 5, 2010
 */

#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

static const double negativePivotThreshold = -1e-1;
static const double zeroPivotThreshold = 1e-6;
static const double underconstrainedPrior = 1e-5;
static const int underconstrainedExponentDifference = 12;

/* ************************************************************************* */
static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) {
  // Get pivot value
  double alpha = ATA(k, k);

  // Correct negative pivots from round-off error
  if (alpha < negativePivotThreshold) {
    return -1;
  } else if (alpha < 0.0)
    alpha = 0.0;

  const double beta = sqrt(alpha);

  if (beta > zeroPivotThreshold) {
    const double betainv = 1.0 / beta;

    // Update k,k
    ATA(k, k) = beta;

    if (k < (order - 1)) {
      // Update A(k,k+1:end) <- A(k,k+1:end) / beta
      typedef Matrix::RowXpr::SegmentReturnType BlockRow;
      BlockRow V = ATA.row(k).segment(k + 1, order - (k + 1));
      V *= betainv;

      // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
      ATA.block(k + 1, k + 1, order - (k + 1), order - (k + 1)) -= V.transpose() * V;
      //      ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
      //          .rankUpdate(V.adjoint(), -1);
    }
    return 1;
  } else {
    // For zero pivots, add the underconstrained variable prior
    ATA(k, k) = underconstrainedPrior;
    for (size_t j = k + 1; j < order; ++j)
      ATA(k, j) = 0.0;
    return 0;
  }
}

/* ************************************************************************* */
pair<size_t, bool> choleskyCareful(Matrix& ATA, int order) {
  // Check that the matrix is square (we do not check for symmetry)
  assert(ATA.rows() == ATA.cols());

  // Number of rows/columns
  const size_t n = ATA.rows();

  // Negative order means factor the entire matrix
  if (order < 0)
    order = int(n);

  assert(size_t(order) <= n);

  // The index of the row after the last non-zero row of the square-root factor
  size_t maxrank = 0;
  bool success = true;

  // Factor row-by-row
  for (size_t k = 0; k < size_t(order); ++k) {
    int stepResult = choleskyStep(ATA, k, size_t(order));
    if (stepResult == 1) {
      maxrank = k + 1;
    } else if (stepResult == -1) {
      success = false;
      break;
    } /* else if(stepResult == 0) Found zero pivot */
  }

  return make_pair(maxrank, success);
}

/* ************************************************************************* */
bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {

  gttic(choleskyPartial);
  if (nFrontal == 0)
    return true;

  assert(ABC.cols() == ABC.rows());
  const Eigen::DenseIndex n = ABC.rows() - topleft;
  assert(n >= 0 && nFrontal <= size_t(n));

  // Create views on blocks
  auto A = ABC.block(topleft, topleft, nFrontal, nFrontal);
  auto B = ABC.block(topleft, topleft + nFrontal, nFrontal, n - nFrontal);
  auto C = ABC.block(topleft + nFrontal, topleft + nFrontal, n - nFrontal, n - nFrontal);

  // Compute Cholesky factorization A = R'*R, overwrites A.
  gttic(LLT);
  Eigen::LLT<Matrix, Eigen::Upper> llt(A);
  Eigen::ComputationInfo lltResult = llt.info();

  if (lltResult != Eigen::Success) {
    return false;
  }

  auto R = A.triangularView<Eigen::Upper>();
  R = llt.matrixU();
  gttoc(LLT);

  // Compute S = inv(R') * B
  gttic(compute_S);
  if (nFrontal < n)
    R.transpose().solveInPlace(B);
  gttoc(compute_S);

  // Compute L = C - S' * S
  gttic(compute_L);
  if (nFrontal < n)
    C.selfadjointView<Eigen::Upper>().rankUpdate(B.transpose(), -1.0);
  gttoc(compute_L);

  // Check last diagonal element - Eigen does not check it
  if (nFrontal >= 2) {
    int exp2, exp1;
    (void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2);
    (void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1);
    return (exp2 - exp1 < underconstrainedExponentDifference);
  } else if (nFrontal == 1) {
    int exp1;
    (void)frexp(R(0, 0), &exp1);
    return (exp1 > -underconstrainedExponentDifference);
  } else {
    return true;
  }

} // END choleskyPartial()
}  // namespace gtsam


#include <geometry.h>
#include <tiny_math_types.h>

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

typedef tiny::MathTypes<double>   MT;
typedef MT::vector3_type          V;
typedef MT::real_type             T;
typedef MT::value_traits          VT;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(inside_dop_test)
{
  std::vector<V> corners(2u);

  corners[0] = V::make(-1.0,-1.0,-1.0);
  corners[1] = V::make( 1.0, 1.0, 1.0);

  geometry::DOP<T, 6u> const dop = geometry::make_dop(corners.begin(), corners.end(), geometry::make3<V>() );

  BOOST_CHECK(geometry::is_valid(dop));

  {
    V    const p    = V::make( 0.0, 0.0, 0.0);
    bool const test = geometry::inside_dop(p, dop);
    BOOST_CHECK( test );
  }
  {
    V    const p    = V::make( 1.0, 1.0, 1.0);
    bool const test = geometry::inside_dop(p, dop);
    BOOST_CHECK( test );
  }
  {
    V    const p    = V::make( 2.0, 0.0, 0.0);
    bool const test = geometry::inside_dop(p, dop);
    BOOST_CHECK( !test );
  }

}

BOOST_AUTO_TEST_CASE(outside_dop_test)
{
  std::vector<V> corners(2u);

  corners[0] = V::make(-1.0,-1.0,-1.0);
  corners[1] = V::make( 1.0, 1.0, 1.0);

  geometry::DOP<T, 6u> const dop = geometry::make_dop(corners.begin(), corners.end(), geometry::make3<V>() );

  BOOST_CHECK(geometry::is_valid(dop));

  {
    V    const p    = V::make( 0.0, 0.0, 0.0);
    bool const test = geometry::outside_dop(p, dop);
    BOOST_CHECK( ! test );
  }
  {
    V    const p    = V::make( 1.0, 1.0, 1.0);
    bool const test = geometry::outside_dop(p, dop);
    BOOST_CHECK( ! test );
  }
  {
    V    const p    = V::make( 2.0, 0.0, 0.0);
    bool const test = geometry::outside_dop(p, dop);
    BOOST_CHECK( test );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <math/prim/scal/prob/util.hpp>
#include <vector>

TEST(ProbDistributionsBernoulli, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::bernoulli_rng(0.6, rng));

  EXPECT_THROW(stan::math::bernoulli_rng(1.6, rng), std::domain_error);
  EXPECT_THROW(stan::math::bernoulli_rng(-0.6, rng), std::domain_error);
  EXPECT_THROW(stan::math::bernoulli_rng(stan::math::positive_infinity(), rng),
               std::domain_error);
}

TEST(ProbDistributionsBernoulli, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;

  std::vector<double> expected;
  expected.push_back(N * (1 - 0.4));
  expected.push_back(N * 0.4);

  std::vector<int> counts(2);
  for (int i = 0; i < N; ++i) {
    ++counts[stan::math::bernoulli_rng(0.4, rng)];
  }

  assert_chi_squared(counts, expected, 1e-6);
}


#include "tensor.hpp"

#include <Eigen/Dense>

using namespace Eigen;

TensorXd::TensorXd(int nrow, int ncol, int nslice)
    : nrow_(nrow), ncol_(ncol), nslice_(nslice),
      value_(MatrixXd::Zero(nrow_, ncol_ * nslice_)) {}

Block<MatrixXd> TensorXd::operator[](int slice) {
  return value_.block(0, slice * ncol_, nrow_, ncol_);
}


/* Boost test/fmod.cpp
 * test the fmod with specially crafted integer intervals
 *
 * Copyright Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors nor Polytechnic University make any
 * representation about the suitability of this software for any
 * purpose. It is provided "as is" without express or implied warranty.
 *
 * $Id: fmod.cpp,v 1.3 2003/02/05 17:34:36 gmelquio Exp $
 */

#include <boost/numeric/interval/interval.hpp>
#include <boost/numeric/interval/arith.hpp>
#include <boost/numeric/interval/arith2.hpp>
#include <boost/numeric/interval/utility.hpp>
#include <boost/numeric/interval/checking.hpp>
#include <boost/numeric/interval/rounding.hpp>
#include <boost/test/minimal.hpp>

struct my_rounded_arith {
  int sub_down(int x, int y) { return x - y; }
  int sub_up  (int x, int y) { return x - y; }
  int mul_down(int x, int y) { return x * y; }
  int mul_up  (int x, int y) { return x * y; }
  int div_down(int x, int y) {
    int q = x / y;
    return (x % y < 0) ? (q - 1) : q;
  }
  int int_down(int x) { return x; }
};

using namespace boost;
using namespace numeric;
using namespace interval_lib;

typedef change_rounding<interval<int>, save_state_nothing<my_rounded_arith> >::type I;

int test_main(int, char *[]) {

  BOOST_CHECK(equal(fmod(I(6,9), 7), I(6,9)));
  BOOST_CHECK(equal(fmod(6, I(7,8)), I(6,6)));
  BOOST_CHECK(equal(fmod(I(6,9), I(7,8)), I(6,9)));

  BOOST_CHECK(equal(fmod(I(13,17), 7), I(6,10)));
  BOOST_CHECK(equal(fmod(13, I(7,8)), I(5,6)));
  BOOST_CHECK(equal(fmod(I(13,17), I(7,8)), I(5,10)));

  BOOST_CHECK(equal(fmod(I(-17,-13), 7), I(4,8)));
  BOOST_CHECK(equal(fmod(-17, I(7,8)), I(4,7)));
  BOOST_CHECK(equal(fmod(I(-17,-13), I(7,8)), I(4,11)));

  return 0;
}


#include "kernel_test.hpp"

#include <boost/test/unit_test.hpp>
#include <random>
#include <iostream>

namespace GooBalls {
namespace d2 {
namespace Physics {
/**
 * Properties a kernel should have:
 * C^0, C^1 continuous
 * evaluated at r = h, the value should be zero
 * Kernel must be normalized: int_X W(|x - x_i|) dx = 1
 */

/// Create `samples` 2d points on unit disk
Coordinates2d randomUnitDisk(int samples){
    std::mt19937 rnd;
    rnd.seed(23);
    std::uniform_real_distribution<> dist(0.0, 1.0);
    Coordinates2d Xs(samples, 2);
    for(int i = 0; i < samples; ++i){
        do {
            Xs(i, 0) = dist(rnd);
            Xs(i, 1) = dist(rnd);
            // rejection sampling
        } while (Xs.row(i).norm() > 1.0);
    }
    return Xs;
}

Coordinates1d increasingToOne(int n){
    Coordinates1d Xorig(n);
    for(int i = 0; i  < n; ++i){
        Xorig[i] = double(i)/n;
    }
    assert(Xorig.maxCoeff() < 1.0);
    return Xorig;
}

std::vector<Float> getHs(){
    std::vector<Float> hs;
    hs.push_back(0.1);
    hs.push_back(0.5);
    hs.push_back(1.0);
    hs.push_back(5.0);
    hs.push_back(10.0);
    hs.push_back(50.0);
    hs.push_back(100.0);
    return hs;
}

void testRadialSymmetry(Kernel& k, int experiments){
    int radiusSamples = std::sqrt(experiments);
    int angleSamples = radiusSamples;
    auto hs = getHs();
    for(auto h : hs){
        k.setH(h);
        Coordinates2d Xs(angleSamples, 2);
        Coordinates1d w, wgradN, wlap;
        Coordinates2d wgrad;
        Coordinates2d dirs(angleSamples, 2);
        for(int i = 1; i < radiusSamples; ++i){
            Float radius = i/double(radiusSamples) * h;
            for(int j = 0; j < angleSamples; ++j){
                Float angle = i/double(angleSamples) * 2*M_PI;
                dirs(j, 0) = std::sin(angle);
                dirs(j, 1) = std::cos(angle);
            }
            Xs = dirs * radius;
            k.compute(Xs, &w, &wgrad, &wlap);
            wgradN = wgrad.rowwise().norm();
            BOOST_TEST(w.maxCoeff() - w.minCoeff() < 0.0001);
            BOOST_TEST(wlap.maxCoeff() - wlap.minCoeff() < 0.0001);
            BOOST_TEST(wgradN.maxCoeff() - wgradN.minCoeff() < 0.0001);
            // TODO: check gradient direction
        }
    }
}

void testZeroBorder1d(Kernel& k){
    auto hs = getHs();
    Coordinates1d ws(hs.size());
    for(size_t i = 0; i < hs.size(); ++i){
        Float h = hs[i];
        k.setH(h);
        Coordinates1d Xs(1);
        Xs[0] = h;
        Coordinates1d w;
        k.compute1d(Xs.array()*Xs.array(), &w, nullptr, nullptr);
        ws[i] = w[0];
        // BOOST_TEST(w[0] == 0.0);
    }
    ws = ws.array().abs();
    BOOST_TEST(ws.maxCoeff() == 0.0);
}

void testZeroBorder(Kernel& k, int experiments){
    std::mt19937 rnd;
    rnd.seed(17); // we use a fixed seed for repeatability
    std::uniform_real_distribution<> dist(-10.0, 10.0);
    Coordinates1d ws(experiments);
    for(int i = 0; i < experiments; ++i){
        Coordinates2d x(1,2);
        x(0,0) = dist(rnd);
        x(0,1) = dist(rnd);
        auto h = x.norm();
        k.setH(h);
        Coordinates1d w;
        k.compute(x, &w, nullptr, nullptr);
        ws[i] = w[0];
        //BOOST_TEST(w[0] == 0.0);
    }
    ws = ws.array().abs();
    BOOST_TEST(ws.maxCoeff() == 0.0);
}

void testZeroBorderGradient1d(Kernel& k){
    auto hs = getHs();
    for(auto h : hs){
        k.setH(h);
        Coordinates1d Xs(1);
        Xs[0] = h;
        Coordinates1d w;
        k.compute1d(Xs.array()*Xs.array(), nullptr, &w, nullptr);
        BOOST_TEST(w[0] == 0.0);
    }
}

void testZeroBorderGradient(Kernel& k, int experiments){
    Coordinates2d Xorig = randomUnitDisk(experiments).rowwise().normalized();
    auto hs = getHs();
    for(auto h : hs){
        Coordinates2d Xs = h*Xorig;
        Coordinates2d Wgrad;
        k.setH(h);
        k.compute(Xs, nullptr, &Wgrad, nullptr);
        int i;
        auto extrema = Wgrad.rowwise().norm();
        extrema.maxCoeff(&i);
        BOOST_TEST(Wgrad.row(i).norm() == 0.0);
        BOOST_TEST(Wgrad(i, 0) == 0.0);
        BOOST_TEST(Wgrad(i, 1) == 0.0);
    }
}

void testNonNegativity(Kernel& k, int experiments){
    Coordinates2d Xs = randomUnitDisk(experiments);
    Coordinates1d W;
    k.setH(1.0);
    k.compute(Xs, &W, nullptr, nullptr);
    for(int i = 0; i < experiments; ++i){
        BOOST_TEST(0 <= W[i]);
    }
}

void testMonotonicity(Kernel& k, int experiments){
    Coordinates2d Xs(experiments, 2);
    Xs.setZero();
    Float h = 1.0;
    for(int i = 0; i < experiments; ++i){
        Xs(i, 0) = i/double(experiments-1)*h;
    }
    k.setH(h);
    Coordinates1d w;
    k.compute(Xs, &w, nullptr, nullptr);
    for(int i = 1; i < experiments; ++i){
        BOOST_TEST(w[i-1] >= w[i], std::to_string(i) + ": " + std::to_string(w[i-1]) + " >=! " + std::to_string(w[i]));
    }
}

void testGradientFiniteDifference(Kernel& k, int experiments){
    const Coordinates2d Xorig = randomUnitDisk(experiments);
    auto hs = getHs();
    for(auto h : hs){
        Float dx = h * 0.00000001;
        k.setH(h);
        Coordinates2d Xs = 0.99 * h * Xorig.array();
        Coordinates2d Xsx = Xs;
        Xsx.col(0).array() += dx;
        Coordinates2d Xsy = Xs;
        Xsy.col(1).array() += dx;
        Coordinates1d W0, Wx, Wy;
        Coordinates2d WgradReceived;
        k.compute(Xs, &W0, &WgradReceived, nullptr);
        k.compute(Xsx, &Wx, nullptr, nullptr);
        k.compute(Xsy, &Wy, nullptr, nullptr);
        Coordinates2d gradExpected(Xs.rows(), 2);
        gradExpected.col(0) = (Wx - W0)/dx;
        gradExpected.col(1) = (Wy - W0)/dx;
        auto diff = (gradExpected - WgradReceived).array().abs();
        int i, j;
        diff.maxCoeff(&i,&j);
        BOOST_TEST(gradExpected(i, j) == WgradReceived(i, j));
    }
}

void testGradientFiniteDifference1d(Kernel& k, int experiments){
    auto hs = getHs();
    Coordinates1d Xorig = increasingToOne(experiments);
    for(auto h : hs){
        Float dx = h * 0.000001;
        k.setH(h);
        Coordinates1d Xs = 0.99 * h * Xorig;
        Coordinates1d Xsx = Xs.array() + dx;
        Coordinates1d W0, Wx;
        Coordinates1d WgradReceived;
        k.compute1d(Xs.array()*Xs.array(), &W0, &WgradReceived, nullptr);
        k.compute1d(Xsx.array()*Xsx.array(), &Wx, nullptr, nullptr);
        Coordinates1d gradExpected = (Wx - W0)/dx;
        auto diff = (gradExpected - WgradReceived).array().abs();
        int i;
        diff.maxCoeff(&i);
        BOOST_TEST(gradExpected[i] == WgradReceived[i]);
    }
}

void testLaplacianFromGradientFiniteDifferences1d(Kernel& k, int experiments){
    const Coordinates1d Xorig = increasingToOne(experiments);
    auto hs = getHs();
    for(auto h : hs){
        Float dx = h * 0.000001;
        k.setH(h);
        Coordinates1d Xs = 0.99*h*Xorig.array();
        Coordinates1d Xsx = Xs.array() + dx;

        Coordinates1d G0, Gx;
        Coordinates1d Wlap;
        k.compute1d(Xs.array()*Xs.array(), nullptr, &G0, &Wlap);
        k.compute1d(Xsx.array()*Xsx.array(), nullptr, &Gx, nullptr);
        Coordinates1d expectedLaplacian = (Gx - G0)/dx;

        auto diff = (expectedLaplacian - Wlap).array().abs();
        int i;
        diff.maxCoeff(&i);
        BOOST_TEST(expectedLaplacian[i] == Wlap[i]);
    }
}

void testLaplacianFromGradientFiniteDifferences(Kernel& k, int experiments){
    const Coordinates2d Xorig = randomUnitDisk(experiments);
    auto hs = getHs();
    for(auto h : hs){
        Float dx = h * 0.000001;
        k.setH(h);
        Coordinates2d Xs = 0.99*h*Xorig;
        Coordinates2d Xsx, Xsy;
        Xsx = Xs; Xsx.col(0).array() += dx;
        Xsy = Xs; Xsy.col(1).array() += dx;

        Coordinates2d G0, Gx, Gy;
        Coordinates1d Wlap;
        k.compute(Xs, nullptr, &G0, &Wlap);
        k.compute(Xsx, nullptr, &Gx, nullptr);
        k.compute(Xsy, nullptr, &Gy, nullptr);
        Coordinates1d expectedLaplacian = (Gx - G0).col(0)/dx + (Gy - G0).col(1)/dx;

        auto diff = (expectedLaplacian - Wlap).array().abs();
        int i;
        diff.maxCoeff(&i);
        BOOST_TEST(expectedLaplacian[i] == Wlap[i]);
    }
}

void testLaplacianFiniteDifference1d(Kernel& k, int experiments){
    const Coordinates1d Xorig = increasingToOne(experiments);
    auto hs = getHs();
    for(auto h : hs){
        Float dx = h * 0.0001;
        k.setH(h);
        Coordinates1d Xs = 0.99 * h * Xorig.array() + dx;
        Coordinates1d Xsx1 = Xs.array() - dx;
        Coordinates1d Xsx2 = Xs.array() + dx;

        Coordinates1d W0, Wx1, Wx2;
        Coordinates1d Wlap;
        k.compute1d(Xs.array()*Xs.array(), &W0, nullptr, &Wlap);
        k.compute1d(Xsx1.array()*Xsx1.array(), &Wx1, nullptr, nullptr);
        k.compute1d(Xsx2.array()*Xsx2.array(), &Wx2, nullptr, nullptr);
        Coordinates1d expectedLaplacian = (Wx1 + Wx2 - 2*W0) / (dx*dx);

        auto diff = (expectedLaplacian - Wlap).array().abs();
        int i;
        diff.maxCoeff(&i);
        BOOST_TEST(expectedLaplacian[i] == Wlap[i]);
    }
}

void testLaplacianFiniteDifference(Kernel& k, int experiments){
    const Coordinates2d Xorig = randomUnitDisk(experiments);
    auto hs = getHs();
    for(auto h : hs){
        Float dx = h * 0.00001;
        k.setH(h);
        Coordinates2d Xs = 0.99 * h * Xorig.array() + dx;
        Coordinates2d Xsx1, Xsx2, Xsy1, Xsy2;
        Xsx1 = Xs; Xsx1.col(0).array() -= dx;
        Xsx2 = Xs; Xsx2.col(0).array() += dx;
        Xsy1 = Xs; Xsy1.col(1).array() -= dx;
        Xsy2 = Xs; Xsy2.col(1).array() += dx;

        Coordinates1d W0, Wx1, Wx2, Wy1, Wy2;
        Coordinates1d Wlap;
        k.compute(Xs, &W0, nullptr, &Wlap);
        k.compute(Xsx1, &Wx1, nullptr, nullptr);
        k.compute(Xsx2, &Wx2, nullptr, nullptr);
        k.compute(Xsy1, &Wy1, nullptr, nullptr);
        k.compute(Xsy2, &Wy2, nullptr, nullptr);
        Coordinates1d expectedLaplacian = (Wx1 + Wx2 + Wy1 + Wy2 - 4*W0) / (dx*dx);

        auto diff = (expectedLaplacian - Wlap).array().abs();
        int i;
        diff.maxCoeff(&i);
        BOOST_TEST(expectedLaplacian[i] == Wlap[i]);
    }
}

void testNormalization1d(Kernel& k, int experiments){
    auto hs = getHs();
    Coordinates1d Xorig = increasingToOne(experiments);
    // we integrate over [-h, h] with `experiments` steps of size (2*h/experiments)
    for(auto h : hs){
        Coordinates1d Xs = h*(Xorig.array()*2 - 1);
        Float dA = h*2/experiments; // area of one test segment
        Coordinates1d W;
        k.setH(h);
        k.compute1d(Xs.array()*Xs.array(), &W, nullptr, nullptr);
        Float totalWeight = W.sum()*dA;
        BOOST_TEST(totalWeight == 1.0);
    }
}

void testNormalization(Kernel& k, int experiments){
    auto hs = getHs();
    int n = std::max(100.0, std::sqrt(experiments));
    for(auto h : hs){
        Coordinates2d Xs(n*n, 2);
        int inserted = 0;
        for(int i = 0; i < n; ++i){
            for(int j = 0; j < n; ++j){
                TranslationVector pos;
                pos[0] = h*(2*i/double(n)-1);
                pos[1] = h*(2*j/double(n)-1);
                if(pos.norm() > h){
                    continue;
                }
                Xs.row(inserted++) = pos;
            }
        }
        Xs.conservativeResize(inserted, Eigen::NoChange);
        Float dA;
        dA = 2*h/n; // area of one test square
        dA = dA*dA;
        Coordinates1d W;
        k.setH(h);
        k.compute(Xs, &W, nullptr, nullptr);
        Float totalWeight = W.sum()*dA;
        BOOST_TEST(totalWeight == 1.0);
    }
}

} // Physics
} // d2
} // GooBalls


/*
 * benchmark_hogwild_regression.cpp
 * author: Abhijit Chowdhary (achowdh2@ncsu.edu)
 *
 * Benchmark HOGWILD! as applied to regression on to a simple random normal 50
 * x 50 matrix A and random normal vector b:
 *
 *  minimize (1/2)||Ax-b||_2^2
 *
 * Outputs to results.txt and stdout time taken to reach desired tolerance for
 * each core count possible in system.
 *  ---------------------------------------------------------------------------
 * This Source Code Form is subject to the terms of the Mozilla Public License
 * v. 2.0. If a copy of the MPL was not distributed with this file, You can
 * obtain one at http://mozilla.org/MPL/2.0/.
 */
#include <omp.h>
#include <stdio.h>

#include <algorithm>
#include <array>
#include <atomic>
#include <iostream>
#include <random>
#include <vector>

#include <Eigen/Dense>

#include "compute_variance.h"

#define N 1024
#define ETA 0.001
#define NUM_EPOCHS 20
#define TRIALS_PER_BAND 100
#define MAX_BAND_POW 10

int
main(int argc, char **argv)
{
  // Initialize random state, and parallel parameters.
  unsigned P = omp_get_max_threads();
  Eigen::initParallel();
  omp_set_dynamic(0);
  std::mt19937 gen(0);

  // Construct sampling w/ replacement vector.
  std::uniform_int_distribution<std::mt19937::result_type> distN(0,N-1);
  unsigned *rand_selection = new unsigned[N*NUM_EPOCHS];
  for (unsigned k = 0; k < N*NUM_EPOCHS; k++)
  {
    rand_selection[k] = distN(gen);
  }

  std::vector<double> computed_variances;
  Eigen::MatrixXd X = Eigen::MatrixXd::Constant(N, 1, 1.0);
  for (unsigned bp = 0; bp < MAX_BAND_POW; bp++)
  {
    unsigned band = 1 << bp;
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N, N);
    A.diagonal(0) = Eigen::MatrixXd::Random(N,1);
    for (int k = 1; k <= band; k++)
    {
      A.diagonal(k) = Eigen::MatrixXd::Random(N-k, 1);
      A.diagonal(-k) = Eigen::MatrixXd::Random(N-k, 1);
    }

    Eigen::VectorXd b = A*X;

    std::vector<double> computed_norms;
    std::array<std::atomic<double>, N> x;
    for (unsigned trial = 0; trial < TRIALS_PER_BAND; ++trial)
    { // Begin SGD trial
      for (unsigned k = 0; k < N; k++) { x[k] = 0.0; }

      #pragma omp parallel for
      for (unsigned k = 0; k < N*NUM_EPOCHS; k++)
      { // Begin parallel SGD iterations
        unsigned id = rand_selection[k];
        double dg = 0;
        for (unsigned i = 0; i < N; i++) { dg += A(id, i)*x[i].load(); }
        dg -= b(id);
        for (unsigned i = 0; i < N; i++)
        {
          double dgi = x[i].load() - ETA*( A(id,i)*dg );
          x[i].exchange( dgi );
        }
      } // End parallel SGD iterations

      Eigen::MatrixXd xx(N,1);
      for (int k = 0; k < N; k++) { xx(k) = x[k].load(); }
      computed_norms.push_back( 0.5*(A*xx-b).squaredNorm() );
    } // End SGD trial

    computed_variances.push_back( compute_variance(computed_norms) );
  }
  for (unsigned k = 0; k < computed_variances.size(); k++)
  {
    printf("%.9f\n", computed_variances[k]);
  }
  delete []rand_selection;
  return 0;
}


//
// Copyright (c) 2015-2019 CNRS INRIA
//

/*
 * Validate the sparse Cholesky decomposition of the mass matrix.  The code
 * tests both the numerical value and the computation time. For a strong
 * computation benchmark, see benchmark/timings.
 *
 */

#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include <iostream>
#ifdef NDEBUG
#  include <Eigen/Cholesky>
#endif

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

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_cholesky )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
  crba(model,data,q);

  pinocchio::cholesky::decompose(model,data);
  data.M.triangularView<Eigen::StrictlyLower>() =
  data.M.triangularView<Eigen::StrictlyUpper>().transpose();

  const Eigen::MatrixXd & U = data.U;
  const Eigen::VectorXd & D = data.D;
  const Eigen::MatrixXd & M = data.M;

  #ifndef NDEBUG
    std::cout << "M = [\n" << M << "];" << std::endl;
    std::cout << "U = [\n" << U << "];" << std::endl;
    std::cout << "D = [\n" << D.transpose() << "];" << std::endl;
  #endif

  BOOST_CHECK(M.isApprox(U*D.asDiagonal()*U.transpose() , 1e-12));

  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
// std::cout << "v = [" << v.transpose() << "]';" << std::endl;

  Eigen::VectorXd Uv = v; pinocchio::cholesky::Uv(model,data,Uv);
  BOOST_CHECK(Uv.isApprox(U*v, 1e-12));

  Eigen::VectorXd Utv = v; pinocchio::cholesky::Utv(model,data,Utv);
  BOOST_CHECK(Utv.isApprox(U.transpose()*v, 1e-12));

  Eigen::VectorXd Uiv = v; pinocchio::cholesky::Uiv(model,data,Uiv);
  BOOST_CHECK(Uiv.isApprox(U.inverse()*v, 1e-12));

  Eigen::VectorXd Utiv = v; pinocchio::cholesky::Utiv(model,data,Utiv);
  BOOST_CHECK(Utiv.isApprox(U.transpose().inverse()*v, 1e-12));

  Eigen::VectorXd Miv = v; pinocchio::cholesky::solve(model,data,Miv);
  BOOST_CHECK(Miv.isApprox(M.inverse()*v, 1e-12));

  Eigen::VectorXd Mv = v; Mv = pinocchio::cholesky::Mv(model,data,Mv);
  BOOST_CHECK(Mv.isApprox(M*v, 1e-12));
  Mv = v;                 pinocchio::cholesky::UDUtv(model,data,Mv);
  BOOST_CHECK(Mv.isApprox(M*v, 1e-12));
}

/* The flag triger the following timers:
 * 000001: sparse UDUt cholesky
 * 000010: dense Eigen LDLt cholesky (with pivot)
 * 000100: sparse resolution
 * 001000: sparse U*v multiplication
 * 010000: sparse U\v substitution
 * 100000: sparse M*v multiplication without Cholesky
 */
BOOST_AUTO_TEST_CASE ( test_timings )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model,true);
  pinocchio::Data data(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
  crba(model,data,q);

  long flag = BOOST_BINARY(1111111);
  PinocchioTicToc timer(PinocchioTicToc::US);
  #ifdef NDEBUG
    #ifdef _INTENSE_TESTING_
      const size_t NBT = 1000*1000;
    #else
      const size_t NBT = 10;
    #endif
  #else
    const size_t NBT = 1;
    std::cout << "(the time score in debug mode is not relevant)  " ;
  #endif

  bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
  if(verbose) std::cout <<"--" << std::endl;

  if( flag >> 0 & 1 )
    {
      timer.tic();
      SMOOTH(NBT)
      {
	pinocchio::cholesky::decompose(model,data);
      }
      if(verbose) std::cout << "Decompose =\t";
      timer.toc(std::cout,NBT);
    }

  if( flag >> 1 & 1 )
    {
      timer.tic();
      Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
      Eigen::VectorXd res(model.nv);
      SMOOTH(NBT)
      {
	Eigen::LDLT <Eigen::MatrixXd> Mchol(data.M);
	res = Mchol.solve(v);
      }
      if(verbose) std::cout << "Eigen::LDLt =\t";
      timer.toc(std::cout,NBT);
    }

  if( flag >> 2 & 31 )
    {
      std::vector<Eigen::VectorXd> randvec(NBT);
      for(size_t i=0;i<NBT;++i ) randvec[i] = Eigen::VectorXd::Random(model.nv);
      Eigen::VectorXd zero = Eigen::VectorXd(model.nv);
      Eigen::VectorXd res (model.nv);

      if( flag >> 2 & 1 )
	{
	  timer.tic();
	  SMOOTH(NBT)
	  {
	    pinocchio::cholesky::solve(model,data,randvec[_smooth]);
	  }
	  if(verbose) std::cout << "solve =\t\t";
	  timer.toc(std::cout,NBT);
	}

      if( flag >> 3 & 1 )
	{
	  timer.tic();
	  SMOOTH(NBT)
	  {
	    pinocchio::cholesky::Uv(model,data,randvec[_smooth]);
	  }
	  if(verbose) std::cout << "Uv =\t\t";
	  timer.toc(std::cout,NBT);
	}

      if( flag >> 4 & 1 )
	{
	  timer.tic();
	  SMOOTH(NBT)
	  {
	    pinocchio::cholesky::Uiv(model,data,randvec[_smooth]);
	  }
	  if(verbose) std::cout << "Uiv =\t\t";
	  timer.toc(std::cout,NBT);
	}
      if( flag >> 5 & 1 )
	{
	  timer.tic();
	  Eigen::VectorXd res;
	  SMOOTH(NBT)
	  {
	    res = pinocchio::cholesky::Mv(model,data,randvec[_smooth]);
	  }
	  if(verbose) std::cout << "Mv =\t\t";
	  timer.toc(std::cout,NBT);
	}
      if( flag >> 6 & 1 )
	{
    timer.tic();
    SMOOTH(NBT)
    {
      pinocchio::cholesky::UDUtv(model,data,randvec[_smooth]);
    }
    if(verbose) std::cout << "UDUtv =\t\t";
    timer.toc(std::cout,NBT);
	}
    }
}

  BOOST_AUTO_TEST_CASE(test_Minv_from_cholesky)
  {
    using namespace Eigen;
    using namespace pinocchio;

    pinocchio::Model model;
    pinocchio::buildModels::humanoidRandom(model,true);
    pinocchio::Data data(model);

    model.lowerPositionLimit.head<3>().fill(-1.);
    model.upperPositionLimit.head<3>().fill(1.);
    VectorXd q = randomConfiguration(model);
    crba(model,data,q);
    data.M.triangularView<Eigen::StrictlyLower>() =
    data.M.triangularView<Eigen::StrictlyUpper>().transpose();
    MatrixXd Minv_ref(data.M.inverse());

    cholesky::decompose(model,data);
    VectorXd v_unit(VectorXd::Unit(model.nv,0));

    VectorXd Ui_v_unit(model.nv);
    VectorXd Ui_v_unit_ref(model.nv);

    for(int k = 0; k < model.nv; ++k)
    {
      v_unit = VectorXd::Unit(model.nv,k);
      Ui_v_unit.setZero();
      cholesky::internal::Miunit(model,data,k,Ui_v_unit);
      Ui_v_unit_ref = v_unit;
      cholesky::Uiv(model,data,Ui_v_unit_ref);
      Ui_v_unit_ref.array() *= data.Dinv.array();
      cholesky::Utiv(model,data,Ui_v_unit_ref);

      BOOST_CHECK(Ui_v_unit.isApprox(Ui_v_unit_ref));

      Ui_v_unit_ref = v_unit;
      cholesky::solve(model,data,Ui_v_unit_ref);
      BOOST_CHECK(Ui_v_unit.isApprox(Ui_v_unit_ref));

//      std::cout << "Ui_v_unit : " << Ui_v_unit.transpose() << std::endl;
//      std::cout << "Ui_v_unit_ref : " << Ui_v_unit_ref.transpose() << std::endl << std::endl;
    }

    MatrixXd Minv(model.nv,model.nv);
    Minv.setZero();
    cholesky::computeMinv(model,data,Minv);

    BOOST_CHECK(Minv.isApprox(Minv_ref));

    // Check second call to cholesky::computeMinv
    cholesky::computeMinv(model,data,Minv);
    BOOST_CHECK(Minv.isApprox(Minv_ref));

    // Call the second signature of cholesky::computeMinv
    Data data_bis(model);
    crba(model,data_bis,q);
    cholesky::decompose(model,data_bis);
    cholesky::computeMinv(model,data_bis);
    BOOST_CHECK(data_bis.Minv.isApprox(Minv_ref));
  }

BOOST_AUTO_TEST_SUITE_END ()


///////////////////////////////////////////////////////////////
//  Copyright 2021 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt
//
// Simple test that cpp_int -> double conversion has less than 0.5ulp error
// and rounds to even in case of ties.
// See https://github.com/boostorg/multiprecision/issues/360.
//
#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include "test.hpp"

using namespace boost::multiprecision;

#ifdef BOOST_MSVC
#pragma warning(disable : 4127)
#endif

template <class T>
T generate_random(unsigned bits_wanted)
{
   static boost::random::mt19937               gen;
   typedef boost::random::mt19937::result_type random_type;

   T        max_val;
   unsigned digits;
   if (std::numeric_limits<T>::is_bounded && (bits_wanted == (unsigned)std::numeric_limits<T>::digits))
   {
      max_val = (std::numeric_limits<T>::max)();
      digits  = std::numeric_limits<T>::digits;
   }
   else
   {
      max_val = T(1) << bits_wanted;
      digits  = bits_wanted;
   }

   unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
   while ((random_type(1) << bits_per_r_val) > (gen.max)())
      --bits_per_r_val;

   unsigned terms_needed = digits / bits_per_r_val + 1;

   T val = 0;
   for (unsigned i = 0; i < terms_needed; ++i)
   {
      val *= (gen.max)();
      val += gen();
   }
   val %= max_val;
   return val;
}

template <class From, class To>
void test_convert()
{
   boost::random::mt19937 gen;
   boost::random::uniform_int_distribution<> d(20, (std::min)(200, std::numeric_limits<To>::max_exponent - 2));

   for (unsigned i = 0; i < 10000; ++i)
   {
      int  bits = d(gen);
      From from = generate_random<From>(bits);
      To   t1(from);
      From b(t1);
      std::size_t m = msb(from);
      if (m >= std::numeric_limits<To>::digits)
      {
         // For error <= 1ulp
         // Note msb(from) returns one less than the number of bits in from:
         From max_error = (From(1) << (m - std::numeric_limits<To>::digits));
         BOOST_TEST_GE(max_error, abs(b - from));
         if (max_error < abs(b - from))
            // debugging help:
            std::cout << from << std::endl
                      << b << std::endl
                      << abs(b - from) << std::endl;
         if (max_error == abs(b - from))
         {
            // Check we rounded to even in case of tie:
            BOOST_TEST_GT(lsb(b), (1 + msb(from) - std::numeric_limits<To>::digits));
            if (lsb(b) <= (1 + msb(from) - std::numeric_limits<To>::digits))
            {
               // debugging help:
               std::cout << from << std::endl
                         << b << std::endl
                         << abs(b - from) << std::endl;
            }
         }
      }
      else
      {
         BOOST_TEST_EQ(b, from);
      }
   }
}

int main()
{
   test_convert<cpp_int, float>();
   test_convert<cpp_int, double>();
   return boost::report_errors();
}


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

// Written by Simon Praetorius

#ifndef ITL_GCR_INCLUDE
#define ITL_GCR_INCLUDE

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

namespace itl
{

  template <typename Matrix, typename Vector, typename Preconditioner, typename Iteration>
  int gcr_full(const Matrix& A, Vector& x, const Vector& b, const Preconditioner& P, Iteration& iter)
  {
    using math::reciprocal;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type Size;

    if (size(b) == 0)
      throw mtl::logic_error("empty rhs vector");

    Scalar zero= math::zero(b[0]), dbl_tol= 1.e-16; // TODO: tolerance as parameter or otherwise generalized

    Size k, kmax(std::min(size(x), Size(iter.max_iterations() - iter.iterations())));

    Vector r(b - A * x);
    Vector uk(size(x), zero), ck(size(x), zero);
    mtl::matrix::multi_vector<Vector> C(Vector(resource(x), zero), kmax);
    mtl::matrix::multi_vector<Vector> U(Vector(resource(x), zero), kmax);

    Scalar res(two_norm(r));
    Scalar alpha(zero), beta(zero), gamma(zero);

    for (k= 0; k < kmax && !iter.finished(res); ++k, ++iter)
    {
      uk = solve(P, r);
      // In order to avoid breakdown, use LSQR switch
      // Cite: C. Vuik, Further experiences with GMRESR, 1993
      //  if (two_norm(uk) < dbl_tol)
      //    uk = trans(A) * r; // requires transposed multiplication

      ck = A * uk;
      for (size_t i = 0; i < k; i++)
      {
        alpha = dot(C.vector(i), ck);

        ck -= alpha * C.vector(i);
        uk -= alpha * U.vector(i);
      }

      beta = two_norm(ck);
      if (beta < dbl_tol)
        return iter.fail(2, "search direction close to 0");

      C.vector(k) = ck * reciprocal(beta);
      U.vector(k) = uk * reciprocal(beta);

      gamma = dot(C.vector(k), r);
      x += U.vector(k) * gamma;
      r -= C.vector(k) * gamma;

      res = two_norm(r);
    }

    return iter;
  }

#if 0
  template <typename Matrix, typename Vector, typename LeftPreconditioner, typename RightPreconditioner, typename Iteration>
  int gmresr_trunc(const Matrix& A, Vector& x, const Vector& b, const LeftPreconditioner& L, const RightPreconditioner& R,
                   Iteration& iter, , typename mtl::Collection<Vector>::size_type nTrunc)
  {
TODO:
    implement truncated GCR solver, instead of restarted version
Cite:
    C. Vuik, Further experiences with GMRESR, 1993
  }
#endif

  /// Generalized Conjugate Residual method with restart
  /// Cite: S.C. Eisenstat, H.C. Elman, M.H. Schultz, Variational iterative methods for non symmetric systems of linear equations, 1983
  /// Cite: C. Vuik, New insight in GMRES-like methods with variable preconditioners
  template <typename Matrix, typename Vector, typename LeftPreconditioner,
            typename RightPreconditioner, typename Iteration>
  int gcr(const Matrix& A, Vector& x, const Vector& b, LeftPreconditioner& /*L*/, RightPreconditioner& R,
          Iteration& iter, typename mtl::Collection<Vector>::size_type restart)
  {
    do
    {
      Iteration inner(iter);
      inner.set_max_iterations(std::min(int(iter.iterations()+restart), iter.max_iterations()));
      inner.suppress_resume(true);
      gcr_full(A, x, b, R, inner);
      iter.update_progress(inner);
    }
    while (!iter.finished());

    return iter;
  }
} // namespace itl;

#endif // ITL_GMR_INCLUDE


//
// Created by Shiina Miyuki on 2019/1/30.
//
#include "../core/transform.h"
#include "../core/interaction.h"
#define BOOST_AUTO_TEST_MAIN
#define  BOOST_TEST_MODULE TransformTest

#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_log.hpp>
#include <boost/filesystem/fstream.hpp>

namespace utf = boost::unit_test;
using namespace Miyuki;
BOOST_AUTO_TEST_CASE(TestIndentity, *utf::tolerance(float(0.0001))) {
    std::random_device rd;
    std::uniform_real_distribution<Float> dist(-1.0f, 1.f);
    auto I = Matrix4x4::identity();
    for (int i = 0; i < 10; i++) {
        Vec3f v(dist(rd),dist(rd),dist(rd));
        Vec3f v2 = v;
        v = I.mult(v);
        BOOST_TEST(v.x() == v2.x());
        BOOST_TEST(v.y() == v2.y());
        BOOST_TEST(v.z() == v2.z());
    }
}
BOOST_AUTO_TEST_CASE(TestInverse, *utf::tolerance(float(0.001))) {
    std::random_device rd;
    std::uniform_real_distribution<Float> dist(-1.0f, 1.f);
    for (int i = 0; i < 10; i++) {
        Matrix4x4 m;
        for(int k = 0;k<16;k++){
            m[k] = dist(rd);
        }
        Matrix4x4 inv;
        if(Matrix4x4::inverse(m, inv)){
            Vec3f v(dist(rd),dist(rd),dist(rd));
            Vec3f v2 = v;
            auto I = inv.mult(m);
            v = inv.mult(m.mult(v));
            BOOST_TEST(v.x() == v2.x());
            BOOST_TEST(v.y() == v2.y());
            BOOST_TEST(v.z() == v2.z());
        }
    }
}

BOOST_AUTO_TEST_CASE(TestTranslation, *utf::tolerance(float(0.0001))) {
    std::random_device rd;
    std::uniform_real_distribution<Float> dist(-10000.0f, 10000.0f);
    auto t = Transform();
    auto m = Matrix4x4::identity();
    for (int i = 0; i < 10; i++) {
        Vec3f tr(dist(rd), dist(rd), dist(rd));
        m = m.mult(Matrix4x4::translation(tr));
        t.translation += tr;
        Vec3f v(dist(rd), dist(rd), dist(rd));
        v.w() = 1;
        auto a = m.mult(v);
        auto b = t.apply(v);
        BOOST_TEST(a.x() == b.x());
        BOOST_TEST(a.y() == b.y());
        BOOST_TEST(a.z() == b.z());
    }
}
BOOST_AUTO_TEST_CASE(TestWorldLocal, *utf::tolerance(float(0.001))) {
    std::random_device rd;
    std::uniform_real_distribution<Float> dist(-1.0f, 1.f);
    Interaction interaction;
    interaction.normal = Vec3f{dist(rd), dist(rd), dist(rd)};
    interaction.normal.normalize();
    interaction.computeLocalCoordinate();
    Vec3f v(dist(rd), dist(rd), dist(rd));
    v.normalize();
    Vec3f v3 = interaction.worldToLocal(v);
    Vec3f v2 = interaction.localToWorld(v3);
    BOOST_TEST(v.x() == v2.x());
    BOOST_TEST(v.y() == v2.y());
    BOOST_TEST(v.z() == v2.z());
}


/**
 * \file hungarian.hpp
 * \brief
 *
 * \author Andrew Price
 * \date 2016-4-19
 *
 * \copyright
 *
 * Copyright (c) 2020, Andrew Price
 * All rights reserved.
 *
 * This file is provided under the following "BSD-style" License:
 * Redistribution and use in source and binary forms, with or
 * without modification, are permitted provided that the following
 * conditions are met:
 * * Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above
 *   copyright notice, this list of conditions and the following
 *   disclaimer in the documentation and/or other materials provided
 *   with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef HUNGARIAN_HPP
#define HUNGARIAN_HPP

#include <Eigen/Core>
#include <vector>

// Implementation of the Hungarian Algorithm derived from https://www.topcoder.com/community/data-science/data-science-tutorials/assignment-problem-and-hungarian-algorithm/

namespace mps
{

// TODO: template for multiple cost matrix types

/**
 * @brief The Hungarian class solves the optimal nxn assignment problem in O(n^3) time.
 * The results are reported as a mapping from x to y (row to column of cost matrix)
 *
 * Formally, the system is defined as follows:
 * Given two sets \f$ X, Y \in \mathbb{R}^n \f$ and a cost matrix \f$ C:X \times Y \rightarrow \mathbb{R} \f$,
 * find a bijection \f$ f:X \rightarrow Y \f$ that minimizes the cost function \f$ \sum_{x\in X}C[x,f(x)] \f$.
 */
class Hungarian
{
public:
	Hungarian(const Eigen::MatrixXd& cost);

	/**
	 * @brief getSolutionCost returns the current linear cost of the assignment
	 * @return Solution cost of the form C(x1,xToY(x1)) + C(x2,xToY(x2)) + ...
	 */
	double getSolutionCost();

	/**
	 * @brief getAssignment Returns the optimal choice of y for each x in sequence
	 * @return y-index of optimal pairing for each x
	 */
	const std::vector<int>& getAssignment() { return xy; }

protected:
	Eigen::MatrixXd C;            ///< Cost matrix (nxn)
	const int N;                  ///< Number of pairings to find
	int maxMatch;                 ///< Number of pairings locked so far
	std::vector<double> lx;
	std::vector<double> ly;
	std::vector<int> xy;          ///< Mapping from x to y index
	std::vector<int> yx;          ///< Mapping from y to x index
	std::vector<double> slack;
	std::vector<double> slackx;
	std::vector<int> prev;        ///< Alternating tree
	std::vector<bool> S;          ///< Set of matched x indices
	std::vector<bool> T;          ///< Set of matched y indices

	void augment();
	void relabel();
	void addToTree(int x, int prevx);

};

double Hungarian::getSolutionCost()
{
	double cost = 0;
	for (int x = 0; x < N; ++x)
	{
		cost += C(x, xy[x]);
	}
	return -cost;
}

Hungarian::Hungarian(const Eigen::MatrixXd& cost)
    : C(-cost), // Negate cost, as this algorithm finds the maximum assignment
      N(C.cols()),
      maxMatch(0),
      lx(N, 0),
      ly(N, 0),
      xy(N,-1),
      yx(N,-1),
      slack(N),
      slackx(N),
      prev(N),
      S(N),
      T(N)
{
	// Initial labelling
	for (int x = 0; x < N; ++x)
		for (int y = 0; y < N; ++y)
			lx[x] = std::max(lx[x], C(x,y));

	// Solve
	augment();
}

void Hungarian::relabel()
{
	double delta = std::numeric_limits<double>::infinity();

	for (int y = 0; y < N; ++y) // compute delta with slack
		if (!T[y]) { delta = std::min(delta, slack[y]); }
	assert(std::isfinite(delta));
	for (int x = 0; x < N; ++x) // update X labels
		if (S[x]) { lx[x] -= delta; }
	for (int y = 0; y < N; ++y) // update Y labels
		if (T[y]) { ly[y] += delta; }
	for (int y = 0; y < N; ++y) // update slack array
		if (!T[y]) { slack[y] -= delta; }
}

void Hungarian::addToTree(int x, int prevx)
{
	S[x] = true;
	prev[x] = prevx;
	for (int y = 0; y < N; ++y)
	{
		double newCost = lx[x] + ly[y] - C(x,y);
		if (newCost < slack[y])
		{
			slack[y] = newCost;
			slackx[y] = x;
		}
	}
}

void Hungarian::augment()
{
	if (maxMatch == N) { return; }
	int x = -1, y = -1, root = -1;
	std::vector<int> q(N);
	int wr = 0, rd = 0;

	// Initialize sets and tree
	std::fill(S.begin(), S.end(), false);
	std::fill(T.begin(), T.end(), false);
	std::fill(prev.begin(), prev.end(), -1);

	// Find root of tree
	for (x = 0; x < N; ++x)
	{
		if (-1 == xy[x])
		{
			q[wr++] = root = x;
			prev[x] = -2;
			S[x] = true;
			break;
		}
	}

	// Initialize slack array
	for (y = 0; y < N; ++y)
	{
		slack[y] = lx[root] + ly[y] - C(root,y);
		slackx[y] = root;
	}

	while (true)
	{
		while (rd < wr) // build tree with bfs
		{
			x = q[rd++];
			for (y = 0; y < N; ++y)
			{
				if (C(x,y) == lx[x] + ly[y] && !T[y])
				{
					if (-1 == yx[y]) { break; } // found an isolated Y vertex
					T[y] = true; // else y is in T
					q[wr++] = yx[y]; // add target of y to frontier of bfs
					addToTree(yx[y], x);
				}
			}
			if (y < N) { break; } // Breaking out from inmost loop
		}
		if (y < N) { break; } // Breaking out from inmost loop

		relabel();
		wr = rd = 0;

		for (y = 0; y < N; ++y)
		{
			if (!T[y] && 0 == slack[y])
			{
				if (-1 == yx[y])
				{
					x = slackx[y];
					break;
				}
				else
				{
					T[y] = true;
					if (!S[yx[y]])
					{
						q[wr++] = yx[y];
						addToTree(yx[y], slackx[y]);
					}
				}
			}
		}
		if (y < N) { break; }
	}

	if (y < N)
	{
		maxMatch++;
		for (int cx = x, cy = y, ty; cx != -2; cx = prev[cx], cy = ty)
		{
			ty = xy[cx];
			yx[cy] = cx;
			xy[cx] = cy;
		}
		augment();
	}
}

/*
void hungarian(Eigen::MatrixXd& C)
{
	assert(C.rows() == C.cols());
	const size_t N = C.cols();

	// Initialize
	hungarianInit(C);

}

void hungarianInit(Eigen::MatrixXd& C)
{
	// Subtract smallest element from each row
	for (size_t i = 0; i < N; ++i)
	{
		double rowMin = C(i,0);
		for (size_t j = 0; j < N; ++j)
			if (C(i,j) < rowMin) { rowMin = C(i,j); }
		for (size_t j = 0; j < N; ++j)
			C(i,j) -= rowMin;
	}
	// Subtract smallest element from each column
	for (size_t j = 0; j < N; ++j)
	{
		double colMin = C(0,j);
		for (size_t i = 0; i < N; ++i)
			if (C(i,j) < colMin) { colMin = C(i,j); }
		for (size_t i = 0; i < N; ++i)
			C(i,j) -= colMin;
	}
}
*/
} // namespace gtri
#endif // HUNGARIAN_HPP


#include "CEGO/concurrentqueue.h"
#include "CEGO/CEGO.hpp"
#include <Eigen/Dense>
#if defined(PYBIND11)
#include <pybind11/embed.h>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
namespace py = pybind11;
#include <atomic>
#include <fstream>

std::atomic_size_t Ncalls(0);

inline bool ValidNumber(double x)
{
    // Idea from http://www.johndcook.com/IEEE_exceptions_in_cpp.html
    return (x <= DBL_MAX && x >= -DBL_MAX);
};

class Antoine {
public:
    double m_Tt, m_Tc, m_pc, m_Dc;
    Eigen::ArrayXd m_LHS, m_T;
    std::string m_name;
    Antoine(const std::string &name) :  m_name(name)
    {
        py::module ct = py::module::import("ctREFPROP.ctREFPROP"); // Import cytpes wrapper of REFPROP dll
        std::string RPPREFIX(getenv("RPPREFIX"));
        auto R = ct.attr("REFPROPFunctionLibrary")(RPPREFIX + "/REFPRP64.DLL");
        auto version = R.attr("RPVersion")().cast<std::string>();
        auto setup_info = R.attr("SETUPdll")(1, name+".FLD", "HMX.BNC","DEF");
        if (setup_info.attr("ierr").cast<int>() != 0) {
            throw std::invalid_argument( setup_info.attr("herr").cast<std::string>() );
        }
        auto info = R.attr("INFOdll")(1);
        m_pc = info.attr("Pc").cast<double>();
        m_Tc = info.attr("Tc").cast<double>();
        m_Tt = info.attr("Ttrp").cast<double>();
        m_Dc = info.attr("Dc").cast<double>();

        std::size_t N = 300;
        m_LHS.resize(N);
        m_T.resize(N);
        std::vector<double> z(1,1);
        Eigen::ArrayXd Tvec = Eigen::ArrayXd::LinSpaced(N, m_Tc*0.999, 0.99*m_Tt);
        Eigen::Index j = 0;

        for (auto i = 0; i < Tvec.size(); ++i){
            auto o = R.attr("TQFLSHdll")(Tvec[i], 1, &z, 0);
            int ierr = o.attr("ierr").cast<int>();

            if (ierr <= 100){
                double p = o.attr("P").cast<double>();
                if (!ValidNumber(p)){ continue; }
                m_LHS(j) = p;
            }
            m_T(j) = Tvec(i);
            j++;
        }
        m_LHS.conservativeResize(j);
        m_T.conservativeResize(j);
    }
    void plot_curve(){
        #if defined(PYBIND11)
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(1/m_T, m_LHS);
        plt.attr("show")();
        #else
        std::cout << "No pybind11 support, so no plots\n";
        #endif
    }
    Eigen::ArrayXd eval_RHS(const Eigen::ArrayXd& T, const Eigen::ArrayXd &c) {
        return c[3]*pow(10,c[0]-c[1]/(c[2]+T));
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const std::vector<double> &c = static_cast<const CEGO::NumericalIndividual<double>*>(pind)->get_coefficients();
        return objective(Eigen::Map<const Eigen::ArrayXd>(&(c[0]), c.size()) );
    }
    double objective(const Eigen::ArrayXd &c) {
        Ncalls++;
        double ssq = ((eval_RHS(m_T, c) - m_LHS)/m_LHS).square().sum();
        return ssq;
    }
    void plot_trace(const std::vector<double> &best_costs) {
        #if defined(PYBIND11)
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(best_costs);
        plt.attr("show")();
        #else
        std::cout << "No pybind11 support, so no plots\n";
        #endif
    }
    const Eigen::ArrayXd &get_T(){ return m_T; }
};

int main()
{
    py::scoped_interpreter interp{};
    std::srand((unsigned int)time(0));

    // Construct the bounds
    std::vector<CEGO::Bound> bounds;
    for (auto i = 0; i < 3; ++i) {
        bounds.push_back(CEGO::Bound(std::pair<double, double>(-10000, 10000)));
    }
    for (auto i = 0; i < 1; ++i) {
        bounds.push_back(CEGO::Bound(std::pair<double, double>(0.1,100)));
    }
    Antoine rp("PROPANE");
    //rp.plot_curve();

    CEGO::CostFunction cost_wrapper = std::bind((double (Antoine::*)(const CEGO::AbstractIndividual *)) &Antoine::objective, &rp, std::placeholders::_1);
    auto Nlayers = 5;
    auto layers = CEGO::Layers<double>(cost_wrapper, bounds.size(), 2000, Nlayers, 3);
    layers.parallel = false;
    layers.set_bounds(bounds);

    auto flags = layers.get_evolver_flags();
    flags["Nelite"] = 2;
    flags["Fmin"] = 0.5;
    flags["Fmax"] = 1.0;
    flags["CR"] = 0.7;
    layers.set_evolver_flags(flags);

    std::vector<double> best_costs;
    std::vector<std::vector<double> > objs;
    double VTR = 1e-6, best_cost = 999999.0;
    auto startTime = std::chrono::system_clock::now();
    for (auto counter = 0; counter < 3000; ++counter) {
        layers.do_generation();

        // Store the best objective function in each layer
        std::vector<double> oo;
        for (auto &&cost_coefficients : layers.get_best_per_layer()) {
            oo.push_back(std::get<0>(cost_coefficients));
        }
        objs.push_back(oo);

        // For the overall best result, print it, and write JSON to file
        auto best_layer = layers.get_best();
        best_cost = std::get<0>(best_layer); best_costs.push_back(best_cost);
        if (counter % 50 == 0) {
            std::cout << counter << ": best: " << best_cost << "\n ";// << CEGO::vec2string(best_coeffs) << "\n";
        }
        if (best_cost < VTR){ break; }
    }
    auto best_layer = layers.get_best();
    auto c = std::get<1>(best_layer);
    auto endTime = std::chrono::system_clock::now();
    double elap = std::chrono::duration<double>(endTime - startTime).count();

    // Get the results stored in the thread-safe queue
    std::vector<CEGO::Result> results; Eigen::MatrixXd mat;
    results = layers.get_results();

    std::cout << "cbest" << Eigen::Map<Eigen::ArrayXd>(&(c[0]), c.size()) << std::endl;

    std::cout << "run:" << elap << " s\n";
    std::cout << "NFE:" << Ncalls << std::endl;
}

#else

int main(){
    std::cout << "Due to lack of pybind11 support, this file cannot be run\n";
}

#endif


#include <stdlib.h>
#include <math.h>
#include <stdio.h>

#include <NTL/config.h>

#ifdef NTL_GMP_HACK

#include <gmp.h>
#include <NTL/mach_desc.h>

int gcd(int a, int b)
{
   int u, v, t, x;

   if (b==0)
      x = a;
   else {
      u = a;
      v = b;
      do {
         t = u % v;
         u = v;
         v = t;
      } while (v != 0);

      x = u;
   }

   return x;
}

int pow2(int a)
{
   int m;
   int k;

   m = 1;
   k = 0;

   while (m < a) {
      m = 2*m;
      k++;
   }

   if (m == a)
      return k;
   else
      return -1;
}

const char *accum[2] = { "", "yy | " };

void lip_to_gmp(int A, int B)
{
   int d, na, nb;
   int i, j, r;
   int xx, yy;
   int shamt;

   int iter, na2, nb2;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   na2 = pow2(na);
   nb2 = pow2(nb);

   printf("static\n");
   printf("void lip_to_gmp(const long *x, mp_limb_t *y, long n)\n");
   printf("{\n");
   printf("   long r, q, xx;\n");
   printf("   mp_limb_t yy;\n\n");

   if (na2 != -1) {
      printf("   r = n & %d;\n", na-1);
      printf("   q = n >> %d;\n", na2);
   }
   else {
      printf("   r = n % %d;\n", na);
      printf("   q = n / %d;\n", na);
   }

   printf("\n");

   printf("   if (q > 0) {\n");

   if (na2 != -1)
      printf("      x += (q << %d);\n", na2);
   else
      printf("      x += (q * %d);\n", na);

   if (nb2 != -1)
      printf("      y += (q << %d);\n", nb2);
   else if (pow2(nb+1) != -1)
      printf("      y += (q << %d) - q;\n", pow2(nb+1));
   else
      printf("      y += (q * %d);\n", nb);

   printf("   }\n");

   printf("\n");
   printf("   if (r > 0) {\n");
   printf("      yy = 0;\n");
   printf("\n");

   printf("      switch (r-1) {\n");

   xx = 0;
   yy = 0;

   for (i = 1; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      if (xx) {
         printf("         ");
         printf("yy = %s (((mp_limb_t)(xx)) << %d);\n", accum[yy], B-r);
         yy = 1;
      }

      printf("      ");
      printf("case %d:\n", na-1-i);

      shamt = A-B+r;

      printf("         ");

      if (shamt >= 0) {
         printf("y[%d] = ", nb-1-j);
      }
      else {
         printf("yy = ");
      }

      printf("%s", accum[yy]);

      if (shamt == 0) {
         printf("x[0];\n");
         break;
      }
      else if (shamt < 0) {
         printf("(((mp_limb_t)(x[%d])) << %d);\n", na-1-i, -shamt);
         xx = 0;
         yy = 1;
      }
      else {
         printf("((xx=x[%d]) >> %d);\n", na-1-i, shamt);
         xx = 1;
         yy = 0;
      }
   }

   printf("      }\n\n");
   printf("      n -= r;\n");
   printf("   }\n\n");
   printf("   while (n > 0) {\n");
   printf("      x -= %d;\n", na);
   printf("      n -= %d;\n", na);
   printf("      y -= %d;\n", nb);
   printf("\n");

   xx = 0;
   yy = 0;

   for (i = 0; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      if (xx) {
         printf("      ");
         printf("yy = %s (((mp_limb_t)(xx)) << %d);\n", accum[yy], B-r);
         yy = 1;
      }

      shamt = A-B+r;

      printf("      ");

      if (shamt >= 0) {
         printf("y[%d] = ", nb-1-j);
      }
      else {
         printf("yy = ");
      }

      printf("%s", accum[yy]);

      if (shamt == 0) {
         printf("x[0];\n");
         break;
      }
      else if (shamt < 0) {
         printf("(((mp_limb_t)(x[%d])) << %d);\n", na-1-i, -shamt);
         xx = 0;
         yy = 1;
      }
      else {
         printf("((xx=x[%d]) >> %d);\n", na-1-i, shamt);
         xx = 1;
         yy = 0;
      }
   }

   printf("   }\n");
   printf("}\n");
}

void gmp_to_lip(int A, int B, int alt)
{
   int d, na, nb;
   int i, j, r;
   int shamt;

   int na2, nb2;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   na2 = pow2(na);
   nb2 = pow2(nb);

   printf("static\n");
   if (!alt)
      printf("void gmp_to_lip(long *x, const mp_limb_t *y, long n)\n");
   else
      printf("void gmp_to_lip1(long *x, const mp_limb_t *y, long n)\n");
   printf("{\n");
   printf("   long r, q; unsigned long xx;\n");
   printf("   mp_limb_t yy;\n\n");

   if (na2 != -1) {
      printf("   r = n & %d;\n", na-1);
      printf("   q = n >> %d;\n", na2);
   }
   else {
      printf("   r = n % %d;\n", na);
      printf("   q = n / %d;\n", na);
   }

   printf("\n");

   printf("   if (q > 0) {\n");

   if (na2 != -1)
      printf("      x += (q << %d);\n", na2);
   else
      printf("      x += (q * %d);\n", na);

   if (nb2 != -1)
      printf("      y += (q << %d);\n", nb2);
   else if (pow2(nb+1) != -1)
      printf("      y += (q << %d) - q;\n", pow2(nb+1));
   else
      printf("      y += (q * %d);\n", nb);

   printf("   }\n");

   printf("\n");

   printf("   if (r > 0) {\n");

   if (!alt) {
      if (na - nb == 1)
         printf("      yy = y[r-1];\n");
      else if (na2 != -1)
         printf("      yy = y[((r*%d + %d) >> %d) - 1];\n", nb, na-1, na2);
      else
         printf("      yy = y[((r*%d + %d) / %d) - 1];\n", nb, na-1, na);

   }
   else
      printf("      yy = 0;\n");

   printf("\n");

   printf("      switch (r-1) {\n");

   for (i = 1; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      printf("      ");
      printf("case %d:\n", na-1-i);

      shamt = A-B+r;

      if (shamt > 0) {
         printf("         ");
         printf("xx = ((unsigned long)(yy)) << %d;\n", shamt);
         printf("         ");
         printf("x[%d] = (xx | ((unsigned long)((yy = y[%d]) >> %d))) & NTL_RADIXM;\n",
                na-1-i, nb-2-j, 2*B-r-A);
      }
      else if (shamt == 0) {
         printf("         ");
         printf("x[0] = ((unsigned long)(yy)) & NTL_RADIXM;\n");
         break;
      }
      else {
         printf("         ");
         printf("x[%d] = ((unsigned long)(yy >> %d)) & NTL_RADIXM;\n", na-1-i, -shamt);
      }
   }

   printf("      }\n\n");
   printf("      n -= r;\n");
   printf("   }\n\n");
   printf("   while (n > 0) {\n");
   printf("      x -= %d;\n", na);
   printf("      n -= %d;\n", na);
   printf("      y -= %d;\n", nb);
   printf("\n");

   printf("      yy = y[%d];\n", nb-1);

   for (i = 0; ; i++) {
      j = (i*A)/B;
      r = (i*A)%B;

      shamt = A-B+r;

      if (shamt > 0) {
         printf("      ");
         printf("xx = ((unsigned long)(yy)) << %d;\n", shamt);
         printf("      ");
         printf("x[%d] = (xx | ((unsigned long)((yy = y[%d]) >> %d))) & NTL_RADIXM;\n",
                na-1-i, nb-2-j, 2*B-r-A);
      }
      else if (shamt == 0) {
         printf("      ");
         printf("x[0] = ((unsigned long)(yy)) & NTL_RADIXM;\n");
         break;
      }
      else {
         printf("      ");
         printf("x[%d] = ((unsigned long)(yy >> %d)) & NTL_RADIXM;\n", na-1-i, -shamt);
      }
   }

   printf("   }\n");
   printf("}\n");

}

void rdup(int a, int b)
{
   printf("((");
   if (pow2(a) != -1) {
      printf("(x << %d)", pow2(a));
   }
   else if (pow2(a+1) != -1) {
      printf("((x << %d) - x)", pow2(a+1));
   }
   else {
      printf("(x*%d)", a);
   }

   printf(" + %d)", b-1);

   if (pow2(b) != -1) {
      printf(" >> %d)", pow2(b));
   }
   else {
      printf(" / %d)", b);
   }
}

void G_TO_L(int A, int B)
{
   int d, na, nb;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   printf("#define G_TO_L(x) ");

   rdup(na, nb);

   printf("\n");
}

void L_TO_G(int A, int B)
{
   int d, na, nb;

   d = gcd(A, B);
   na = B/d;
   nb = A/d;

   printf("#define L_TO_G(x) ");

   rdup(nb, na);

   printf("\n");
}

void L_TO_G_CHECK(int BPL, int BPI)
{
   if (BPL != BPI) {
      printf("#define L_TO_G_CHECK_LEN\n");
   }

}

void Error(const char *s)
{
   fprintf(stderr, "%s\n", s);
   abort();
}

int main()
{
   mpz_t tt;
   int A, B, BPL, BPI;

   A = NTL_NBITS_MAX;

   /*
    * We compute B as the number of bits of a gmp limb.
    * We require that this quantity correspond to the number of bits
    * of a long, or possibly a "long long" that is twice as
    * wide as a long.  These restrictions may not be entirely
    * necessary, but they are satisfied on all platforms that I know of.
    */

   if (sizeof(mp_limb_t)==sizeof(long) &&
            mp_bits_per_limb == NTL_BITS_PER_LONG)

      B = NTL_BITS_PER_LONG;

   else if (sizeof(mp_limb_t) == 2*sizeof(long) &&
            mp_bits_per_limb == 2*NTL_BITS_PER_LONG)

      B = 2*NTL_BITS_PER_LONG;

   else
      Error("sorry...this is a funny gmp");

   /*
    * The following test is a bit redundant, but it doesn't hurt.
    */

   if (A >= B) Error("sorry...this is a funny gmp");

   /*
    * Next, we check if either the _mp_size field of an mpz struct
    * or the type mp_size_t is narrower than type "long".
    * This is done to enable some overflow checks.
    * For simplicity, we require that the sizeof
    * these types is that of an int or a long, and we also make
    * the somewhat DIRTY assumption that this sizeof value implies
    * a corresponding bit count.  Since this assumption is true
    * on all platforms that I know of, and since this assumption
    * only affects some overflow tests, it seems reasonable.
    */

   if (sizeof(tt->_mp_size) == sizeof(int))
      BPI = NTL_BITS_PER_INT;
   else if (sizeof(tt->_mp_size) == sizeof(long))
      BPI = NTL_BITS_PER_LONG;
   else
      Error("sorry...this is a funny gmp");

   if (sizeof(mp_size_t) != sizeof(int) && sizeof(mp_size_t) != sizeof(long))
      Error("sorry...this is a funny gmp");

   if (sizeof(mp_size_t) < sizeof(tt->_mp_size))
      BPI = NTL_BITS_PER_INT;

   BPL = NTL_BITS_PER_LONG;

   fprintf(stderr, "\ngmp looks OK...%d, %d, %d, %d.\n", A, B, BPI, BPL);
   fprintf(stderr, "generating file lip_gmp_aux.c.\n");

   lip_to_gmp(A, B);
   printf("\n");

   gmp_to_lip(A, B, 0);
   printf("\n");
   gmp_to_lip(A, B, 1);
   printf("\n");

   G_TO_L(A, B);
   L_TO_G(A, B);
   L_TO_G_CHECK(BPL, BPI);

   printf("#define HAVE_LIP_GMP_AUX\n");

   return 0;
}

#else

int main()
{
   fprintf(stderr, "NTL_GMP_HACK flag not set.\n");
   return 0;
}

#endif


/*
 * MCSatSamplePerfectlyStrategy.cpp
 *
 *  Created on: Apr 23, 2012
 *      Author: selman.joe@gmail.com
 */

#include <cstdlib>
#include <boost/random/bernoulli_distribution.hpp>
#include "MCSatSamplePerfectlyStrategy.h"
#include "../logic/Domain.h"

void MCSatSamplePerfectlyStrategy::sampleSentences(const Model& m, const Domain& d, boost::mt19937& rng, std::vector<ELSentence>& sampled) {
    // sample on an interval basis for each formula
    for (Domain::formula_const_iterator it = d.formulas_begin(); it != d.formulas_end(); it++) {
        ELSentence curSentence = *it;

        if (curSentence.hasInfWeight()) {
            sampled.push_back(curSentence); // have to take it
            continue;
        }
        SISet satisfied = curSentence.dSatisfied(m, d);

        double prob = 1.0 - exp(-(double)(curSentence.weight()));   // probability to sample an interval
        SISet where(false, d.maxInterval());
        // iterate over each interval, sampling!
        for (SISet::const_iterator sisetIt = satisfied.begin(); sisetIt != satisfied.end(); sisetIt++) {
            SpanInterval si = *sisetIt;
            for (SpanInterval::const_iterator siIt = si.begin(); siIt != si.end(); siIt++) {
                Interval interval = *siIt;
                // sample it with probability 1-exp(-w)
                boost::bernoulli_distribution<double> flip(prob);
                if (flip(rng)) {
                    where.add(SpanInterval(interval.start(), interval.start(), interval.finish(), interval.finish()));
                }
            }
        }
        if (!where.empty()) {
            curSentence.setQuantification(where);
            sampled.push_back(curSentence);
        }
    }
}


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

  @copyright 2017 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_ALGORITHM_MAX_VAL_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_MAX_VAL_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/maximum.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/is_aligned.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns the value of the element with the smallest value in the range [first,last),
    The largest possible value for the order if the range is empty.

    @param first  Beginning of the range of elements to max_val
    @param last   End of the range of elements to max_val
    @param comp   comparison function object that will be applied.

    @par Requirement

      - @c first and @c last must be pointer to Vectorizable type.

      - @c comp must be a polymorphic unary function object, i.e callable on generic types.
      - if @c comp is not present the function test is done with operator <

    @par Example

    The following code uses simd::max_val to find the greatest and smallest element
    @c std::vector.
    @snippet max_val.cpp max_val
    @snippet max_val.txt max_val

    @return the maximum value of the range elements.
  **/
  template<typename T, typename Comp>
  T max_val(T const* first, T const* last, Comp comp)
  {
    if (first == last) return comp(T(0), T(1)) ? Inf<T>() : Minf<T>();
    auto pr = segmented_aligned_range(first,last);

    T m = *first;
    for( T e : pr.head ) { if (comp(m, e))  m = e; }

    // main SIMD part
    pack<T> mm(m);
    for(pack<T> e : pr.body ) mm =  if_else(comp(m, e), e, m);

    m =  mm[0];
    for(T v : mm) if(comp(m, v)) m = v;

    for( T e : pr.tail ) { if(comp(m, e))  m = e; }

    return m;
  }

  /*!
    @ingroup group-algo

    Returns the value of the element with the smallest value in the range [first,last),
    The largest possible value for the order if the range is empty.

    @param first  Beginning of the range of elements to max_val
    @param last   End of the range of elements to max_val

    @par Requirement

      - @c first and @c last must be pointer to Vectorizable type.

    @par Example

    The following code uses simd::max_val to find the greatest and smallest element
    @c std::vector.
    @snippet max_val.cpp max_val
    @snippet max_val.txt max_val

    @return the maximum value of the range elements.
  **/
  template<typename T> T max_val(T const* first, T const* last)
  {
    if (first == last) return Minf<T>();
    auto pr = segmented_aligned_range(first,last);

    T m = *first;
    for( T e : pr.head ) m = max(e, m);

    pack<T> mm(m);
    for(pack<T> e : pr.body ) mm = max(e, mm);

    m =  maximum(mm);
    for(T e : pr.tail ) m = max(e, m);

    return m;
  }
} }

#endif


#ifndef MOCHIMOCHI_ADAM_HPP_
#define MOCHIMOCHI_ADAM_HPP_

#include <Eigen/Dense>
#include <cassert>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include "../../functions/enumerate.hpp"
#include "../factory/binary_oml.hpp"

class ADAM : public BinaryOML {
private :
  const std::size_t kDim;

private :
  std::size_t _timestep;
  Eigen::VectorXd _w;
  Eigen::VectorXd _m;
  Eigen::VectorXd _v;

public :
  ADAM(const std::size_t dim)
    : kDim(dim),
      _timestep(0),
      _w(Eigen::VectorXd::Zero(kDim)),
      _m(Eigen::VectorXd::Zero(kDim)),
      _v(Eigen::VectorXd::Zero(kDim)) {

    assert(dim > 0);
  }

  virtual ~ADAM() { }

private :

  double suffer_loss(const Eigen::VectorXd& x, const int y) const {
    return std::max(0.0, 1.0 - y * _w.dot(x));
  }

  double calculate_margin(const Eigen::VectorXd& x) const {
    return _w.dot(x);
  }

public :

  std::string name() const override {
    return std::string("ADAM");
  }

  bool update(const Eigen::VectorXd& feature, const int label) override {
    constexpr auto kAlpha = 0.001;
    constexpr auto kBeta1 = 0.9;
    constexpr auto kBeta2 = 0.999;
    constexpr auto kEpsilon = 0.00000001;
    constexpr auto kLambda = 0.99999999;

    if (suffer_loss(feature, label) <= 0.0) { return false; }

    const Eigen::VectorXd gradiant = -label * feature;
    const auto beta1_t = std::pow(kLambda, _timestep) * kBeta1;

    _timestep++;
    functions::enumerate(gradiant.data(), gradiant.data() + gradiant.size(), 0,
                       [&](const std::size_t index, const double value) {
                         _m[index] = beta1_t * _m[index] + (1.0 - beta1_t) * value;
                         _v[index] = kBeta2 * _v[index] + (1.0 - kBeta2) * value * value;
                         const auto m_t = _m[index] / (1.0 - std::pow(kBeta1, _timestep));
                         const auto v_t = _v[index] / (1.0 - std::pow(kBeta2, _timestep));
                         _w[index] -= kAlpha * m_t / (std::sqrt(v_t) + kEpsilon);
                       });

    return true;
  }

  int predict(const Eigen::VectorXd& feature) const override {
    return calculate_margin(feature) > 0.0 ? 1 : -1;
  }

  void save(const std::string& filename) override {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) override {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> w_vector(_w.data(), _w.data() + _w.size());
    std::vector<double> m_vector(_m.data(), _m.data() + _m.size());
    std::vector<double> v_vector(_v.data(), _v.data() + _v.size());

    ar & boost::serialization::make_nvp("w", w_vector);
    ar & boost::serialization::make_nvp("m", m_vector);
    ar & boost::serialization::make_nvp("v", v_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> w_vector;
    std::vector<double> m_vector;
    std::vector<double> v_vector;

    ar & boost::serialization::make_nvp("w", w_vector);
    ar & boost::serialization::make_nvp("m", m_vector);
    ar & boost::serialization::make_nvp("v", v_vector);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));

    _w = Eigen::Map<Eigen::VectorXd>(&w_vector[0], w_vector.size());
    _m = Eigen::Map<Eigen::VectorXd>(&m_vector[0], m_vector.size());
    _v = Eigen::Map<Eigen::VectorXd>(&v_vector[0], v_vector.size());
  }

};

#endif //MOCHIMOCHI_ADAM_HPP_