// This is main.
// This is Car Parrinello molecular dynamics for simulating dynamics of ions near a nanoparticle (NP) surface
// This will power app 2: nanosphere electrostatics lab. the app is part of the nanoparticle characterization framework.
// The framework is expected to launch nanorod electrostatics lab, nanodisc electrostatics lab etc. apps
// Problem : Compute the density profile of ions around a NP; and estimate a zeta potential or effective charge of the ion
/* Useful studies :
		     1. Role of dielectric contrast
		     2. Role of valency of ions
		     3. Role of varying salt concentration
		     4. Role of NP charge
*/

/* @kadupitiya
 * e = E = 78.5 does simple MD; e != E invokes cpmd
 * these parameters produce good converged results that can be compared against the available data (which is plotted in the image sent separately):
 * -a 2.6775 -b 14.28 -e 2 -E 78.5 -V -60 -v 1 -g 1082 -m 6 -t 0.001 -s 10000 -p 100 -f 10 -M 6 -T 0.001 -k 0.0025 -q 0.001 -L 5 -l 5 -S 10000000 -P 100000 -F 100 -X 10000 -U 1000 -Y 500000 -W 1000000 -B 0.025
 * change valency v to 1 (red in image), 2 (green), 3 (blue); everything else can remain the same
 * m = M, k, t = T, q depend on g (the grid size); g depends on e & E, and a. higher the dielectric contrast, that is difference between e and E, higher g is needed to resolve the induced charge density.
 * the selection of these 5 parameters is made manually by monitoring energies, trial and error-- I spent a lot of time just doing that before submitting a useful run to get the ion density. i think there is a possibility of using ML to select these 5 CPMD parameters judiciously. we can pursue this tangentially to nanohub project; hoping it spirals into one of our collaborative hpc/ml projects.
 * the default parameters right now in boost are for a fast simulation that works. as you see they are different than the above parameters for different g etc.:
 * -a 2.6775 -b 14.28 -e 2 -E 78.5 -V -60 -v 1 -g 132 -m 1 -t 0.001 -s 10000 -p 100 -f 10 -M 1 -T 0.001 -k 0.01 -q 1 -L 5 -l 5 -S 50000 -P 10000 -F 100 -X 1000 -U 1000 -Y 10000 -W 10000 -B 0.1
 * the quick_check_polarized_data has a short run from these paramaters for you to perform a quick check against any code changes, if you want.
 * the following is slightly slower but produces a better profile (still far from converged):
 * -a 2.6775 -b 14.28 -e 2 -E 78.5 -V -60 -v 1 -g 132 -m 1 -t 0.001 -s 10000 -p 100 -f 10 -M 1 -T 0.001 -k 0.01 -q 1 -L 5 -l 5 -S 200000 -P 100000 -F 100 -X 10000 -U 10000 -Y 100000 -W 10000 -B 0.1
 * for cpmd, successful simulation demands more than energy conservation:
 * 	1. _ind_*.dat files in verifiles should roughly match the _cpmd_*.dat files in computedfiles
 * 	2. total_induced_charge.dat in outfiles should be very close to 0
 * 	3. track_deviation.dat in outfiles should be small (< 1) and stable-- usually this will happen if the above 2 criteria hold; see also average deviation before R in the output at the end-- should be small
 * 	4. and of course, R should be small like before (could be a bit higher than normal MD)
 */

#include <boost/program_options.hpp>
#include "functions.h"
#include "precalculations.h"
#include "NanoParticleDisk.h"
#include "NanoParticleSphere.h"

//MPI boundary parameters
unsigned int lowerBoundIons;
unsigned int upperBoundIons;
unsigned int sizFVecIons;
unsigned int extraElementsIons;
unsigned int lowerBoundMesh;
unsigned int upperBoundMesh;
unsigned int sizFVecMesh;
unsigned int extraElementsMesh;
mpi::environment env;
mpi::communicator world;

vector<int> condensedIonsPerStep; // Number of condensed ions per step (after equilibrium) at specified frequency

using namespace boost::program_options;

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

    // Electrostatic system variables
    double radius;        // radius of the dielectric sphere
    double ein;            // permittivity of inside medium
    double eout;            // permittivity of outside medium
    int counterion_valency;    // counterion valency (positive by convention)
    double counterion_diameter;    // counterion diameter
    int salt_valency_in;        // salt valency inside
    int salt_valency_out;        // salt valency outside
    double salt_conc_in;        // salt concentration outside	(enter in M)
    double salt_conc_out;        // salt concentration outside	(enter in M)
    double saltion_diameter_in;    // inside salt ion diameter	(positive and negative ions assumed to have same diameter at this point)
    double saltion_diameter_out;  // outside salt ion diameter	(positive and negative ions assumed to have same diameter at this point)
    double real_T;            // temperature at which the system of ions is
    double nanoparticle_bare_charge; // bare charge of the NP

    // Simulation related variables
    int total_gridpoints;        // total number of grid points that discretize the continuous interface
    double Q;            // thermostat mass required to generate canonical ensemble
    double fake_T;            // fake temperature useful for Car Parrinello dynamics
    double fake_Q;            // fake thermostat mass required to generate canonical ensemble for fake degrees
    unsigned int chain_length_real;
    unsigned int chain_length_fake;
    double box_radius;        // simulation box size, measured as radius in case of a sphere
    double bin_width_R;        // width of the bins (in R direction) used to compute density profiles
    double bin_width_Z;        // width of the bins (in Z direction) used to compute density profiles
    CONTROL fmdremote;        // remote control for fmd
    CONTROL cpmdremote;        // remote control for cpmd

    // Different parts of the system
    //INTERFACE nanoparticle;        // interface(s)
    vector<PARTICLE> counterion;    // counterions
    vector<PARTICLE> saltion_in;    // salt ions inside
    vector<PARTICLE> saltion_out;    // salt ions outside
    vector<PARTICLE> ion;        // all ions in the system
    vector<VERTEX> s;        // all vertices

    // Analysis
    string np_shape; // np shape
    NanoParticle *nanoParticle;
    VECTOR3D np_pos(0, 0, 0);

    // Get input values from the user
    options_description desc("Usage:\nrandom_mesh <options>");
    desc.add_options()
            ("help,h", "print usage message")
            ("radius,a", value<double>(&radius)->default_value(2.6775),
             "sphere radius")                // enter in nanometers
            ("epsilon_in,e", value<double>(&ein)->default_value(78.5), "dielectric const inside")
            ("epsilon_out,E", value<double>(&eout)->default_value(78.5), "dielectric const outside")
            ("counterion_valency,v", value<int>(&counterion_valency)->default_value(1), "counterion valency")
            ("nanoparticle_charge,V", value<double>(&nanoparticle_bare_charge)->default_value(-60),
             "nanoparticle charge")
            ("salt_valency_in,z", value<int>(&salt_valency_in)->default_value(1), "salt valency inside")
            ("salt_valency_out,Z", value<int>(&salt_valency_out)->default_value(1), "salt valency outside")
            ("salt_conc_in,c", value<double>(&salt_conc_in)->default_value(0.0), "salt concentration inside")
            ("salt_conc_out,C", value<double>(&salt_conc_out)->default_value(0.0), "salt concentration outside")
            ("counterion_diameter,x", value<double>(&counterion_diameter)->default_value(0.357),
             "counterion diameter")            // enter in nanometers
            ("saltion_diameter_in,d", value<double>(&saltion_diameter_in)->default_value(0.357),
             "salt ion diameter inside")        // enter in nanometers
            ("saltion_diameter_out,D", value<double>(&saltion_diameter_out)->default_value(0.357),
             "salt ion diameter outside")        // enter in nanometers
            ("total_gridpoints,g", value<int>(&total_gridpoints)->default_value(132), "gridpoints")
            ("thermostat_mass,Q", value<double>(&Q)->default_value(1.0), "thermostat mass")
            ("chain_length_real,L", value<unsigned int>(&chain_length_real)->default_value(5),
             "chain length for real system: enter L+1 if you want L thermostats")
            ("fake_temperature,k", value<double>(&fake_T)->default_value(0.01), "fake temperature")
            ("fake_thermostat_mass,q", value<double>(&fake_Q)->default_value(1.0), "fake thermostat mass")
            ("chain_length_fake,l", value<unsigned int>(&chain_length_fake)->default_value(5),
             "chain length for fake system: enter L+1 if you want L thermostats")
            ("box_radius,b", value<double>(&box_radius)->default_value(14.28),
             "simulation box radius")        // enter in nanometers
            ("bin_width_R,R", value<double>(&bin_width_R)->default_value(0.1), "bin width R")
            ("bin_width_Z,B", value<double>(&bin_width_Z)->default_value(0.2), "bin width Z")
            ("anneal_fmd,A", value<char>(&fmdremote.anneal)->default_value('n'), "anneal in fmd on?")
            ("fmd_fake_mass,m", value<double>(&fmdremote.fakemass)->default_value(1.0), "fmd fake mass")
            ("cpmd_fake_mass,M", value<double>(&cpmdremote.fakemass)->default_value(1.0), "cpmd fake mass")
            ("fmd_timestep,t", value<double>(&fmdremote.timestep)->default_value(0.001), "time step used in fmd")
            ("cpmd_timestep,T", value<double>(&cpmdremote.timestep)->default_value(0.001), "time step used in cpmd")
            ("fmd_steps,s", value<int>(&fmdremote.steps)->default_value(10000), "steps used in fmd")
            ("cpmd_steps,S", value<int>(&cpmdremote.steps)->default_value(50000), "steps used in cpmd")
            ("fmd_eqm,p", value<int>(&fmdremote.hiteqm)->default_value(100), "production begin (fmd)")
            ("cpmd_eqm,P", value<int>(&cpmdremote.hiteqm)->default_value(10000), "production begin (cpmd)")
            ("fmd_freq,f", value<int>(&fmdremote.freq)->default_value(10), "sample frequency (fmd)")
            ("cpmd_freq,F", value<int>(&cpmdremote.freq)->default_value(100), "sample frequency (cpmd)")
            ("fmd_verify,y", value<int>(&fmdremote.verify)->default_value(0), "verify (fmd)")
            ("cpmd_verify,Y", value<int>(&cpmdremote.verify)->default_value(10000), "verify (cpmd)")
            ("cpmd_writedata,U", value<int>(&cpmdremote.writedata)->default_value(1000), "write data files")
            ("cpmd_extra_compute,X", value<int>(&cpmdremote.extra_compute)->default_value(1000),
             "compute additional (cpmd)")
            ("cpmd_writedensity,W", value<int>(&cpmdremote.writedensity)->default_value(10000), "write density files")
            ("np_shape,G", value<string>(&np_shape)->default_value("Sphere"), "nanoparticle shape")
            ("verbose,I", value<bool>(&cpmdremote.verbose)->default_value(true),
             "verbose true: provides detailed output");

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);
    if (vm.count("help")) {
        if (world.rank() == 0)
            std::cout << desc << "\n";
        return 0;
    }

    if (world.rank() == 0)
        cout << "\nProgram starts\n";

    int numOfNodes = world.size();
    if (world.rank() == 0) {
#pragma omp parallel default(shared)
        {
            if (omp_get_thread_num() == 0) {
                printf("The app comes with MPI and OpenMP (Hybrid) parallelization)\n");
                printf("Number of MPI processes used %d\n", numOfNodes);
                printf("Number of OpenMP threads per MPI process %d\n", omp_get_num_threads());
                printf("Make sure that number of grid points / ions is greater than %d\n",
                       omp_get_num_threads() * numOfNodes);
            }
        }
    }

    //serve different NP for density bin
    if (np_shape.compare("Sphere") == 0) {
        //Sphere
        vector<BinShell> bin_pos, bin_neg;
        vector<double> sample_density_pos, sample_density_neg;
        NanoParticleSphere np("Sphere", bin_pos, bin_neg, bin_width_R, ion, sample_density_pos, sample_density_neg, 0,
                              0, cpmdremote,
                              np_pos, radius / unitlength, ein, eout,
                              nanoparticle_bare_charge);

        nanoParticle = &np;

    } else {
        //disk

        vector<vector<double> > density_pos, density_neg;
        vector<vector<BinRing> > bin_disk_pos, bin_disk_neg;
        NanoParticleDisk np("Disk", bin_disk_pos, bin_disk_neg, bin_width_R, bin_width_Z, ion, density_pos, density_neg,
                            0, 0, cpmdremote,
                            np_pos, radius / unitlength, ein, eout,
                            nanoparticle_bare_charge);

        nanoParticle = &np;

    }

    // Set up the system
    real_T = 1;

    // set temperature
    // make interface
    nanoParticle->set_up(salt_conc_in, salt_conc_out, salt_valency_in, salt_valency_out, total_gridpoints,
                         box_radius / unitlength);    // set up properties inside and outside the interface

    // If running a standard simulation (charged particle and/or with salt) populate the ions:
    if (abs(nanoparticle_bare_charge) > 0)
        nanoParticle->put_counterions(counterion, counterion_valency, counterion_diameter, ion);                    // put counterions	Note: ion contains all ions
    if (salt_conc_in > 0)
        nanoParticle->put_saltions_inside(saltion_in, salt_valency_in, salt_conc_in, saltion_diameter_in, ion);                // put salt ions inside
    if (salt_conc_out > 0)
        nanoParticle->put_saltions_outside(saltion_out, salt_valency_out, salt_conc_out, saltion_diameter_out, ion);            // put salt ions outside

    //  If the charge is set to zero (for testing), insert test two test ions at chosen positions:
    /*if (nanoparticle_bare_charge == 0)
    {
        ion.push_back(PARTICLE(int(ion.size()) + 1, counterion_diameter, counterion_valency, counterion_valency * 1.0, 1.0, eout, VECTOR3D(5 / unitlength,0,0)));
        ion.push_back(PARTICLE(int(ion.size()) + 1, counterion_diameter, counterion_valency, counterion_valency * 1.0, 1.0, eout, VECTOR3D(6 / unitlength,0,0)));
    }*/

    nanoParticle->discretize(s,radius / unitlength);                                // discretize interface

    // if dielectric environment inside and outside NP are different, NPs get polarized
    if (nanoParticle->ein == nanoParticle->eout)
        nanoParticle->POLARIZED = false;
    else
        nanoParticle->POLARIZED = true;

    if (world.rank() == 0) {
        if (nanoParticle->POLARIZED)
            cout << "NP is polarized " << endl;
        else
            cout << "NP is not polarized " << endl;
    }

    nanoParticle->RANDOMIZE_ION_FEATURES = false;

    // NOTE: sizing the arrays employed in precalculate functions
    for (unsigned int k = 0; k < s.size(); k++) {
        s[k].presumgwEw.resize(s.size());
        s[k].presumgEwEq.resize(s.size());
        s[k].presumgEwEw.resize(s.size());
        s[k].presumfwEw.resize(s.size());
        s[k].presumfEwEq.resize(s.size());
        s[k].presumhEqEw.resize(s.size());
    }

    // could only do precalculate if CPMD
    if (nanoParticle->POLARIZED)
        precalculate(s, nanoParticle);                        // precalculate

    for (unsigned int k = 0; k < s.size(); k++)               // get polar coordinates for the vertices
        s[k].get_polar();

    //make bins
    nanoParticle->make_bins();

    if (world.rank() == 0) {
        // output to screen the parameters of the problem
        cout << "\n";
        if (cpmdremote.verbose)
            cout << "Reduced units: scalefactor entering in Coloumb interaction is " << scalefactor << endl;
        cout << "Units : length (cms) " << unitlength * pow(10.0, -7) << " | " << "energy(ergs) " << unitenergy
             << " | " << "mass(g) " << unitmass << " | " << "time(s) " << unittime << endl;
        cout << "Radius of the dielectric sphere (interface) " << nanoParticle->radius << endl;
        cout << "Nanoparticle charge " << nanoParticle->bare_charge << endl;
        cout << "Permittivity inside " << nanoParticle->ein << endl;
        cout << "Permittivity outside " << nanoParticle->eout << endl;
        if (cpmdremote.verbose)
            cout << "Contrast strength "
                 << 2 * (nanoParticle->eout - nanoParticle->ein) / (nanoParticle->eout + nanoParticle->ein)
                 << endl;
        cout << "Counterion valency " << counterion_valency << endl;
        if (cpmdremote.verbose) {
            cout << "Salt ion valency inside " << salt_valency_in << endl;
            cout << "Salt ion valency outside " << salt_valency_out << endl;
            cout << "Counterion diameter " << counterion_diameter / unitlength << endl;
            cout << "Salt ion diameter inside " << saltion_diameter_in / unitlength << endl;
            cout << "Salt ion diameter outside " << saltion_diameter_out / unitlength << endl;
            cout << "Salt concentration inside " << salt_conc_in << endl;
            cout << "Salt concentration outside " << salt_conc_out << endl;
            cout << "Debye length inside " << nanoParticle->inv_kappa_in << endl;
            cout << "Debye length outside " << nanoParticle->inv_kappa_out << endl;
            cout << "Mean separation inside " << nanoParticle->mean_sep_in << endl;
            cout << "Mean separation outside " << nanoParticle->mean_sep_out << endl;
        }
        cout << "Simulation box (spherical) radius " << nanoParticle->box_radius << endl;
        cout << "Number of counterions " << counterion.size() << endl;
        if (cpmdremote.verbose) {
            cout << "Number of salt ions inside " << saltion_in.size() << endl;
            cout << "Number of salt ions outside " << saltion_out.size() << endl;
            cout << "Number of points discretizing the interface " << s.size() << endl;
            //cout << "Binning width (uniform) " << bin_disk[0][0].width_R << endl;
            nanoParticle->printBinSize();
        }
    }
    // write to files
    // initial configuration
    ofstream initial_configuration("outfiles/initialconfig.dat");
    if (world.rank() == 0)
        for (unsigned int i = 0; i < ion.size(); i++)
            initial_configuration << "ion" << setw(5) << ion[i].id << setw(15) << "charge" << setw(5) << ion[i].q
                                  << setw(15) << "position" << setw(15) << ion[i].posvec << endl;
    initial_configuration.close();

    // initial density
    nanoParticle->compute_initial_density_profile();

    // some calculations before simulation begins
    if (world.rank() == 0)
        cout << "Total charge inside the sphere " << nanoParticle->total_charge_inside(ion) << endl;

    // NEW NOTE : resizing the member arrays Gion and gradGion to store dynamic precalculations in fmd and cpmd force routines
    for (unsigned int k = 0; k < s.size(); k++) {
        s[k].Gion.resize(ion.size());
        s[k].gradGion.resize(ion.size());
    }

    //MPI Boundary calculation for ions
    unsigned int rangeIons = ion.size() / world.size() + 1.5;
    lowerBoundIons = world.rank() * rangeIons;
    upperBoundIons = (world.rank() + 1) * rangeIons - 1;
    extraElementsIons = world.size() * rangeIons - ion.size();
    sizFVecIons = upperBoundIons - lowerBoundIons + 1;
    if (world.rank() == world.size() - 1) {
        upperBoundIons = ion.size() - 1;
        sizFVecIons = upperBoundIons - lowerBoundIons + 1 + extraElementsIons;
    }
    if (world.size() == 1) {
        lowerBoundIons = 0;
        upperBoundIons = ion.size() - 1;
    }

    //MPI Boundary calculation for meshPoints
    unsigned int rangeMesh = s.size() / world.size() + 1.5;
    lowerBoundMesh = world.rank() * rangeMesh;
    upperBoundMesh = (world.rank() + 1) * rangeMesh - 1;
    extraElementsMesh = world.size() * rangeMesh - s.size();
    sizFVecMesh = upperBoundMesh - lowerBoundMesh + 1;
    if (world.rank() == world.size() - 1) {
        upperBoundMesh = s.size() - 1;
        sizFVecMesh = upperBoundMesh - lowerBoundMesh + 1 + extraElementsMesh;
    }
    if (world.size() == 1) {
        lowerBoundMesh = 0;
        upperBoundMesh = s.size() - 1;
    }

    for (unsigned int k = 0; k < s.size(); k++) {
        s[k].w = 0.0;                                // Initialize fake degree value		(unconstrained)
        s[k].wmean = 0.0;
    }

    // Fictitious molecular dynamics
    if (nanoParticle->POLARIZED) {
        if (world.rank() == 0)
            cout << "Polarized charges detected; simulation will proceed using dynamical optimization framework (CPMD)"
                 << endl;
        fmd(s, ion, nanoParticle, fmdremote, cpmdremote);
    } else if (world.rank() == 0)
        cout << "no induced charges; simulation will proceed using simple MD" << endl;

    ofstream induced_density("outfiles/induced_density.dat");
    if (world.rank() == 0)
        // result of fmd
        for (unsigned int k = 0; k < s.size(); k++)
            induced_density << k + 1 << setw(15) << s[k].theta << setw(15) << s[k].phi << setw(15) << s[k].w << setw(15)
                            << s[k].wmean << endl;

    induced_density.close();
    // prepare for cpmd : make real and fake baths

    vector<THERMOSTAT> real_bath;
    if (chain_length_real == 1)
        real_bath.push_back(THERMOSTAT(0, real_T, 3 * ion.size(), 0.0, 0, 0));
    else {
        real_bath.push_back(THERMOSTAT(Q, real_T, 3 * ion.size(), 0, 0, 0));
        while (real_bath.size() != chain_length_real - 1)
            real_bath.push_back(THERMOSTAT(Q / (3 * ion.size()), real_T, 1, 0, 0, 0));
        real_bath.push_back(THERMOSTAT(0, real_T, 3 * ion.size(), 0.0, 0,
                                       0));            // finally, the coding trick: dummy bath (dummy bath always has zero mass)
    }

    vector<THERMOSTAT> fake_bath;
    if (!nanoParticle->POLARIZED)
        fake_T = 0;
//K is used for thermostat fake_T is k
    if (chain_length_fake == 1)
        fake_bath.push_back(THERMOSTAT(0, fake_T, s.size(), 0.0, 0, 0));
    else {
        fake_bath.push_back(THERMOSTAT(fake_Q, fake_T, s.size(), 0, 0, 0));
        while (fake_bath.size() != chain_length_fake - 1)
            fake_bath.push_back(THERMOSTAT(fake_Q / s.size(), fake_T, 1, 0, 0, 0));
        fake_bath.push_back(THERMOSTAT(0, fake_T, s.size(), 0.0, 0,
                                       0));            // finally, the coding trick: dummy bath (dummy bath always has zero mass)
    }
    if (world.rank() == 0 && cpmdremote.verbose) {
        cout << "Number of chains for real system" << setw(3) << real_bath.size() - 1 << endl;
        cout << "Number of chains for fake system" << setw(3) << fake_bath.size() - 1 << endl;
    }

    // Car-Parrinello Molecular Dynamics
    cpmd(ion, s, nanoParticle, real_bath, fake_bath, fmdremote, cpmdremote);

    if (world.rank() == 0) {
        // Post simulation analysis (useful for short runs, but performed otherwise too)
        if (cpmdremote.verbose)
            cout << "MD trust factor R (should be < 0.05) is " << compute_MD_trust_factor_R(cpmdremote.hiteqm) << endl;
        if (nanoParticle->POLARIZED && cpmdremote.verbose)
            cout << "MD trust factor RV (should be < 0.15) is " << compute_MD_trust_factor_R_v(cpmdremote.hiteqm)
                 << endl;
        //auto_correlation_function();
        cout << "Program ends" << endl;
        cout << endl;
    }
    return 0;
}
// End of main


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <scitbx/math/minimum_covering_sphere.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>

SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(
  scitbx::math::sphere_3d<>)
SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(
  scitbx::math::minimum_covering_sphere_3d<>)

namespace scitbx { namespace math {
namespace {

  struct sphere_3d_wrappers
  {
    typedef sphere_3d<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("sphere_3d", no_init)
        .def(init<vec3<double> const&, double>(
          (arg("center"), arg("radius"))))
        .def("center", &w_t::center)
        .def("radius", &w_t::radius)
        .def("expand", &w_t::expand, (arg("additional_radius")))
        .def("expand_relative", &w_t::expand_relative,
          (arg("additional_relative_radius")))
        .def("is_inside", &w_t::is_inside, (arg("point")))
        .def("box_min", &w_t::box_min)
        .def("box_max", &w_t::box_max)
      ;
    }
  };

  struct minimum_covering_sphere_3d_wrappers
  {
    typedef minimum_covering_sphere_3d<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t, bases<sphere_3d<> > >("minimum_covering_sphere_3d", no_init)
        .def(init<
          af::const_ref<vec3<double> > const&,
          optional<
            double const&,
            double const&,
            vec3<double> const&> >((
              arg("points"),
              arg("epsilon")=1.e-6,
              arg("radius_if_one_or_no_points")=1,
              arg("center_if_no_points")=w_t::default_center_if_no_points)))
        .def("n_iterations", &w_t::n_iterations)
      ;
    }
  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_minimum_covering_sphere()
  {
    sphere_3d_wrappers::wrap();
    minimum_covering_sphere_3d_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


#include <frovedis.hpp>
#include <frovedis/ml/recommendation/als.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

bool isError (std::vector<std::pair<size_t,double>>& v1,
              std::vector<std::pair<size_t,double>>& v2) {
  double tol = 0.01;
  double sum = 0.0;
  if (v1.size() != v2.size()) return false;
  for(size_t i=0; i<v1.size(); ++i) {
    if (v1[i].first != v2[i].first) return false;
    double diff = (v1[i].second - v2[i].second);
    sum += (diff * diff);
  }
  double r = sqrt(sum/v1.size());
  //cout << "RMSE: " << r << endl;
  return (r > tol);
}

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // loading the input rank matrix
    auto data = make_crs_matrix_load<double> ("./data");

    // setting the algorithm parameter
    size_t num_iteration = 100;
    size_t factor = 2;
    double alpha = 0.01;
    double regParam = 0.01;
    long seed = 0;

    // getting the trained model
    auto model = matrix_factorization_using_als::train(data, factor,
                                   num_iteration, alpha,
                                   regParam, seed);

    // predicting rating by a particular user for a particular product
    double r1 = model.predict(0,1);
    double r2 = model.predict(3,3);
    double e_r1 = 0.25;
    double e_r2 = 0.25;
    double tol = 0.01;
    BOOST_CHECK ((r1 - e_r1) < tol);
    BOOST_CHECK ((r2 - e_r2) < tol);

    // recommending top 3 users for product id 1
    auto ru = model.recommend_users(1,3);
    //for(auto &i: ru) cout << i.first << " " << i.second << endl;
    std::vector<std::pair<size_t,double>> e_out;
    e_out.push_back(make_pair<size_t,double>(2,1.18));
    e_out.push_back(make_pair<size_t,double>(1,0.97));
    e_out.push_back(make_pair<size_t,double>(3,0.56));
    BOOST_CHECK (isError(e_out,ru) == false);

    // recommending top 3 products for user id 1
    auto rp = model.recommend_products(1,3);
    //for(auto &i: rp) cout << i.first << " " << i.second << endl;
    e_out.clear();
    e_out.push_back(make_pair<size_t,double>(1,0.97));
    e_out.push_back(make_pair<size_t,double>(3,0.93));
    e_out.push_back(make_pair<size_t,double>(0,0.25));
    BOOST_CHECK (isError(e_out,rp) == false);
}


/**
 * @file propagator.h
 * @brief NPDE homework NonLinSchroedingerEquation code
 * @author Oliver Rietmann
 * @date 04.05.2020
 * @copyright Developed at ETH Zurich
 */

#include "propagator.h"

#include <cmath>
#include <complex>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

namespace NonLinSchroedingerEquation {

// KineticPropagator
/* SAM_LISTING_BEGIN_1 */
KineticPropagator::KineticPropagator(const SparseMatrixXd &A,
                                     const SparseMatrixXcd &M, double tau) {
  //====================
  // Your code goes here
  //====================
}

Eigen::VectorXcd KineticPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  //====================
  // Your code goes here
  // Replace mu by its value after a timestep tau
  return mu;
  //====================
}
/* SAM_LISTING_END_1 */

// InteractionPropagator
/* SAM_LISTING_BEGIN_2 */
InteractionPropagator::InteractionPropagator(double tau) {
  //====================
  // Your code goes here
  //====================
}

Eigen::VectorXcd InteractionPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  //====================
  // Your code goes here
  // Replace mu by its value after a timestep tau
  return mu;
  //====================
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
//====================
// Your code goes here
// Change this dummy implementation of the constructor:
SplitStepPropagator::SplitStepPropagator(const SparseMatrixXd &A,
                                         const SparseMatrixXcd &M, double tau) {
}
//====================

Eigen::VectorXcd SplitStepPropagator::operator()(
    const Eigen::VectorXcd &mu) const {
  Eigen::VectorXcd nu(mu.size());
  //====================
  // Your code goes here
  // Implement the Strang splitting
  //====================
  return nu;
}
/* SAM_LISTING_END_3 */

}  // namespace NonLinSchroedingerEquation


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

#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"

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

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;

template<typename JointModel>
void test_constraint_mimic(const JointModelBase<JointModel> & jmodel)
{
  typedef typename traits<JointModel>::JointDerived Joint;
  typedef typename traits<Joint>::Constraint_t ConstraintType;
  typedef typename traits<Joint>::JointDataDerived JointData;
  typedef ScaledConstraint<ConstraintType> ScaledConstraintType;

  JointData jdata = jmodel.createData();

  const double scaling_factor = 2.;
  ConstraintType constraint_ref(jdata.S), constraint_ref_shared(jdata.S);
  ScaledConstraintType scaled_constraint(constraint_ref_shared,scaling_factor);

  BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv());

  typedef typename JointModel::TangentVector_t TangentVector_t;
  TangentVector_t v = TangentVector_t::Random();

  Motion m = scaled_constraint * v;
  Motion m_ref = scaling_factor * (Motion)(constraint_ref * v);

  BOOST_CHECK(m.isApprox(m_ref));

  {
    SE3 M = SE3::Random();
    typename ScaledConstraintType::DenseBase S = M.act(scaled_constraint);
    typename ScaledConstraintType::DenseBase S_ref = scaling_factor * M.act(constraint_ref);

    BOOST_CHECK(S.isApprox(S_ref));
  }

  {
    typename ScaledConstraintType::DenseBase S = scaled_constraint.matrix();
    typename ScaledConstraintType::DenseBase S_ref = scaling_factor * constraint_ref.matrix();

    BOOST_CHECK(S.isApprox(S_ref));
  }

  {
    Motion v = Motion::Random();
    typename ScaledConstraintType::DenseBase S = v.cross(scaled_constraint);
    typename ScaledConstraintType::DenseBase S_ref = scaling_factor * v.cross(constraint_ref);

    BOOST_CHECK(S.isApprox(S_ref));
  }

  // Test transpose operations
  {
    const Eigen::DenseIndex dim = 20;
    const Matrix6x Fin = Matrix6x::Random(6,dim);
    Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin;
    Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin);
    BOOST_CHECK(Fout.isApprox(Fout_ref));

    Force force_in(Force::Random());
    Eigen::MatrixXd Stf = (scaled_constraint.transpose() * force_in);
    Eigen::MatrixXd Stf_ref = scaling_factor * (constraint_ref.transpose() * force_in);
    BOOST_CHECK(Stf_ref.isApprox(Stf));
  }

  // CRBA Y*S
  {
    Inertia Y = Inertia::Random();
    Eigen::MatrixXd YS = Y * scaled_constraint;
    Eigen::MatrixXd YS_ref = scaling_factor * (Y * constraint_ref);

    BOOST_CHECK(YS.isApprox(YS_ref));
  }

}

struct TestJointConstraint
{

  template <typename JointModel>
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0,0,0);

    test_constraint_mimic(jmodel);
  }

  void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
  {
    JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_constraint_mimic(jmodel);
  }

  void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const
  {
    JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_constraint_mimic(jmodel);
  }

};

BOOST_AUTO_TEST_CASE(test_constraint)
{
  using namespace pinocchio;
  typedef boost::variant<
  JointModelRX, JointModelRY, JointModelRZ
  , JointModelRevoluteUnaligned
  , JointModelPX, JointModelPY, JointModelPZ
  , JointModelPrismaticUnaligned
  , JointModelRUBX, JointModelRUBY, JointModelRUBZ
  > Variant;

  boost::mpl::for_each<Variant::types>(TestJointConstraint());
}

template<typename JointModel>
void test_joint_mimic(const JointModelBase<JointModel> & jmodel)
{
  typedef typename traits<JointModel>::JointDerived Joint;
  typedef typename traits<Joint>::JointDataDerived JointData;

  JointData jdata = jmodel.createData();

  const double scaling_factor = 1.;
  const double offset = 0.;

  typedef JointMimic<Joint> JointMimicType;
  typedef typename traits<JointMimicType>::JointModelDerived JointModelMimicType;
  typedef typename traits<JointMimicType>::JointDataDerived JointDataMimicType;

  // test constructor
  JointModelMimicType jmodel_mimic(jmodel.derived(),scaling_factor,offset);
  JointDataMimicType jdata_mimic = jmodel_mimic.createData();

  BOOST_CHECK(jmodel_mimic.nq() == 0);
  BOOST_CHECK(jmodel_mimic.nv() == 0);

  BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q());
  BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v());

  BOOST_CHECK(jmodel_mimic.idx_q() == 0);
  BOOST_CHECK(jmodel_mimic.idx_v() == 0);

  typedef typename JointModelMimicType::ConfigVector_t ConfigVectorType;
  typedef typename LieGroup<JointModel>::type LieGroupType;
  ConfigVectorType q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones());

  jmodel.calc(jdata,q0);
  jmodel_mimic.calc(jdata_mimic,q0);

  BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M()));
  BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix()));

  typedef typename JointModelMimicType::TangentVector_t TangentVectorType;

  q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones());
  TangentVectorType v0 = TangentVectorType::Random();
  jmodel.calc(jdata,q0,v0);
  jmodel_mimic.calc(jdata_mimic,q0,v0);

  BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M()));
  BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix()));
  BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v()));
}

struct TestJointMimic
{

  template <typename JointModel>
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0,0,0);

    test_joint_mimic(jmodel);
  }

  void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
  {
    JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_mimic(jmodel);
  }

  void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const
  {
    JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_mimic(jmodel);
  }

};

BOOST_AUTO_TEST_CASE(test_joint)
{
  using namespace pinocchio;
  typedef boost::variant<
  JointModelRX, JointModelRY, JointModelRZ
  , JointModelRevoluteUnaligned
  , JointModelPX, JointModelPY, JointModelPZ
  , JointModelPrismaticUnaligned
  , JointModelRUBX, JointModelRUBY, JointModelRUBZ
  > Variant;

  boost::mpl::for_each<Variant::types>(TestJointMimic());
}

BOOST_AUTO_TEST_CASE(test_transform_linear_affine)
{
  typedef JointModelRX::ConfigVector_t ConfigVectorType;
  double scaling = 1., offset = 0.;

  ConfigVectorType q0 = ConfigVectorType::Random();
  ConfigVectorType q1;
  LinearAffineTransform::run(q0,scaling,offset,q1);
  BOOST_CHECK(q0 == q1);

  offset = 2.;
  LinearAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1);
  BOOST_CHECK(q1 == ConfigVectorType::Constant(offset));
}

BOOST_AUTO_TEST_CASE(test_transform_linear_revolute)
{
  typedef JointModelRUBX::ConfigVector_t ConfigVectorType;
  double scaling = 1., offset = 0.;

  ConfigVectorType q0 = ConfigVectorType::Random().normalized();
  ConfigVectorType q1;
  UnboundedRevoluteAffineTransform::run(q0,scaling,offset,q1);
  BOOST_CHECK(q0.isApprox(q1));

  offset = 2.;
  UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1);
  BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset),math::sin(offset)));
}

BOOST_AUTO_TEST_CASE(test_joint_generic_cast)
{
  JointModelRX jmodel_ref;
  jmodel_ref.setIndexes(1,2,3);

  JointModelMimic<JointModelRX> jmodel(jmodel_ref,2.,1.);
  jmodel.setIndexes(1,-1,-1);

  BOOST_CHECK(jmodel.id() == jmodel_ref.id());
  BOOST_CHECK(jmodel.idx_q() == jmodel_ref.idx_q());
  BOOST_CHECK(jmodel.idx_v() == jmodel_ref.idx_v());

  JointModel jmodel_generic(jmodel);
  jmodel_generic.setIndexes(1,-2,-2);

  BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id());
}
BOOST_AUTO_TEST_SUITE_END()


// this file mainly refers to libint2's example

#include "jlcxx/jlcxx.hpp"
#include <libint2.hpp>
#include <iostream>
#include <thread>
#include <mutex>
#include <Eigen/Dense>

using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

static std::mutex mtx;

size_t nbasis(const std::vector<libint2::Shell> &shells)
{
    size_t n = 0;
    for (const auto &shell : shells)
        n += shell.size();
    return n;
}

size_t max_nprim(const std::vector<libint2::Shell> &shells)
{
    size_t n = 0;
    for (auto shell : shells)
        n = std::max(shell.nprim(), n);
    return n;
}

int max_l(const std::vector<libint2::Shell> &shells)
{
    int l = 0;
    for (auto shell : shells)
        for (auto c : shell.contr)
            l = std::max(c.l, l);
    return l;
}

std::vector<libint2::Shell> construct_shells(int Nshell, int *l, double *alpha, double *coeff, double *cart)
{
    std::vector<libint2::Shell> shells;
    for (int i = 0; i < Nshell; ++i)
    {
        double *a = alpha + 3 * i;
        double *d = coeff + 3 * i;
        double *r = cart + 3 * i;
        shells.push_back({{a[0], a[1], a[2]},
                          {{l[i], false, {d[0], d[1], d[2]}}},
                          {{r[0], r[1], r[2]}}});
    }
    return shells;
}

std::vector<size_t> map_shell_to_basis_function(const std::vector<libint2::Shell>& shells) {
  std::vector<size_t> result;
  result.reserve(shells.size());

  size_t n = 0;
  for (auto shell: shells) {
    result.push_back(n);
    n += shell.size();
  }

  return result;
}

void Ve(double *ve, int Nshell, int *l, double *alpha, double *d, double *R) {
  const auto shells = construct_shells(Nshell, l, alpha, d, R);
  const auto n = nbasis(shells);
  auto index = [&n](int i, int j, int k, int l) {
        return n * (n * (n * l + k) + j) + i;
  };
  std::memset(ve, 0, n*n*n*n*sizeof(double));
  libint2::Engine engine(libint2::Operator::coulomb, max_nprim(shells), max_l(shells), 0);

  auto shell2bf = map_shell_to_basis_function(shells);

  const auto& buf = engine.results();

  for(auto s1=0; s1!=shells.size(); ++s1) {

    auto bf1_first = shell2bf[s1];
    auto n1 = shells[s1].size();

    for(auto s2=0; s2!=shells.size(); ++s2) {

      auto bf2_first = shell2bf[s2];
      auto n2 = shells[s2].size();

      for(auto s3=0; s3!=shells.size(); ++s3) {

        auto bf3_first = shell2bf[s3];
        auto n3 = shells[s3].size();

        for(auto s4=0; s4!=shells.size(); ++s4) {

          auto bf4_first = shell2bf[s4];
          auto n4 = shells[s4].size();

          engine.compute(shells[s1], shells[s2], shells[s3], shells[s4]);
          const auto* buf_1234 = buf[0];
          if (buf_1234 == nullptr)
            continue;
          for(auto f1=0, f1234=0; f1!=n1; ++f1) {
            const auto bf1 = f1 + bf1_first;
            for(auto f2=0; f2!=n2; ++f2) {
              const auto bf2 = f2 + bf2_first;
              for(auto f3=0; f3!=n3; ++f3) {
                const auto bf3 = f3 + bf3_first;
                for(auto f4=0; f4!=n4; ++f4, ++f1234) {
                  const auto bf4 = f4 + bf4_first;
                  ve[index(bf1, bf4, bf3, bf2)] = buf_1234[f1234];
                }
              }
            }
          }
        }
      }
    }
  }
}

// copy from libint2's example
void compute_2body_fock(double *veff, double* rho, int Nshell, int *l, double *alpha, double *d, double *R) {
  const auto shells = construct_shells(Nshell, l, alpha, d, R);
  const auto n = nbasis(shells);
  Matrix G = Matrix::Zero(n,n);
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>> D(rho, n, n);

  // construct the 2-electron repulsion integrals engine
  libint2::Engine engine(libint2::Operator::coulomb, max_nprim(shells), max_l(shells), 0);

  auto shell2bf = map_shell_to_basis_function(shells);

  const auto& buf = engine.results();

  // The problem with the simple Fock builder is that permutational symmetries of the Fock,
  // density, and two-electron integrals are not taken into account to reduce the cost.
  // To make the simple Fock builder efficient we must rearrange our computation.
  // The most expensive step in Fock matrix construction is the evaluation of 2-e integrals;
  // hence we must minimize the number of computed integrals by taking advantage of their permutational
  // symmetry. Due to the multiplicative and Hermitian nature of the Coulomb kernel (and realness
  // of the Gaussians) the permutational symmetry of the 2-e ints is given by the following relations:
  //
  // (12|34) = (21|34) = (12|43) = (21|43) = (34|12) = (43|12) = (34|21) = (43|21)
  //
  // (here we use chemists' notation for the integrals, i.e in (ab|cd) a and b correspond to
  // electron 1, and c and d -- to electron 2).
  //
  // It is easy to verify that the following set of nested loops produces a permutationally-unique
  // set of integrals:
  // foreach a = 0 .. n-1
  //   foreach b = 0 .. a
  //     foreach c = 0 .. a
  //       foreach d = 0 .. (a == c ? b : c)
  //         compute (ab|cd)
  //
  // The only complication is that we must compute integrals over shells. But it's not that complicated ...
  //
  // The real trick is figuring out to which matrix elements of the Fock matrix each permutationally-unique
  // (ab|cd) contributes. STOP READING and try to figure it out yourself. (to check your answer see below)

  // loop over permutationally-unique set of shells
  for(auto s1=0; s1!=shells.size(); ++s1) {

    auto bf1_first = shell2bf[s1]; // first basis function in this shell
    auto n1 = shells[s1].size();   // number of basis functions in this shell

    for(auto s2=0; s2<=s1; ++s2) {

      auto bf2_first = shell2bf[s2];
      auto n2 = shells[s2].size();

      for(auto s3=0; s3<=s1; ++s3) {

        auto bf3_first = shell2bf[s3];
        auto n3 = shells[s3].size();

        const auto s4_max = (s1 == s3) ? s2 : s3;
        for(auto s4=0; s4<=s4_max; ++s4) {

          auto bf4_first = shell2bf[s4];
          auto n4 = shells[s4].size();

          // compute the permutational degeneracy (i.e. # of equivalents) of the given shell set
          auto s12_deg = (s1 == s2) ? 1.0 : 2.0;
          auto s34_deg = (s3 == s4) ? 1.0 : 2.0;
          auto s12_34_deg = (s1 == s3) ? (s2 == s4 ? 1.0 : 2.0) : 2.0;
          auto s1234_deg = s12_deg * s34_deg * s12_34_deg;

          engine.compute(shells[s1], shells[s2], shells[s3], shells[s4]);
          const auto* buf_1234 = buf[0];
          if (buf_1234 == nullptr)
            continue; // if all integrals screened out, skip to next quartet

          // ANSWER
          // 1) each shell set of integrals contributes up to 6 shell sets of the Fock matrix:
          //    F(a,b) += (ab|cd) * D(c,d)
          //    F(c,d) += (ab|cd) * D(a,b)
          //    F(b,d) -= 1/4 * (ab|cd) * D(a,c)
          //    F(b,c) -= 1/4 * (ab|cd) * D(a,d)
          //    F(a,c) -= 1/4 * (ab|cd) * D(b,d)
          //    F(a,d) -= 1/4 * (ab|cd) * D(b,c)
          // 2) each permutationally-unique integral (shell set) must be scaled by its degeneracy,
          //    i.e. the number of the integrals/sets equivalent to it
          // 3) the end result must be symmetrized
          for(auto f1=0, f1234=0; f1!=n1; ++f1) {
            const auto bf1 = f1 + bf1_first;
            for(auto f2=0; f2!=n2; ++f2) {
              const auto bf2 = f2 + bf2_first;
              for(auto f3=0; f3!=n3; ++f3) {
                const auto bf3 = f3 + bf3_first;
                for(auto f4=0; f4!=n4; ++f4, ++f1234) {
                  const auto bf4 = f4 + bf4_first;

                  const auto value = buf_1234[f1234];

                  const auto value_scal_by_deg = value * s1234_deg;

                  G(bf1,bf2) += D(bf3,bf4) * value_scal_by_deg;
                  G(bf3,bf4) += D(bf1,bf2) * value_scal_by_deg;
                  G(bf1,bf3) -= 0.25 * D(bf2,bf4) * value_scal_by_deg;
                  G(bf2,bf4) -= 0.25 * D(bf1,bf3) * value_scal_by_deg;
                  G(bf1,bf4) -= 0.25 * D(bf2,bf3) * value_scal_by_deg;
                  G(bf2,bf3) -= 0.25 * D(bf1,bf4) * value_scal_by_deg;
                }
              }
            }
          }

        }
      }
    }
  }

  // symmetrize the result
  for(int i=0; i<n; ++i)
  {
    for(int j=0; j<n; ++j)
    {
      veff[i*n + j] = (G(i,j) + G(j,i))/4.;
    }
  }
}

JLCXX_MODULE define_julia_module(jlcxx::Module &mod)
{
    mod.method("initialize", [](){libint2::initialize();});
    mod.method("Ve", &Ve);
    mod.method("Veff", &compute_2body_fock);
}


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

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

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

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include "toplevelfixture.hpp"
#include <boost/assign/list_of.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ql/math/comparison.hpp>
#include <ql/settings.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/yield/zerocurve.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <qle/termstructures/discountratiomodifiedcurve.hpp>

using QuantExt::DiscountRatioModifiedCurve;
using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace boost::assign;
using std::vector;

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

BOOST_AUTO_TEST_SUITE(DiscountingRatioModifiedCurveTest)

BOOST_AUTO_TEST_CASE(testStandardCurves) {

    BOOST_TEST_MESSAGE("Testing discount ratio modified curve with some standard curves");

    SavedSettings backup;

    Date today(15, Aug, 2018);
    Settings::instance().evaluationDate() = today;

    Actual365Fixed dc;

    // Base curve with fixed reference date of 15th Aug 2018
    vector<Date> baseDates = list_of(today)(today + 1 * Years);
    vector<Real> baseDfs = list_of(1.0)(0.98);
    Handle<YieldTermStructure> baseCurve(boost::make_shared<DiscountCurve>(baseDates, baseDfs, dc));
    baseCurve->enableExtrapolation();

    // Numerator curve with fixed reference date of 15th Aug 2018
    vector<Date> numDates = list_of(today)(today + 1 * Years)(today + 2 * Years);
    vector<Real> numZeroes = list_of(0.025)(0.025)(0.026);
    Handle<YieldTermStructure> numCurve(boost::make_shared<ZeroCurve>(numDates, numZeroes, dc));
    numCurve->enableExtrapolation();

    // Denominator curve with floating reference date
    Handle<YieldTermStructure> denCurve(boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, dc));

    DiscountRatioModifiedCurve curve(baseCurve, numCurve, denCurve);

    Date discountDate = today + 18 * Months;
    BOOST_CHECK(
        close(curve.discount(discountDate),
              baseCurve->discount(discountDate) * numCurve->discount(discountDate) / denCurve->discount(discountDate)));

    discountDate = today + 3 * Years;
    BOOST_CHECK(
        close(curve.discount(discountDate),
              baseCurve->discount(discountDate) * numCurve->discount(discountDate) / denCurve->discount(discountDate)));

    // When we change evaluation date, we may not get what we expect here because reference date is taken from the
    // base curve which has been set up here with a fixed reference date. However, the denominator curve has been
    // set up here with a floating reference date. See the warning in the ctor of DiscountRatioModifiedCurve
    Settings::instance().evaluationDate() = today + 3 * Months;
    BOOST_TEST_MESSAGE("Changed evaluation date to " << Settings::instance().evaluationDate());

    BOOST_CHECK(!close(curve.discount(discountDate), baseCurve->discount(discountDate) *
                                                         numCurve->discount(discountDate) /
                                                         denCurve->discount(discountDate)));
    Time t = dc.yearFraction(curve.referenceDate(), discountDate);
    BOOST_CHECK(close(curve.discount(discountDate),
                      baseCurve->discount(discountDate) * numCurve->discount(discountDate) / denCurve->discount(t)));
}

BOOST_AUTO_TEST_CASE(testExtrapolationSettings) {

    BOOST_TEST_MESSAGE("Testing extrapolation settings for discount ratio modified curve");

    SavedSettings backup;

    Date today(15, Aug, 2018);
    Settings::instance().evaluationDate() = today;

    Actual365Fixed dc;

    // Base curve with fixed reference date of 15th Aug 2018
    vector<Date> baseDates = list_of(today)(Date(15, Aug, 2019));
    vector<Real> baseDfs = list_of(1.0)(0.98);
    Handle<YieldTermStructure> baseCurve(boost::make_shared<DiscountCurve>(baseDates, baseDfs, dc));

    // Numerator curve with fixed reference date of 15th Aug 2018
    vector<Date> numDates = list_of(today)(Date(15, Aug, 2019))(Date(15, Aug, 2020));
    vector<Real> numZeroes = list_of(0.025)(0.025)(0.026);
    Handle<YieldTermStructure> numCurve(boost::make_shared<ZeroCurve>(numDates, numZeroes, dc));

    // Denominator curve with floating reference date
    Handle<YieldTermStructure> denCurve(boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, dc));

    // Create the discount ratio curve
    DiscountRatioModifiedCurve curve(baseCurve, numCurve, denCurve);

    // Extrapolation is always true
    BOOST_CHECK(curve.allowsExtrapolation());

    // Max date is maximum possible date
    BOOST_CHECK_EQUAL(curve.maxDate(), Date::maxDate());

    // Extrapolation is determined by underlying curves
    BOOST_CHECK_NO_THROW(curve.discount(Date(15, Aug, 2019)));
    BOOST_CHECK_THROW(curve.discount(Date(15, Aug, 2019) + 1 * Days), QuantLib::Error);
    baseCurve->enableExtrapolation();
    BOOST_CHECK_NO_THROW(curve.discount(Date(15, Aug, 2019) + 1 * Days));
    BOOST_CHECK_THROW(curve.discount(Date(15, Aug, 2020) + 1 * Days), QuantLib::Error);
    numCurve->enableExtrapolation();
    BOOST_CHECK_NO_THROW(curve.discount(Date(15, Aug, 2020) + 1 * Days));
}

BOOST_AUTO_TEST_CASE(testConstructionNullUnderlyingCurvesThrow) {

    BOOST_TEST_MESSAGE("Testing construction with null underlying curves throw");

    boost::shared_ptr<DiscountRatioModifiedCurve> curve;

    // All empty handles throw
    Handle<YieldTermStructure> baseCurve;
    Handle<YieldTermStructure> numCurve;
    Handle<YieldTermStructure> denCurve;
    BOOST_CHECK_THROW(curve = boost::make_shared<DiscountRatioModifiedCurve>(baseCurve, numCurve, denCurve),
                      QuantLib::Error);

    // Numerator and denominator empty handles throw
    Handle<YieldTermStructure> baseCurve_1(
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, Actual365Fixed()));
    BOOST_CHECK_THROW(curve = boost::make_shared<DiscountRatioModifiedCurve>(baseCurve_1, numCurve, denCurve),
                      QuantLib::Error);

    // Denominator empty handles throw
    Handle<YieldTermStructure> numCurve_1(boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, Actual365Fixed()));
    BOOST_CHECK_THROW(curve = boost::make_shared<DiscountRatioModifiedCurve>(baseCurve_1, numCurve_1, denCurve),
                      QuantLib::Error);

    // No empty handles succeeds
    Handle<YieldTermStructure> denCurve_1(boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, Actual365Fixed()));
    BOOST_CHECK_NO_THROW(curve = boost::make_shared<DiscountRatioModifiedCurve>(baseCurve_1, numCurve_1, denCurve_1));
}

BOOST_AUTO_TEST_CASE(testLinkingNullUnderlyingCurvesThrow) {

    BOOST_TEST_MESSAGE("Testing that linking with null underlying curves throw");

    boost::shared_ptr<DiscountRatioModifiedCurve> curve;

    // All empty handles throw
    RelinkableHandle<YieldTermStructure> baseCurve(
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, Actual365Fixed()));
    RelinkableHandle<YieldTermStructure> numCurve(
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, Actual365Fixed()));
    RelinkableHandle<YieldTermStructure> denCurve(
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.0255, Actual365Fixed()));

    // Curve building succeeds since no empty handles
    BOOST_CHECK_NO_THROW(curve = boost::make_shared<DiscountRatioModifiedCurve>(baseCurve, numCurve, denCurve));

    // Switching base curve to empty handle should give a failure
    BOOST_CHECK_THROW(baseCurve.linkTo(boost::shared_ptr<YieldTermStructure>()), QuantLib::Error);
    BOOST_CHECK_NO_THROW(baseCurve.linkTo(*numCurve));

    // Switching numerator curve to empty handle should give a failure
    BOOST_CHECK_THROW(numCurve.linkTo(boost::shared_ptr<YieldTermStructure>()), QuantLib::Error);
    BOOST_CHECK_NO_THROW(numCurve.linkTo(*denCurve));

    // Switching denominator curve to empty handle should give a failure
    BOOST_CHECK_THROW(denCurve.linkTo(boost::shared_ptr<YieldTermStructure>()), QuantLib::Error);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_BESSEL_FUNCTIONS_SCALAR_Y0_HPP_INCLUDED
#define NT2_BESSEL_FUNCTIONS_SCALAR_Y0_HPP_INCLUDED

#include <nt2/bessel/functions/y0.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/log.hpp>
#include <nt2/include/functions/scalar/j0.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/functions/scalar/sin.hpp>
#include <boost/simd/sdk/math.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::y0_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::y0(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::y0_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if (is_ltz(a0)||is_nan(a0)) return Nan<result_type>();
      if (is_inf(a0)) return Zero<result_type>();
      if (is_eqz(a0)) return Minf<result_type>();
#if defined(BOOST_SIMD_HAS__Y0)
      return ::_y0(a0);
#elif defined(BOOST_SIMD_HAS_Y0)
      return ::y0(a0);
#else
#error y0 not supported
#endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::y0_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type stype;
      if (is_ltz(a0)||is_nan(a0)) return Nan<result_type>();
      if (is_inf(a0)) return Zero<result_type>();
      if (is_eqz(a0)) return Minf<result_type>();
      if (a0 <= Two<A0>())
      {
        A0 z = sqr(a0);
        A0 p2 = (z-single_constant<A0, 0x3edd4b3a>())*
          horner< NT2_HORNER_COEFF_T(stype, 5,
                                     (0x33cb0920,
                                      0xb71ded71,
                                      0x3a0c1a3e,
                                      0xbc81c8f4,
                                      0x3e2edb4f
                                       ) ) > (z);
        return p2+single_constant<A0, 0x3f22f983>()*nt2::log(a0)*nt2::j0(a0);
      }
      A0 q = rec(a0);
      A0 w = nt2::sqrt(q);
      A0 p3 = w *
        horner< NT2_HORNER_COEFF_T(stype, 8,
                                   (0xbd8c100e,
                                    0x3e3ef887,
                                    0xbe5ba616,
                                    0x3df54214,
                                    0xbb69539e,
                                    0xbd4b8bc1,
                                    0xb6612dc2,
                                    0x3f4c422a
                                     ) ) > (q);
       w = sqr(q);
       A0 xn =  q*
         horner< NT2_HORNER_COEFF_T(stype, 8,
                                    (0x4201aee0,
                                     0xc2113945,
                                     0x418c7f6a,
                                     0xc09f3306,
                                     0x3f8040aa,
                                     0xbe46a57f,
                                     0x3d84ed6e,
                                     0xbdffff97
                                      ) ) > (w)-Pio_4<A0>();
      return p3*nt2::sin(xn+a0);
    }
  };
} }

#endif


/* Copyright (C) 5/23/18 Julian Stobbe - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the MIT license.
 *
 * You should have received a copy of the MIT license with
 * this file.
 */

#ifndef SRC_BLACKSCHOLES_NETWORK_HPP_
#define SRC_BLACKSCHOLES_NETWORK_HPP_

#define USE_SPARSE_INTERNAL 1

#include <stdexcept>
#include <algorithm>

#include "easylogging++.h"
// Tina's Random Number Generator
#include "trng/yarn2.hpp"
#include "trng/uniform01_dist.hpp"
#include "trng/lognormal_dist.hpp"
#include "trng/correlated_normal_dist.hpp"
#include "Eigen/Dense"
#include "Eigen/Sparse"
#include <Eigen/SparseLU>

#ifdef USE_MPI
#include <boost/mpi.hpp>
#endif

#include <boost/serialization/vector.hpp>
#include <boost/serialization/optional.hpp>

#include "ValuationConfig.h"
#include "StatAcc.hpp"
#include "RndGraphGen.hpp"

struct BSParameters
{
    double T, r, sigma, S0, default_prob_scale;
};

struct NetworkParameters
{
    int N, which_to_set;
    double conn, colSums;
};

class BlackScholesNetwork
{
    using Mat = Eigen::MatrixXd;
    using Vec = Eigen::VectorXd;
private:
    double T, r, exprt;
    int N;
    bool initialized;
    Vec x, S0, St, St_full, debt, solvent, sigma;
    bool jacobian_set;
    //Mat sigma_diag;
#if USE_SPARSE_INTERNAL
    Eigen::SparseLU<Eigen::SparseMatrix<double, Eigen::ColMajor>> lu;
    Eigen::SparseMatrix<double, Eigen::ColMajor> Id;
    Eigen::SparseMatrix<double, Eigen::ColMajor> M;
    //Eigen::SparseMatrix<double, Eigen::ColMajor> GreekMat;
    //Eigen::SparseMatrix<double, Eigen::ColMajor> Jrs;
    //Eigen::SparseMatrix<double, Eigen::ColMajor> J_a;
    //Eigen::SparseMatrix<double, Eigen::ColMajor> Z;
#else
    Mat M;
    //Mat J_a;
    Mat Id;
    Eigen::PartialPivLU<Eigen::MatrixXd> lu;
#endif
    Mat GreekMat;

    void set_solvent();

public:
    BlackScholesNetwork(const BlackScholesNetwork&) = delete;

    /* BlackScholesNetwork& operator=(const BlackScholesNetwork& rhs) = delete;
    {
        T = rhs.T;
        r = rhs.r;
        N = rhs.N;
        M = rhs.M;
        x = rhs.x;
        S0 = rhs.S0;
        St = rhs.St;
        debt = rhs.debt;
        solvent = rhs.solvent;
        exprt = rhs.exprt;
    }*/

    //BlackScholesNetwork()
    //{
        //LOG(WARNING) << "Default constructor for BlackScholesNetwork used. This could be unintentional.";
    //    initialized = false;
    //}

    /**
     * @brief
     * @param T         maturity
     * @param r         interest rate
     */
    BlackScholesNetwork(const Eigen::Ref<Vec>& S0, const Eigen::Ref<Vec>& debt, const Eigen::Ref<Vec>& sigma_, const double T,const double r);

    /**
     * @brief
     * @param M         Combined cross equity and cross debt matrix
     * @param assets    exogenous assets
     * @param debt      debts
     * @param T         maturity
     * @param r         interest rate
     */
    BlackScholesNetwork(const Eigen::Ref<Mat>& M, const Eigen::Ref<Vec>& S0, const Eigen::Ref<Vec>& assets, const Eigen::Ref<Vec>& debt, const Eigen::Ref<Vec>& sigma_, const double T, const double r);

    /**
     * @brief               Finds the fixed point of the cross holding problem at maturity T.
     * @param iterations    maximum number of self consistency iterations.
     * @return              returns vector of value of debt and value of equity.
     */
    const Mat run_valuation(unsigned int iterations);

    inline void set_St(const Vec &st) {
        if(st.size() != N)
            throw std::logic_error("Mismatch between cross ownership matrix and assets!");
        St = st;
        St_full = S0.array()*St.array();
    }

    void re_init(const Eigen::Ref<const Mat>& M_new)
    {
        if(M_new.rows() != S0.size()) throw std::logic_error("re-initialized with wrong M size!");
#if USE_SPARSE_INTERNAL
        M = M_new.sparseView();
        M.makeCompressed();
#else
        M = M_new;
#endif
    }

    void re_init(const Eigen::Ref<const Mat>& M_new, const Eigen::Ref<const Vec> &s0, const Eigen::Ref<const Vec> &d, const Eigen::Ref<const Vec> &sigma_) {
        N = M_new.rows();
        //Jrs.resize(2*N, 2*N);
        //J_a.resize(2*N, N);
        x.resize(2*N);
        x = Eigen::VectorXd::Zero(2*N);
#if USE_SPARSE_INTERNAL
        M = M_new.sparseView();
        M.makeCompressed();
        Id.resize(2*N, 2*N);
        Id.setIdentity();
#else
        M = M_new;
        lu = Eigen::PartialPivLU<Eigen::MatrixXd>(2*N);
        Id.resize(2*N, 2*N);
        Id.setIdentity();
#endif
        St.resize(N);
        St_full.resize(N);
        St = Eigen::VectorXd::Constant(N, 1.0);
        if(s0.size() != N)
            throw std::logic_error("Mismatch between cross ownership matrix and assets prefactor!");
        S0 = s0;
        sigma.resize(N);
        sigma = sigma_;
        if(d.size() != N)
            throw std::logic_error("Mismatch between cross ownership matrix and debts!");
        debt = d;
        jacobian_set = false;
        initialized = true;
    }

    //@TODO: consistent return typex
    inline const Vec get_S0() const {
        return S0;
    }

    inline const Vec get_St() const {
        LOG(WARNING) << "St does NOT contain S0!";
        return St;
    }

    inline const Vec get_debt() const {
        return debt;
    }

    inline const Mat get_M() const {
#if USE_SPARSE_INTERNAL
        return Eigen::MatrixXd(M);
#else
        return M;
#endif
    }

    const Vec get_assets();

    //@TODO: move implementation to *.cpp
    const Vec get_rs() {
        return x;
    }

    const Vec get_valuation() {
        Vec res = (x.head(N) + x.tail(N));
        return res;
    }

   const Vec get_solvent() {
        return solvent;
   }

    void set_jacobian();

    Mat get_delta_v1() const;

    Mat get_vega(const Eigen::MatrixXd Z) const;

    Mat get_theta(const Eigen::MatrixXd Z) const;

    Mat get_rho() const;
    /*
    std::vector<double> ret;
    ret.resize(N);
    Eigen::VectorXd::Map(&ret[0], N) = x.head(N) + x.tail(N);
    return ret;*/

    Eigen::MatrixXd get_pi() const;

    Eigen::MatrixXd get_scalar_allGreeks(const Eigen::Ref<const Mat>& Z) const;

    void debug_print();

};
#endif // SRC_MULTIVAR_BLACKSCHOLES_HPP_


/**
 * @file OcpDescription.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-07
 */

#pragma once

#include <vector>
#include <Eigen/Dense>
#include <solver_lqr/SolverLqrSetting.hpp>

namespace solverlqr {

  //! Class to define State of OCP
  class StateBase
  {
    public:
	  StateBase(){}
	  StateBase(int xdim) { state_ = Eigen::VectorXd(xdim).setZero(); }
	  ~StateBase(){}

	  StateBase& operator= (const StateBase& rhs) { state_ = rhs.stateVector(); return *this; }
	  StateBase  operator* (double scalar) const { StateBase new_state; new_state.stateVector() = scalar*state_; return new_state; }
	  StateBase& operator*=(double scalar) { state_ *= scalar; return *this; }
	  StateBase  operator+ (const StateBase& rhs) const { StateBase new_state; new_state.stateVector() = state_+rhs.stateVector(); return new_state; }
	  StateBase& operator+=(const StateBase& rhs) { state_ += rhs.stateVector(); return *this; }
	  StateBase  operator/ (double scalar) const { StateBase new_state; new_state.stateVector() = state_/scalar; return new_state; }
	  StateBase& operator/=(double scalar) { state_ /= scalar; return *this; }

      Eigen::VectorXd& stateVector() { return state_; }
      const Eigen::VectorXd& stateVector() const { return state_; }
      void resize(int xdim) { state_ = Eigen::VectorXd(xdim).setZero(); }

    private:
      Eigen::VectorXd state_;
  };
  StateBase operator* (double scalar, const StateBase& rhs);
  StateBase operator- (const StateBase& lhs, const StateBase& rhs);

  class StateSequence
  {
    public:
      StateSequence(){}
      StateSequence(int tdim, int xdim) { this->resize(tdim, xdim); }
      ~StateSequence(){}

      void resize(int tdim, int xdim);
      void setRandom(double scaling = 1.0);
      int size() const { return stateseq_.size(); }
      StateBase& state(int id) { return stateseq_[id]; }
      const StateBase& state(int id) const { return stateseq_[id]; }

    private:
      std::vector<StateBase> stateseq_;
  };

  //! Class to define Control of OCP
  class ControlBase
  {
    public:
      ControlBase(){}
      ControlBase(int xdim, int udim);
      ~ControlBase(){}

      ControlBase& operator= (const ControlBase& rhs);
      ControlBase  operator* (double scalar) const;
      ControlBase& operator*=(double scalar);
      ControlBase  operator+ (const ControlBase& rhs) const;
      ControlBase& operator+=(const ControlBase& rhs);

      void setZero() { feedforward_.setZero(); }
      Eigen::MatrixXd& feedback() { return feedback_; }
      Eigen::VectorXd& feedforward() { return feedforward_; }
      const Eigen::MatrixXd& feedback() const { return feedback_; }
      const Eigen::VectorXd& feedforward() const { return feedforward_; }
      void setRandom(double scaling = 1.0) { feedforward_.setRandom(); feedforward_ *= scaling; }

    private:
      Eigen::MatrixXd feedback_;
      Eigen::VectorXd feedforward_;
  };
  ControlBase operator*(double lhs_scalar, const ControlBase& rhs);

  class ControlSequence
  {
    public:
      ControlSequence(){}
      ControlSequence(int tdim, int xdim, int udim) { this->resize(tdim, xdim, udim); }
      ~ControlSequence(){}

      void setRandom(double scaling = 1.0);
      void resize(int tdim, int xdim, int udim);
      int size() const { return controlseq_.size(); }
      ControlBase& control(int id) { return controlseq_[id]; }
      const ControlBase& control(int id) const { return controlseq_[id]; }

      ControlSequence& operator= (const ControlSequence& rhs);
      ControlSequence  operator* (double scalar) const;
      ControlSequence& operator*=(double scalar);
      ControlSequence  operator+ (const ControlSequence& rhs) const;
      ControlSequence& operator+=(const ControlSequence& rhs);

      static double controlSequenceGradientNorm(const ControlSequence& u1, const ControlSequence& u2);

    private:
      std::vector<ControlBase> controlseq_;
  };
  ControlSequence operator*(double lhs_scalar, const ControlSequence& rhs);

  //! Optimal Control Problem Description Class
  class OcpBase
  {
    public:
	  OcpBase(){}
      ~OcpBase(){}
      void initialize(const SolverLqrSetting& setting);

      const double& dt() const { return this->getLqrSetting().get(SolverLqrDoubleParam_TimeStep); }
      const int& tdim() const { return this->getLqrSetting().get(SolverLqrIntParam_TimeDimension); }
      const int& xdim() const { return this->getLqrSetting().get(SolverLqrIntParam_StateDimension); }
      const int& udim() const { return this->getLqrSetting().get(SolverLqrIntParam_ControlDimension); }

      StateSequence& stateSeq() { return stateseq_; }
      ControlSequence& controlSeq() { return controlseq_; }
      const StateSequence& stateSeq() const { return stateseq_; }
      const ControlSequence& controlSeq() const { return controlseq_; }
      const SolverLqrSetting& getLqrSetting() const { return *setting_; }

      virtual void configure(const YAML::Node& user_parameters) = 0;
      virtual Eigen::MatrixXd processNoiseFilter(int time_id) const;
      virtual Eigen::MatrixXd measurementNoiseFilter(int time_id) const;
      virtual StateBase dynamics(const StateBase& state, const ControlBase& control, int time_id) = 0;
      void internal_dynamics(const StateBase& state, const ControlBase& control, StateBase& new_state, int time_id);
      virtual double objective(const StateBase& state, const ControlBase& control, int time_id, bool is_final_timestep) = 0;

    private:
      friend class Estimator;
      friend class ForwardPass;
      friend class BackwardPass;
      friend class FiniteDifferences;

    private:
      StateSequence stateseq_;
      ControlSequence controlseq_;
      const SolverLqrSetting* setting_;
  };

}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file modal_formula_find_test.cpp
/// \brief Test for find functions.

#include <algorithm>
#include <iterator>
#include <set>
#include <vector>
#include <boost/test/minimal.hpp>
#include "mcrl2/data/detail/print_utility.h"
#include "mcrl2/lps/parse.h"
#include "mcrl2/modal_formula/find.h"
#include "mcrl2/modal_formula/parse.h"

using namespace mcrl2;
using namespace mcrl2::state_formulas;

std::string SPEC =
  "glob                      \n"
  "  m: Nat;                 \n"
  "                          \n"
  "act                       \n"
  "  a: Nat;                 \n"
  "                          \n"
  "proc                      \n"
  "  P(n:Nat) = a(m).P(n+1); \n"
  "                          \n"
  "init P(0);                \n"
  ;

inline
data::variable nat(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_nat::nat());
}

inline
data::variable pos(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_pos::pos());
}

inline
data::variable bool_(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_bool::bool_());
}

void test_find()
{
  lps::specification spec = lps::parse_linear_process_specification(SPEC);
  state_formula f = parse_state_formula("(mu X. X) && (forall b:Bool. true)", spec);

  //--- find_all_variables ---//
  data::variable b = bool_("b");
  std::set<data::variable> v = state_formulas::find_all_variables(f);
  BOOST_CHECK(v.find(b) != v.end());

  //--- find_sort_expressions ---//
  std::set<data::sort_expression> e = state_formulas::find_sort_expressions(f);
  std::cout << "e.size() = " << e.size() << std::endl;
  BOOST_CHECK(std::find(e.begin(), e.end(), data::sort_bool::bool_()) != e.end());
}

void test_free_variables()
{
  variable X("X", data::data_expression_list());
  data::variable b = bool_("b");
  data::variable c = bool_("c");
  data::data_expression phi = data::equal_to(b, c);
  data::variable_list v { b };
  state_formula f = forall(v, phi);
  std::set<data::variable> free_variables = state_formulas::find_free_variables(f);
  std::cout << "free variables: " << core::detail::print_set(free_variables) << std::endl;
  BOOST_CHECK(free_variables.find(b) == free_variables.end());
  BOOST_CHECK(free_variables.find(c) != free_variables.end());
}

int test_main(int argc, char* argv[])
{
  test_find();
  test_free_variables();

  return EXIT_SUCCESS;
}


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "PositionBasedElasticRodsModel.h"
#include "PositionBasedElasticRodsConstraints.h"
#include "PositionBasedElasticRodsTSC.h"
#include <iostream>
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

#define _USE_MATH_DEFINES
#include "math.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void timeStep ();
void buildModel ();
void createRod();
void render ();
void reset();

void TW_CALL setDistanceStiffness(const void* value, void* clientData);
void TW_CALL getDistanceStiffness(void* value, void* clientData);

void TW_CALL setRestDarbouxX(const void *value, void *clientData);
void TW_CALL setRestDarbouxY(const void *value, void *clientData);
void TW_CALL setRestDarbouxZ(const void *value, void *clientData);

void TW_CALL getRestDarbouxX(void *value, void *clientData);
void TW_CALL getRestDarbouxY(void *value, void *clientData);
void TW_CALL getRestDarbouxZ(void *value, void *clientData);

void TW_CALL getBendingAndTwistingStiffnessX(void *value, void *clientData);
void TW_CALL getBendingAndTwistingStiffnessY(void *value, void *clientData);
void TW_CALL getBendingAndTwistingStiffnessZ(void *value, void *clientData);

void TW_CALL setBendingAndTwistingStiffnessX(const void *value, void *clientData);
void TW_CALL setBendingAndTwistingStiffnessY(const void *value, void *clientData);
void TW_CALL setBendingAndTwistingStiffnessZ(const void *value, void *clientData);

DemoBase *base;
PositionBasedElasticRodsTSC sim;
Real distanceStiffness = 1.0;

const int numberOfPoints = 32;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	base = new DemoBase();
	base->init(argc, argv, "Elastic rod demo");

	PositionBasedElasticRodsModel *model = new PositionBasedElasticRodsModel();
	model->init();
	Simulation::getCurrent()->setModel(model);
	PositionBasedElasticRodsTSC *tsc = new PositionBasedElasticRodsTSC();
	tsc->init();
	delete Simulation::getCurrent()->getTimeStep();
	Simulation::getCurrent()->setTimeStep(tsc);

	buildModel();

	base->createParameterGUI();

	// OpenGL
	MiniGL::setClientIdleFunc (timeStep);
	MiniGL::addKeyFunc('r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 5.0, 10.0), Vector3r (5.0, 0.0, 0.0));

	TwAddVarCB(MiniGL::getTweakBar(), "DistanceStiffness", TW_TYPE_REAL, setDistanceStiffness, getDistanceStiffness, model, " label='Distance constraint stiffness'  min=0.0 step=0.1 precision=4 group='BendTwist' ");

	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxX", TW_TYPE_REAL, setRestDarbouxX, getRestDarbouxX, model, " label='Rest Darboux X'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxY", TW_TYPE_REAL, setRestDarbouxY, getRestDarbouxY, model, " label='Rest Darboux Y'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "RestDarbouxZ", TW_TYPE_REAL, setRestDarbouxZ, getRestDarbouxZ, model, " label='Rest Darboux Z'  min=-1.0 max = 1.0 step=0.01 precision=2 group=BendTwist ");

	TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessX", TW_TYPE_REAL, setBendingAndTwistingStiffnessX, getBendingAndTwistingStiffnessX, model, " label='Bending X stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessY", TW_TYPE_REAL, setBendingAndTwistingStiffnessY, getBendingAndTwistingStiffnessY, model, " label='Bending Y stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");
	TwAddVarCB(MiniGL::getTweakBar(), "BendingAndTwistingStiffnessZ", TW_TYPE_REAL, setBendingAndTwistingStiffnessZ, getBendingAndTwistingStiffnessZ, model, " label='Twisting stiffness'  min=0.01 max = 1.0 step=0.01 precision=2 group=BendTwist ");

	MiniGL::mainLoop ();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();
}

void buildModel()
{
	TimeManager::getCurrent()->setTimeStepSize(0.002f);
	PositionBasedElasticRodsModel *model = (PositionBasedElasticRodsModel*)Simulation::getCurrent()->getModel();
	model->setBendingAndTwistingStiffness(Vector3r(0.5, 0.5, 0.5));

	sim.setDamping(0.001f);

	createRod();
}

void timeStep ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		Simulation::getCurrent()->getTimeStep()->step(*model);
		STOP_TIMING_AVG;
	}
}

void render()
{
	base->render();

	// Draw sim model
	PositionBasedElasticRodsModel *model = (PositionBasedElasticRodsModel*) Simulation::getCurrent()->getModel();
	ParticleData &pd = model->getParticles();
	ParticleData &ghostParticles = model->getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model->getConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float pointColor[4] = { 0.1f, 0.2f, 0.6f, 1 };
	float ghostPointColor[4] = { 0.1f, 0.1f, 0.1f, 0.5f };
	float edgeColor[4] = { 0.0f, 0.6f, 0.2f, 1 };

	for (unsigned int i = 0; i < numberOfPoints; i++)
	{
		MiniGL::drawSphere(pd.getPosition(i), 0.07f, pointColor);
	}

	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		MiniGL::drawSphere(ghostParticles.getPosition(i), 0.07f, ghostPointColor);
		MiniGL::drawVector(pd.getPosition(i), pd.getPosition(i + 1), 0.2f, edgeColor);
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

/** Create the elastic rod model
*/
void createRod()
{
	PositionBasedElasticRodsModel *model = (PositionBasedElasticRodsModel*)Simulation::getCurrent()->getModel();
	ParticleData &particles = model->getParticles();
	ParticleData &ghostParticles = model->getGhostParticles();
	SimulationModel::ConstraintVector &constraints = model->getConstraints();

	//centreline points
	for (unsigned int i = 0; i < numberOfPoints; i++)
	{
		particles.addVertex(Vector3r(static_cast<Real>(0.25 * i), 0.0, 0.0));
	}

	//edge ghost points
	for (unsigned int i = 0; i < numberOfPoints-1; i++)
	{
		ghostParticles.addVertex(Vector3r(static_cast<Real>(0.25 * i) + static_cast<Real>(0.125), static_cast<Real>(0.25), 0.0));
	}

	//lock two first particles and first ghost point
	particles.setMass(0, 0.0f);
	particles.setMass(1, 0.0f);
	ghostParticles.setMass(0, 0.0f);

	for (unsigned int i = 0; i < numberOfPoints - 1; i++)
	{
		model->addDistanceConstraint(i, i + 1, distanceStiffness);
		model->addPerpendiculaBisectorConstraint(i, i + 1, i);
		model->addGhostPointEdgeDistanceConstraint(i, i + 1, i);

		if (i < numberOfPoints - 2)
		{
			//  Single rod element:
			//      D   E		//ghost points
			//		|	|
			//  --A---B---C--	// rod points
			int pA = i;
			int pB = i + 1;
			int pC = i + 2;
			int pD = i;
			int pE = i + 1;
			model->addDarbouxVectorConstraint(pA, pB, pC, pD, pE);
		}
	}
}

void TW_CALL setRestDarbouxX(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[0] = *(const Real *)(value);
}

void TW_CALL setRestDarbouxY(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[1] = *(const Real *)(value);
}

void TW_CALL setRestDarbouxZ(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[2] = *(const Real *)(value);
}

void TW_CALL getRestDarbouxX(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[0];
}

void TW_CALL getRestDarbouxY(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[1];
}

void TW_CALL getRestDarbouxZ(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getRestDarbouxVector()[2];
}

void TW_CALL setBendingAndTwistingStiffnessX(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[0] = *(const Real *)(value);
}

void TW_CALL setBendingAndTwistingStiffnessY(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[1] = *(const Real *)(value);
}

void TW_CALL setBendingAndTwistingStiffnessZ(const void *value, void *clientData)
{
	((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[2] = *(const Real *)(value);
}

void TW_CALL getBendingAndTwistingStiffnessX(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[0];
}

void TW_CALL getBendingAndTwistingStiffnessY(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[1];
}

void TW_CALL getBendingAndTwistingStiffnessZ(void *value, void *clientData)
{
	*(Real *)(value) = ((PositionBasedElasticRodsModel*)clientData)->getBendingAndTwistingStiffness()[2];
}

void TW_CALL setDistanceStiffness(const void* value, void* clientData)
{
	distanceStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint, Real, &DistanceConstraint::m_stiffness>(distanceStiffness);
	((SimulationModel*)clientData)->setConstraintValue<DistanceConstraint_XPBD, Real, &DistanceConstraint_XPBD::m_stiffness>(distanceStiffness);
}

void TW_CALL getDistanceStiffness(void* value, void* clientData)
{
	*(Real*)(value) = distanceStiffness;
}


// Test for boost/core/bit.hpp (bit_floor)
//
// Copyright 2020 Peter Dimov
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt

#include <boost/core/bit.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/core/detail/splitmix64.hpp>
#include <boost/cstdint.hpp>
#include <limits>

template<class T> void test_bit_floor( T x )
{
    T y = boost::core::bit_floor( x );

    if( x == 0 )
    {
        BOOST_TEST_EQ( y, 0 );
    }
    else
    {
        BOOST_TEST( boost::core::has_single_bit( y ) );
        BOOST_TEST_LE( +y, +x );
        BOOST_TEST_GT( +y, x >> 1 );
    }
}

int main()
{
    {
        test_bit_floor( static_cast<unsigned char>( 0 ) );
        test_bit_floor( static_cast<unsigned short>( 0 ) );
        test_bit_floor( static_cast<unsigned int>( 0 ) );
        test_bit_floor( static_cast<unsigned long>( 0 ) );
        test_bit_floor( static_cast<unsigned long long>( 0 ) );
    }

    boost::detail::splitmix64 rng;

    for( int i = 0; i < 1000; ++i )
    {
        boost::uint64_t x = rng();

        test_bit_floor( static_cast<unsigned char>( x ) );
        test_bit_floor( static_cast<unsigned short>( x ) );
        test_bit_floor( static_cast<unsigned int>( x ) );
        test_bit_floor( static_cast<unsigned long>( x ) );
        test_bit_floor( static_cast<unsigned long long>( x ) );
    }

    return boost::report_errors();
}


// shasta.
#include "dset64Test.hpp"
#include "vector.hpp"
using namespace shasta;

// Boost libraries.
#include <boost/pending/disjoint_sets.hpp>

// Standard libraries.
#include "chrono.hpp"
#include "iostream.hpp"
#include "iterator.hpp"
#include <random>

void shasta::dset64Test(
    uint64_t n,             // The number of items (vertices).
    uint64_t m,             // The number of union operations (edges).
    uint64_t threadCount,   // The number of threads to use.
    uint64_t batchSize,     // The number of union operations per batch.
    int seed                // The random seed.
    )
{
    Dset64Test(n, m, threadCount, batchSize, seed);
}

Dset64Test::Dset64Test(
    uint64_t n,             // The number of items (vertices).
    uint64_t m,             // The number of union operations (edges).
    uint64_t threadCount,   // The number of threads to use.
    uint64_t batchSize,     // The number of union operations per batch.
    int seed                // The random seed.
    ) : MultithreadedObject(*this)
{

    // Randomly generate the union operations.
    // Prepare to generate uniformly distributed numbers between 0 and 1.
    std::mt19937 randomSource(seed);
    std::uniform_int_distribution<uint64_t> uniformDistribution(0, n-1);
    edges.resize(m);
    // cout << "Edges:" << endl;
    for(auto& p: edges) {
        p.first = uniformDistribution(randomSource);
        p.second = uniformDistribution(randomSource);
        // cout << p.first << " " << p.second << endl;
    }

    // First, do it sequentially using the disjoint sets implementation
    // in the Boost library.
    vector< vector<uint64_t> > sortedComponentsBoost;
    {
        vector<uint64_t> rank(n);
        vector<uint64_t> parent(n);
        boost::disjoint_sets<uint64_t*, uint64_t*> disjointSets(&rank[0], &parent[0]);
        for(uint64_t i=0; i<n; i++) {
            disjointSets.make_set(i);
        }
        const auto t0 = std::chrono::steady_clock::now();
        for(const auto& p: edges) {
            disjointSets.union_set(p.first, p.second);
        }
        const auto t1 = std::chrono::steady_clock::now();
        cout << "Boost implementation ran in " << seconds(t1-t0) << "s." << endl;

        // Gather the components.
        std::map<uint64_t, vector<uint64_t> > componentTable;
        for(uint64_t i=0; i<n; i++) {
            componentTable[disjointSets.find_set(i)].push_back(i);
        }
        getSortedComponents(componentTable, sortedComponentsBoost);

        /*
        cout << "sortedComponentsBoost" << endl;
        for(uint64_t i=0; i<sortedComponentsBoost.size(); i++) {
            cout << i << ": ";
            const auto& v = sortedComponentsBoost[i];
            copy(v.begin(), v.end(), ostream_iterator<uint64_t>(cout, " "));
            cout << endl;
        }
        */
    }

    // Now, do it using dset64.hpp/dset64-gccAtomic.hpp, sequentially.
    vector< vector<uint64_t> > sortedComponentsSequential;
    {
        using Aint = DisjointSets::Aint;
        vector<Aint> data(n);
        DisjointSets disjointSets(&data.front(), n);
        const auto t0 = std::chrono::steady_clock::now();
        for(const auto& p: edges) {
            disjointSets.unite(p.first, p.second);
        }
        const auto t1 = std::chrono::steady_clock::now();
        cout << "Sequential dset64 ran in " << seconds(t1-t0) << "s." << endl;

        // Gather the components.
        std::map<uint64_t, vector<uint64_t> > componentTable;
        for(uint64_t i=0; i<n; i++) {
            componentTable[disjointSets.find(i)].push_back(i);
        }
        getSortedComponents(componentTable, sortedComponentsSequential);
    }
    /*
    cout << "sortedComponentsSequential" << endl;
    for(uint64_t i=0; i<sortedComponentsSequential.size(); i++) {
        cout << i << ": ";
        const auto& v = sortedComponentsSequential[i];
        copy(v.begin(), v.end(), ostream_iterator<uint64_t>(cout, " "));
        cout << endl;
    }
    */
    SHASTA_ASSERT(sortedComponentsSequential == sortedComponentsBoost);

    // Now, do it using dset64.hpp/dset64-gccAtomic.hpp, using the specified number of threads.
    vector< vector<uint64_t> > sortedComponentsParallel;
    {
        using Aint = DisjointSets::Aint;
        vector<Aint> data(n);
        DisjointSets disjointSets(&data.front(), n);
        disjointSetsPointer = &disjointSets;
        const auto t0 = std::chrono::steady_clock::now();
        setupLoadBalancing(edges.size(), batchSize);
        runThreads(&Dset64Test::threadFunction, threadCount);
        const auto t1 = std::chrono::steady_clock::now();
        cout << "Parallel dset64 ran in " << seconds(t1-t0) << "s." << endl;

        // Gather the components.
        std::map<uint64_t, vector<uint64_t> > componentTable;
        for(uint64_t i=0; i<n; i++) {
            componentTable[disjointSets.find(i)].push_back(i);
        }
        getSortedComponents(componentTable, sortedComponentsParallel);
    }
    /*
    cout << "sortedComponentsParallel" << endl;
    for(uint64_t i=0; i<sortedComponentsParallel.size(); i++) {
        cout << i << ": ";
        const auto& v = sortedComponentsParallel[i];
        copy(v.begin(), v.end(), ostream_iterator<uint64_t>(cout, " "));
        cout << endl;
    }
    */
    SHASTA_ASSERT(sortedComponentsParallel == sortedComponentsBoost);

    cout << "No error found. All algorithms found " << sortedComponentsBoost.size();
    cout << " identical connected components." << endl;
}

void Dset64Test::threadFunction(size_t threadId)
{
    uint64_t begin;
    uint64_t end;
    while(getNextBatch(begin, end)) {
        // cout << "Thread function processing batch " << begin << " " << end << endl;
        for(uint64_t i=begin; i!=end; ++i) {
            const auto& p = edges[i];
            // cout << "Thread function processing edge " << p.first << " " << p.second << endl;
            disjointSetsPointer->unite(p.first, p.second);
        }
    }

}

void Dset64Test::getSortedComponents(
    const std::map<uint64_t, vector<uint64_t> >& componentTable,
    vector< vector<uint64_t> >& sortedComponents
)
{
    sortedComponents.clear();
    for(const auto& p: componentTable) {
        sortedComponents.push_back(p.second);
    }
    sort(sortedComponents.begin(), sortedComponents.end());
}