#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <iomanip>
#include "RBF.h"
#include "MLS.h"
#include "HermiteRBF.h"
#include "GeneralizedMLS.h"
#include <boost/mpl/list.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/int.hpp>
using namespace std;
using namespace kt84;
namespace mpl = boost::mpl;

// utility for interpolation algorithms |
//--------------------------------------+
template <template <int, int, class, int> class FuncT>
struct FuncUtil;
template <> struct FuncUtil<RBF> {
    static const char* getName() { return "RBF"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v); }
    template <class Func> static void preprocess(Func& f) { f.factorize_and_solve(); }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return f.gradient(p); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].first) - f.constraints[i].second).norm();
            result.first = max<double>(result.first, error_v);
        }
        return result;
    }
};
template <> struct FuncUtil<MLS> {
    static const char* getName() { return "MLS"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v); }
    template <class Func> static void preprocess(Func& f) { }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return typename Func::Gradient::Zero(); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].first) - f.constraints[i].second).norm();
            result.first = max<double>(result.first, error_v);
        }
        return result;
    }
};
template <> struct FuncUtil<HermiteRBF> {
    static const char* getName() { return "HRBF"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v, g); }
    template <class Func> static void preprocess(Func& f) { f.factorize_and_solve(); }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return f.gradient(p); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].get<0>()) - f.constraints[i].get<1>()).norm();
            double error_g = (f.gradient_fd(f.constraints[i].get<0>(), epsilon) - f.constraints[i].get<2>()).norm();
            result.first  = max<double>(result.first , error_v);
            result.second = max<double>(result.second, error_g);
        }
        return result;
    }
};
template <> struct FuncUtil<GeneralizedMLS> {
    static const char* getName() { return "GMLS"; }
    template <class Func> static void add_constraint(Func& f, const typename Func::Point& p, const typename Func::Value& v, const typename Func::Gradient& g) { f.add_constraint(p, v, g); }
    template <class Func> static void preprocess(Func& f) { }
    template <class Func> static typename Func::Gradient gradient(const Func& f, const typename Func::Point& p) { return typename Func::Gradient::Zero(); }
    template <class Func>
    static pair<double, double> getMaxError(const Func& f, double epsilon) {
        pair<double, double> result(0, 0);
        for (size_t i = 0; i < f.constraints.size(); ++i) {
            double error_v = (f(f.constraints[i].get<0>()) - f.constraints[i].get<1>()).norm();
            double error_g = (f.gradient_fd(f.constraints[i].get<0>(), epsilon) - f.constraints[i].get<2>()).norm();
            result.first  = max<double>(result.first , error_v);
            result.second = max<double>(result.second, error_g);
        }
        return result;
    }
};

// utility for RBF kernels |
//-------------------------+
template <class RBFKernel_Core>
struct KernelUtil;
template <> struct KernelUtil<RBFKernel_Gaussian      > { static const char* getName() { return "Gauss"; } static RBFKernel_Gaussian      ::Param getParam() { return RBFKernel_Gaussian      ::Param(10); } };
template <> struct KernelUtil<RBFKernel_SquaredInverse> { static const char* getName() { return "SqInv"; } static RBFKernel_SquaredInverse::Param getParam() { return RBFKernel_SquaredInverse::Param(20); } };
template <> struct KernelUtil<RBFKernel_Wendland      > { static const char* getName() { return "Wendl"; } static RBFKernel_Wendland      ::Param getParam() { return RBFKernel_Wendland      ::Param(1.5); } };
template <> struct KernelUtil<RBFKernel_Cubed         > { static const char* getName() { return "Cubed"; } static RBFKernel_Cubed         ::Param getParam() { return RBFKernel_Cubed         ::Param()   ; } };
template <> struct KernelUtil<RBFKernel_Identity      > { static const char* getName() { return "Ident"; } static RBFKernel_Identity      ::Param getParam() { return RBFKernel_Identity      ::Param()   ; } };
template <> struct KernelUtil<RBFKernel_SquaredLog    > { static const char* getName() { return "SqLog"; } static RBFKernel_SquaredLog    ::Param getParam() { return RBFKernel_SquaredLog    ::Param()   ; } };

// test functions |
//----------------+
template <int Dim, template <int, int, class, int> class FuncT>
struct Tester;

// 1D test
template <template <int, int, class, int> class FuncT>
struct Tester<1, FuncT> {
    template <class RBFKernel_Core, int DegreePolynomial>
    static void go(const typename RBFKernel_Core::Param& param) {
        typedef FuncT<1, 1, RBFKernel_Core, DegreePolynomial> Func;
        typedef Func::Point Point;
        typedef Func::Value Value;
        typedef Func::Gradient Gradient;

        Func f;
        f.kernel.param() = param;

        ifstream fin("input1d.txt");
        if (!fin)
            throw exception("input1d.txt not found!");
        while (!fin.eof()) {
            Point p;
            Value v;
            Gradient g;

            fin >> p[0] >> v[0] >> g[0];
            if (!fin.eof())
                FuncUtil<FuncT>::add_constraint(f, p, v, g);
        }

        FuncUtil<FuncT>::preprocess(f);

        stringstream fname;
        fname << "test1d_" << FuncUtil<FuncT>::getName() << "_" << KernelUtil<RBFKernel_Core>::getName() << "_p" << DegreePolynomial << ".txt";
        ofstream fout(fname.str().c_str());
        // report error first
        pair<double, double> maxError = FuncUtil<FuncT>::getMaxError(f, 0.00001);
        fout << "# max error (value, gradient): (" << maxError.first << ", " << maxError.second << ")" << endl << endl;
        // plot data
        const int N = 100;
        for (double i = 0; i <= N; ++i) {
            Func::Point point = Func::Point::Constant(i / N);
            Func::Value value = f(point);
            Func::Gradient gradient = FuncUtil<FuncT>::gradient(f, point);
            Func::Gradient gradient_fd = f.gradient_fd(point, 0.00001);
            fout
                << setw(12) << point[0] << " "
                << setw(12) << value[0] << " "
                << setw(12) << gradient[0] << " "
                << setw(12) << gradient_fd[0] << " "
                << endl;
        }
    }
    // generate gnuplot commands |
    //---------------------------+
    template <class RBFKernel_Core>
    static void genCmd(int DegreePolynomial) {
        stringstream fname;
        const char* funcName = FuncUtil<FuncT>::getName();
        const char* kernelName = KernelUtil<RBFKernel_Core>::getName();
        fname << "cmd1d_" << funcName << "_" << kernelName << ".txt";
        ofstream fout(fname.str().c_str());
        fout << "plot\\" << endl;
        for (int i = 0; i <= DegreePolynomial; ++i) {
            fout << "    \"test1d_" << funcName << "_" << kernelName << "_p" << i << ".txt\" w linespoints";
            if (i < DegreePolynomial)
                fout << ",\\";
            fout << endl;
        }
        fout << "pause -1" << endl;
        fout.close();
        fname.str("");
        fname << "cmd1d_" << funcName << "_" << kernelName << "_g.txt";
        fout.open(fname.str().c_str());
        fout << "plot\\" << endl;
        for (int i = 0; i <= DegreePolynomial; ++i) {
            fout << "    \"test1d_" << funcName << "_" << kernelName << "_p" << i << ".txt\" u 1:4 w linespoints";
            if (i < DegreePolynomial)
                fout << ",\\";
            fout << endl;
        }
        fout << "pause -1" << endl;
    }
};

// 2D test
template <template <int, int, class, int> class FuncT>
struct Tester<2, FuncT> {
    template <class RBFKernel_Core, int DegreePolynomial>
    static void go(const typename RBFKernel_Core::Param& param) {
        typedef FuncT<2, 1, RBFKernel_Core, DegreePolynomial> Func;
        typedef Func::Point Point;
        typedef Func::Value Value;
        typedef Func::Gradient Gradient;

        Func f;
        f.kernel.param() = param;

        ifstream fin("input2d.txt");
        if (!fin)
            throw exception("input2d.txt not found!");
        while (!fin.eof()) {
            Point p;
            Value v;
            Gradient g;

            fin >> p[0] >> p[1] >> v[0] >> g[0] >> g[1];
            if (!fin.eof())
                FuncUtil<FuncT>::add_constraint(f, p, v, g);
        }

        FuncUtil<FuncT>::preprocess(f);

        stringstream fname;
        fname << "test2d_" << FuncUtil<FuncT>::getName() << "_" << KernelUtil<RBFKernel_Core>::getName() << "_p" << DegreePolynomial << ".txt";
        ofstream fout(fname.str().c_str());
        // report error first
        pair<double, double> maxError = FuncUtil<FuncT>::getMaxError(f, 0.00001);
        fout << "# max error (value, gradient): (" << maxError.first << ", " << maxError.second << ")" << endl << endl;
        // plot data
        const int N = 32;
        for (double j = 0; j <= N; ++j) {
            for (double i = 0; i <= N; ++i) {
                Func::Point point = Func::Point(i / N, j / N);
                Func::Value value = f(point);
                Func::Gradient gradient = FuncUtil<FuncT>::gradient(f, point);
                Func::Gradient gradient_fd = f.gradient_fd(point, 0.00001);
                fout
                    << setw(12) << point[0] << " "
                    << setw(12) << point[1] << " "
                    << setw(12) << value[0] << " "
                    << setw(12) << gradient[0] << " "
                    << setw(12) << gradient[1] << " "
                    << setw(12) << gradient_fd[0] << " "
                    << setw(12) << gradient_fd[1] << " "
                    << endl;
            }
            fout << endl;
        }
    }
    // generate gnuplot commands |
    //---------------------------+
    template <class RBFKernel_Core>
    static void genCmd(int DegreePolynomial) {
        const char* funcName = FuncUtil<FuncT>::getName();
        const char* kernelName = KernelUtil<RBFKernel_Core>::getName();
        for (int i = 0; i <= DegreePolynomial; ++i) {
            // value
            stringstream fname;
            fname << "cmd2d_" << funcName << "_" << kernelName << "_p" << i << ".txt";
            ofstream fout(fname.str().c_str());
            fout << "set pm3d; unset surface; set pm3d hidden3d 100;set view 60, 320\n";
            fout << "splot \"test2d_" << funcName << "_" << kernelName << "_p" << i << ".txt\" u 1:2:3\n";
            fout << "pause -1\n";
            fout.close();
            // gradient
            fname.str("");
            fname << "cmd2d_" << funcName << "_" << kernelName << "_p" << i << "_g.txt";
            fout.open(fname.str().c_str());
            fout << "set pm3d; unset surface; set pm3d hidden3d 100;set view 60, 320;\n";
            fout << "splot \"test2d_" << funcName << "_" << kernelName << "_p" << i << "_g.txt\" u 1:2:6\n";
            fout << "pause -1\n";
        }
    }
};

// list of valid kernels for each algorithm |
//------------------------------------------+
template <template <int, int, class, int> class FuncT>
struct KernelList;
template <> struct KernelList<RBF> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Wendland      ,
        RBFKernel_Cubed         ,
        RBFKernel_Identity      ,
        RBFKernel_SquaredLog
    > Value;
};
template <> struct KernelList<HermiteRBF> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Cubed
    > Value;
};
template <> struct KernelList<MLS> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Wendland
    > Value;
};
template <> struct KernelList<GeneralizedMLS> {
    typedef mpl::list<
        RBFKernel_Gaussian      ,
        RBFKernel_SquaredInverse,
        RBFKernel_Wendland
    > Value;
};

// utility for looping over parameters |
//-------------------------------------+
template <template <int, int, class, int> class FuncT>
struct TestLooper {
    static void go() {
        typedef mpl::list<
            mpl::int_<1>,
            mpl::int_<2>
        > DimList;
        mpl::for_each<DimList>(LoopDim());
    }
    struct LoopDim {
        template <class IntDim>
        void operator()(const IntDim&) const {
            mpl::for_each<KernelList<FuncT>::Value>(LoopKernel<IntDim::value>());
        }
        template <int Dim>
        struct LoopKernel {
            template <class RBFKernel>
            void operator()(const RBFKernel&) const {
                typedef mpl::list<
                    mpl::int_<0>,
                    mpl::int_<1>,
                    mpl::int_<2>
                > DegreeList;
                mpl::for_each<DegreeList>(LoopDegree<RBFKernel>());
                Tester<Dim, FuncT>::genCmd<RBFKernel>(2);
            }
            template <class RBFKernel>
            struct LoopDegree {
                template <class IntDegree>
                void operator()(const IntDegree&) const {
                    Tester<Dim, FuncT>::go<RBFKernel, IntDegree::value>(KernelUtil<RBFKernel>::getParam());
                }
            };
        };
    };
};

int main() {
    TestLooper<RBF           >::go();       // list of templates cannot be handled by Boost.MPL
    TestLooper<MLS           >::go();
    TestLooper<HermiteRBF    >::go();
    TestLooper<GeneralizedMLS>::go();
}


#include "writer.hpp"
#include <Eigen/Core>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>
#include <nlohmann/json.hpp>
#include <filesystem>

//! Initial condition: grid-aligned rectangle
//! @param[in] (x,y) point to evaulate the initial condition
//! @param[out] evaluation of the function at (x,y)
double ic(double x, double y) {
    if (x > -0.3 && x < 0.3 && y > -0.3 && y < 0.3)
        return 1.0;
    else
        return 0.0;
}

//! Load configuration values from a JSON file
//! @param[in] jsonFilename file (including path if needed) of config.json
//! @param[out] json object with parameters: T, {x,y}{min,max}, Nx, Ny, cfl
nlohmann::json loadConfig(std::string jsonFilename) {
    std::ifstream i(jsonFilename);
    assert(i.good() && "config.json not found in current or parent directory");

    nlohmann::json j;
    i >> j;
    return j;
}

//! Apply periodic boundary conditions to matrix u.
//! u has relevant values in the Nx x Ny submatrix at the center
//! @param[in] u  (Nx+2)x(Ny+2) matrix
void applyBoundaryConditions(Eigen::MatrixXd &u) {
// (write your solution here)
}

//! An implementation of the 1D upwind numerical flux
//! @param[in] uM  value to the left of the interface
//! @param[in] uR  value to the right of the interface
//! @param[in] a   velocity at the interface
double F(double uM, double uP, double a) {
    return std::max(a, 0.)*uM + std::min(a, 0.)*uP;
}

//! Compute one step of the upwind method in 2D
//! @param[in] u           matrix of size (Nx+2)x(Ny+2) which will contain U^{n+1}
//! @param[in] u_old       matrix of size (Nx+2)x(Ny+2) of values U^{n}
//! @param[in] dx, dy, dt  meshsteps and timestep
//! @param[in] a           velocity as a function of R^2 to R^2
void updateUpwind(Eigen::MatrixXd &u, Eigen::MatrixXd &u_old, double dx,
                  double dy, double dt, double xmin, double ymin,
                  const std::function<Eigen::Vector2d(double, double)> &a) {
// (write your solution here)
}

//! Clear the contents of the output file before starting
//! Useful because we write in append mode
//! @param[in] outfile  name of the file to be wiped
void wipeFile(std::string outfile) {
    std::ofstream outstrm;
    outstrm.open(outfile, std::ofstream::out | std::ofstream::trunc);
    outstrm.close();
}

int main() {
    // Path to the config file relative to the binary. Try a couple of likely locations.
    std::string config_file = std::filesystem::exists("../config.json") ? "../config.json" : "config.json";

    auto j = loadConfig(config_file);
    double T = j["T"];
    double xmin = j["xmin"], ymin = j["ymin"], xmax = j["xmax"], ymax = j["ymax"];
    int Nx = j["Nx"], Ny = j["Ny"];
    double cfl = j["cfl"];

    // Derived data
    double dx = (xmax-xmin)/Nx;
    double dy = (ymax-ymin)/Ny;
    auto a = [](double x, double y) { return Eigen::Vector2d(y, -x); };
    double max_ax = ymax; // maximum of a_1 in domain
    double max_ay = -xmin; // maximum of a_2 in domain
    double dt_max = cfl / (max_ax/dx + max_ay/dy);
    std::string outfile = "u.txt";
    Eigen::MatrixXd u(Nx+2, Ny+2); // include ghost cells

    // apply initial condition to (1..Nx)x(1..Ny)
// (write your solution here)
    applyBoundaryConditions(u); // Complete u with BCs
    Eigen::MatrixXd u_old = u;

    wipeFile(outfile); // clear contents of output file

    double t = 0;
    std::vector<double> times;
    times.push_back(t);
    appendMatrixToFile(outfile, u.block(1,1,Nx,Ny));

    // Iterate over time
    while(t < T) {
        double dt = std::min(dt_max, T-t); // make sure we don't go beyond T
        t += dt;

        // Call updateUpwind. Don't forget the boundary conditions!
// (write your solution here)

        appendMatrixToFile(outfile, u.block(1,1,Nx,Ny));
        u_old = u;
        times.push_back(t);
    }
    writeToFile("time.txt", times);
}


#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <gtest/gtest.h>
#include <mpi.h>

#include "tasktorrent/tasktorrent.hpp"

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef array<int, 2> int2;
typedef array<int, 3> int3;

int VERB = 0;
int n_threads_ = 4;
int n_ = 50;

// Simple pseudo random number
// https://locklessinc.com/articles/prng/
typedef unsigned long long u64b;
int rng64(u64b *s)
{
    u64b c = 7319936632422683419ULL;
    u64b x = s[1];

    /* Increment 128bit counter */
    s[0] += c;
    s[1] += c + (s[0] < c);

    /* Two h iterations */
    x ^= x >> 32;
    x *= c;
    x ^= x >> 32;
    x *= c;

    /* Perturb result */
    return static_cast<int>(x + s[0]);
}

void ddot(int n_threads, int block_size)
{
    // MPI info
    const int rank = comm_rank();
    const int n_ranks = comm_size();

    // Problem data
    // Function used to initialize x and y
    // i is a global index into the array
    // x[i] and y[i] are initialized using the value returned by array_initializer(i)
    function<int(int)> array_initializer = [](int i) {
        u64b seed[2];
        seed[0] = i;
        seed[1] = i + 2019;
        return rng64(seed) % 1000; // Do not make the integer too large
    };

    VectorXd z = VectorXd::Zero(n_threads);

    double partial_sum = 0.0;
    double ddot_sum = 0.0;

    // Initialize the communicator structure
    Communicator comm(MPI_COMM_WORLD, VERB);

    // Threadpool
    Threadpool tp(n_threads, &comm, VERB);

    Taskflow<int> dot_tf(&tp, VERB);
    Taskflow<int> add_tf(&tp, VERB);

    // Create active message
    auto am = comm.make_active_msg(
        [&ddot_sum](double &partial_sum) {
            ddot_sum += partial_sum;
        });

    // Log
    DepsLogger dlog(1000000);
    Logger log(1000000);
    tp.set_logger(&log);

    // task flow
    dot_tf.set_mapping([&](int k) {
              return (k % n_threads);
          })
        .set_name([&](int k) {
            return "ddot_" + to_string(k) + "_" + to_string(rank);
        })
        .set_indegree([](int) {
            return 1;
        })
        .set_task([&](int k) {
            const int global_index = rank * n_threads * block_size + k * block_size;
            auto a_init = [&, global_index](int i) {
                return array_initializer(global_index + i);
            };

            VectorXd x = VectorXd::NullaryExpr(block_size, a_init);
            VectorXd y = VectorXd::NullaryExpr(block_size, a_init);

            z[k] = x.dot(y);
        })
        .set_fulfill([&](int k) {
            add_tf.fulfill_promise(0); // same rank
            dlog.add_event(make_unique<DepsEvent>(dot_tf.name(k), add_tf.name(0)));
        });

    add_tf.set_mapping([&](int) {
              return 0;
          })
        .set_name([&](int k) {
            return "add_" + to_string(k) + "_" + to_string(rank);
        })
        .set_indegree([&](int) {
            return n_threads;
        })
        .set_task([&](int) {
            partial_sum = z.sum();
        })
        .set_fulfill([&](int k) {
            am->send(0, partial_sum);
            dlog.add_event(make_unique<DepsEvent>(add_tf.name(k), "Reduce"));
        });

    // Seed tasks
    for (int i = 0; i < n_threads; ++i)
        dot_tf.fulfill_promise(i);

    tp.join();

    if (rank == 0)
    {
        VectorXd x = VectorXd::NullaryExpr(n_ranks * n_threads * block_size, array_initializer);
        VectorXd y = VectorXd::NullaryExpr(n_ranks * n_threads * block_size, array_initializer);

        double ddot_ref = x.dot(y);
        auto err = abs(ddot_sum - ddot_ref);

        if (VERB > 0 && err != 0)
            printf("Error: %g, sum = %g; ref = %g\n", err, ddot_sum, ddot_ref);

        ASSERT_EQ(err, 0);
    }

    // Logging
    std::ofstream logfile;
    string filename = "ddot_" + to_string(rank) + ".log";
    logfile.open(filename);
    logfile << log;
    logfile.close();

    std::ofstream depsfile;
    string dfilename = "ddot_" + to_string(rank) + ".dot";
    depsfile.open(dfilename);
    depsfile << dlog;
    depsfile.close();
}

TEST(ddot, one)
{
    int n_threads = n_threads_;
    int n = n_;
    ddot(n_threads, n);
}

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

    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    if (argc >= 2)
    {
        n_threads_ = atoi(argv[1]);
    }

    if (argc >= 3)
    {
        n_ = atoi(argv[2]);
    }

    if (argc >= 4)
    {
        VERB = atoi(argv[3]);
    }

    if (VERB > 0)
        printf("# threads = %d; block size = %d; VERB = %d\n", n_threads_, n_, VERB);

    const int return_flag = RUN_ALL_TESTS();

    MPI_Finalize();

    return return_flag;
}


#include <stdio.h>
#include <iostream>
#include <math.h>
#include <string>
#include <Eigen/Dense>
#include "ros/ros.h"

#include <yaml-cpp/yaml.h>
#include <ros/package.h>

#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/JointState.h>
#include <robot_localization_msgs/HeadTransform.h>

using namespace ros;
using namespace std;
using namespace Eigen;

float angle_head_yaw=0.0;
float angle_head_pitch=0.0;

void jointCallback(const sensor_msgs::JointState::ConstPtr& msg);

bool RobotYamlRead(string path, float* length_pelvis2headyaw, float* length_headyaw2headpitch, float* length_headpitch2camera);
bool ParticleYamlRead(string path, float& unit);

Matrix4f makeYawMatrix(float& theta);
Matrix4f makePitchMatrix(float& theta);
Matrix4f makeTransMatrix(Vector3f& position);
Matrix4f makeTransMatrix(Vector3f& position, float& unit);

void insertMatrix2Message(Matrix4f& matrix, std_msgs::Float32MultiArray& msg);
void insertMatrix2Message(Matrix4f& matrix, robot_localization_msgs::HeadTransform& msg, int flag); // flag=0 -> original, flag=1 -> unit

int main(int argc, char** argv)
{
    //read setting
    string yaml_robot_path = package::getPath("robot_localization_data") + "/data/kin_dyn_2.yaml";
    string yaml_particle_path = package::getPath("robot_localization_data") + "/data/particle_setting.yaml";

    float length_pelvis2headyaw[3];
    float length_headyaw2headpitch[3];
    float length_headpitch2camera[3];

    float unit;

    if( !RobotYamlRead(yaml_robot_path, length_pelvis2headyaw, length_headyaw2headpitch, length_headpitch2camera) ) exit(0);
    if( !ParticleYamlRead(yaml_particle_path, unit) ) exit(0);

    Vector3f V_length_pelvis2headyaw(length_pelvis2headyaw[0],length_pelvis2headyaw[1],length_pelvis2headyaw[2]);
    Vector3f V_length_headyaw2headpitch(length_headyaw2headpitch[0],length_headyaw2headpitch[1],length_headyaw2headpitch[2]);
    Vector3f V_length_headpitch2camera(length_headpitch2camera[0], length_headpitch2camera[1], length_headpitch2camera[2]);

    //start ros
    ros::init(argc, argv, "robot_localization_head_receive2");
    ros::NodeHandle n;
    ros::Rate loop_rate(100);

    ros::Publisher headtransform_pub = n.advertise<std_msgs::Float32MultiArray>("/alice/camera_transform",10);
    ros::Publisher custom_headtransform_pub = n.advertise<robot_localization_msgs::HeadTransform>("/robot_localization/head_transform",10);
    ros::Subscriber joint_sub = n.subscribe("/robotis/present_joint_states",10,jointCallback);

    while(ros::ok())
    {
        std_msgs::Float32MultiArray headtransform_msg;
        robot_localization_msgs::HeadTransform custom_headtransform_msg;

        //real world
        Matrix4f t_pelvis2headyaw = makeTransMatrix(V_length_pelvis2headyaw);
        Matrix4f r_pelvis2headyaw = Matrix4f::Identity();

        Matrix4f t_headyaw2headpitch = makeTransMatrix(V_length_headyaw2headpitch);
        Matrix4f r_headyaw2headpitch = makeYawMatrix(angle_head_yaw);

        Matrix4f t_headpitch2camera = makeTransMatrix(V_length_headpitch2camera);
        Matrix4f r_headpitch2camera = makePitchMatrix(angle_head_pitch);

        Matrix4f TR01 = r_pelvis2headyaw * t_pelvis2headyaw;
        Matrix4f TR12 = r_headyaw2headpitch * t_headyaw2headpitch;
        Matrix4f TR23 = r_headpitch2camera * t_headpitch2camera;

        Matrix4f Transform_pelvis2head = TR01 * TR12 * TR23;

        //virtual world
        Matrix4f t_pelvis2headyaw_V = makeTransMatrix(V_length_pelvis2headyaw, unit);
        Matrix4f t_headyaw2headpitch_V = makeTransMatrix(V_length_headyaw2headpitch, unit);
        Matrix4f t_headpitch2camera_V = makeTransMatrix(V_length_headpitch2camera, unit);

        Matrix4f TV01 = r_pelvis2headyaw * t_pelvis2headyaw_V;
        Matrix4f TV12 = r_headyaw2headpitch * t_headyaw2headpitch_V;
        Matrix4f TV23 = r_headpitch2camera * t_headpitch2camera_V;

        Matrix4f unit_Transform_pelvis2head = TV01 * TV12 * TV23;

        insertMatrix2Message(Transform_pelvis2head, headtransform_msg);

        insertMatrix2Message(Transform_pelvis2head, custom_headtransform_msg, 0);
        insertMatrix2Message(unit_Transform_pelvis2head, custom_headtransform_msg, 1);

        headtransform_pub.publish(headtransform_msg);
        custom_headtransform_pub.publish(custom_headtransform_msg);

        cout << "-----------------------------------------------" << endl;
        cout << "head_yaw : " << angle_head_yaw * 180 / M_PI << endl;
        cout << "head_pitch : " << angle_head_pitch * 180 / M_PI << endl;
        cout << "-----------------------------------------------" << endl;

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

void jointCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
    int head_yaw_index, head_pitch_index;
    for(int i=0; i<(msg->name).size(); i++)
    {
        if(msg->name[i] == "head_yaw"   ) head_yaw_index = i;
        if(msg->name[i] == "head_pitch" ) head_pitch_index = i;
    }

    angle_head_yaw = msg->position[head_yaw_index];
    angle_head_pitch = msg->position[head_pitch_index];
}

bool RobotYamlRead(string path, float* length_pelvis2headyaw, float* length_headyaw2headpitch, float* length_headpitch2camera)
{
    YAML::Node yaml_node;
    try
    {
        yaml_node = YAML::LoadFile(path.c_str());
        length_pelvis2headyaw[0]    = yaml_node["head_y"]["relative_position"][0].as<float>();
        length_pelvis2headyaw[1]    = yaml_node["head_y"]["relative_position"][1].as<float>();
        length_pelvis2headyaw[2]    = yaml_node["head_y"]["relative_position"][2].as<float>();

        length_headyaw2headpitch[0] = yaml_node["head_p"]["relative_position"][0].as<float>();
        length_headyaw2headpitch[1] = yaml_node["head_p"]["relative_position"][1].as<float>();
        length_headyaw2headpitch[2] = yaml_node["head_p"]["relative_position"][2].as<float>();

        length_headpitch2camera[0]  = yaml_node["cam"]["relative_position"][0].as<float>();
        length_headpitch2camera[1]  = yaml_node["cam"]["relative_position"][1].as<float>();
        length_headpitch2camera[2]  = yaml_node["cam"]["relative_position"][2].as<float>();
    }
    catch(const exception& e)
    {
        ROS_ERROR("fail to read robot yaml file");
        return false;
    }
    return true;
}

bool ParticleYamlRead(string path, float& unit)
{
    YAML::Node yaml_node;
    try
    {
        yaml_node = YAML::LoadFile(path.c_str());
        unit = yaml_node["ETCSetting"]["unit"].as<float>();
    }
    catch(const exception& e)
    {
        ROS_ERROR("fail to read particle yaml file");
        return false;
    }
    return true;
}

void insertMatrix2Message(Matrix4f& matrix, std_msgs::Float32MultiArray& msg)
{
    for(int i=0; i<4; i++)
        for(int j=0; j<4; j++)
            msg.data.push_back(matrix(i,j));
}

void insertMatrix2Message(Matrix4f& matrix, robot_localization_msgs::HeadTransform& msg, int flag)
{
    switch(flag)
    {
        case 0:
            for(int i=0; i<4; i++)
                for(int j=0; j<4; j++)
                    msg.Tdata.push_back(matrix(i,j));
            break;
        case 1:
            for(int i=0; i<4; i++)
                for(int j=0; j<4; j++)
                    msg.Udata.push_back(matrix(i,j));
            break;
        default:
            break;
    }
}

Matrix4f makeYawMatrix(float& theta)
{
    Matrix4f matrix;
    matrix(0,0)=cos(theta); matrix(0,1)=-sin(theta); matrix(0,2)=0; matrix(0,3)=0;
    matrix(1,0)=sin(theta); matrix(1,1)= cos(theta); matrix(1,2)=0; matrix(1,3)=0;
    matrix(2,0)=0;          matrix(2,1)=0;           matrix(2,2)=1; matrix(2,3)=0;
    matrix(3,0)=0;          matrix(3,1)=0;           matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}

Matrix4f makePitchMatrix(float& theta)
{
    Matrix4f matrix;
    matrix(0,0)= cos(theta); matrix(0,1)=0; matrix(0,2)=sin(theta); matrix(0,3)=0;
    matrix(1,0)=0;           matrix(1,1)=1; matrix(1,2)=0;          matrix(1,3)=0;
    matrix(2,0)=-sin(theta); matrix(2,1)=0; matrix(2,2)=cos(theta); matrix(2,3)=0;
    matrix(3,0)=0;           matrix(3,1)=0; matrix(3,2)=0;          matrix(3,3)=1;

    return matrix;
}

Matrix4f makeTransMatrix(Vector3f& position)
{
    Matrix4f matrix;
    matrix(0,0)=1; matrix(0,1)=0; matrix(0,2)=0; matrix(0,3)=position(0);
    matrix(1,0)=0; matrix(1,1)=1; matrix(1,2)=0; matrix(1,3)=position(1);
    matrix(2,0)=0; matrix(2,1)=0; matrix(2,2)=1; matrix(2,3)=position(2);
    matrix(3,0)=0; matrix(3,1)=0; matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}

Matrix4f makeTransMatrix(Vector3f& position, float& unit)
{
    Matrix4f matrix;
    matrix(0,0)=1; matrix(0,1)=0; matrix(0,2)=0; matrix(0,3)=position(0)*unit;
    matrix(1,0)=0; matrix(1,1)=1; matrix(1,2)=0; matrix(1,3)=position(1)*unit;
    matrix(2,0)=0; matrix(2,1)=0; matrix(2,2)=1; matrix(2,3)=position(2)*unit;
    matrix(3,0)=0; matrix(3,1)=0; matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}


#ifndef __HCINDEXSET__CLASS__
#define __HCINDEXSET__CLASS__

#include "SparseIndexSet.hpp"

#include <Eigen/Dense>

/** \brief Hyperbolic cross is implemented as a simple addon for the
*          sparseIndexSet class which provides the corresponding comparison
*          functor. Weights my be unsorted in HC case and no sorting is
*          performed.
*/
class HCindexSet : public SparseIndexSet {
 public:
  void computeIndexSet(int q, const Eigen::VectorXd &w) {
    SparseIndexSet::computeIndexSet(q, w.size(), HCindexSet::cpFun(w));
  };

 protected:
  struct cpFun {
    Eigen::VectorXd _w;
    cpFun(const Eigen::VectorXd &w) : _w(w){};
    double operator()(const Eigen::VectorXi &alpha) const {
      double skap = 1;
      for (int i = 0; i < _w.size(); ++i)
        skap *= std::pow(alpha(i) + 1., _w(i));
      return skap - 1.;
    }
  };
};
#endif


#pragma once

#include "MerweScaledSigmaPoints.hpp"
#include "GaussianDistribution.hpp"

#include <iostream>
#include <Eigen/Dense>

namespace icarus
{
    template<typename T, size_t N>
    struct UnscentedKalmanFilter
    {
        explicit UnscentedKalmanFilter() :
            mSigmaPoints(0.5f)
        {
            reset();
        }

        template<typename ProcessModel, typename MeasurementModel, size_t S>
        void filter(ProcessModel const & processModel, MeasurementModel const & measurementModel, GaussianDistribution<T, S> const & measurement, T timeStep)
        {
            auto points = mSigmaPoints(mState);

            for (auto & point : points) {
                point = processModel(point, timeStep);
            }

            mState = mSigmaPoints.unscentedTransform(points);
            mState.covariance.template triangularView<Eigen::Lower>() += processModel.noise();

            std::array<Eigen::Matrix<T, S, 1>, 2 * N + 1> measurementPoints;

            for (int i = 0; i < 2 * N + 1; ++i) {
                measurementPoints[i] = measurementModel(points[i]);
            }

            auto measurementDistribution = mSigmaPoints.unscentedTransform(measurementPoints);
            measurementDistribution.covariance.template triangularView<Eigen::Lower>() += measurement.covariance;
            measurementDistribution.covariance.template triangularView<Eigen::Upper>() = measurementDistribution.covariance.transpose();

            Eigen::Matrix<T, N, S> gain;
            gain.setZero();

            for (int i = 0; i < mSigmaPoints.size(); ++i) {
                auto weight = mSigmaPoints.covarianceWeight(i);
                auto stateDifference = points[i] - mState.mean;
                auto measurementDifference = measurementPoints[i] - measurementDistribution.mean;

                gain += weight * stateDifference * measurementDifference.transpose();
            }

            gain *= measurementDistribution.covariance.inverse();

            mState.mean += gain * (measurement.mean - measurementDistribution.mean);
            mState.covariance.template triangularView<Eigen::Lower>() -= gain * measurementDistribution.covariance * gain.transpose();
        }

        void reset()
        {
            mState.mean.setZero();
            mState.covariance.setZero();
        }

        Eigen::Matrix<T, N, 1> & stateVector()
        {
            return mState.mean;
        }
    private:
        MerweScaledSigmaPoints<T, N> mSigmaPoints;
        GaussianDistribution<T, N> mState;
    };
}


//
//  ambisonicDecoder.h
//  AmbisonicDecoder
//
//  Created by David Poirier-Quinot on 21/06/2017.
//  Copyright © 2017 ICL. All rights reserved.
//

#ifndef ambisonicDecoder_hpp
#define ambisonicDecoder_hpp

#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigenvalues>

double deg2rad( const double degrees ){
    return degrees * 4.0 * atan (1.0) / 180.0;
}

unsigned int getNumAmbiCh( const unsigned int order )
{
    return pow(order + 1, 2);
}

class AmbisonicDecoder
{

//==========================================================================
// ATTRIBUTES

public:

private:

//==========================================================================
// METHODS

public:

    AmbisonicDecoder() {}

    ~AmbisonicDecoder() {}

    Eigen::MatrixXf getDecodingMatrix( const Eigen::MatrixXf & spkAzimElev, const unsigned int order, const bool useEpad )
    {
        // init
        unsigned long numSpk = spkAzimElev.cols();
        unsigned int numCh = getNumAmbiCh( order );
        Eigen::MatrixXf ambiGains ( numSpk, numCh ) ;
        Eigen::VectorXf gains( numCh );
        float azim; float elev;

        // loop over speaker positions
        for( int i = 0; i < numSpk; i++ ){
            // get spk azim elev
            azim = spkAzimElev(0, i);
            elev = spkAzimElev(1, i);

            // get spherical harmonic coefficients
            getRSH( order, azim, elev, gains );

            // fill output
            for( int j = 0; j < numCh; j++ ){
                ambiGains(i,j) = gains[j];
            }
        }

        if( useEpad ){
            // singular value decomposition
            Eigen::JacobiSVD<Eigen::MatrixXf> svd(ambiGains.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);
            // get ambiGains out of left / right matrices
            ambiGains = svd.matrixV() * svd.matrixU().transpose();
        }

        // normalization (only step required for 'SAD')
        float norm = 4 * M_PI / numSpk;
        ambiGains *= norm;

        return ambiGains;
    }

    // get real spherical harmonics
    void getRSH( const unsigned int n, const float azim, const float elev, Eigen::VectorXf & gains )
    {
        // init
        float r;
        float ri;

        // convert from polarch coord. system to boost's
        float theta = deg2rad( 90 - elev );
        float phi = deg2rad( azim );

        // order 0
        gains[0] = 1.0 / sqrt(4*M_PI);

        // loop over spherical harmonic indices
        int index = 1;
        for( int nn = 1; nn <= n; nn += 1){
            for( int m = -nn; m <= abs(nn); m += 1){
                r = boost::math::spherical_harmonic_r(nn, m, theta, phi);
                ri = boost::math::spherical_harmonic_i(nn, m, theta, phi);

                if( m != 0 ){ r = pow(-1, m) * sqrt(2) * r; }
                ri = - sqrt(2) * ri;

                if( m < 0 ){ gains[index] = ri; }
                else{ gains[index] = r; }
                index++;
            }
        }
    }

};

#endif /* ambisonicDecoder_hpp */


#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include <string>
#include <fstream>
#include <istream>
#include <random>
#include <cmath>
#include <Eigen/Dense>
#include <boost/program_options.hpp>
#include "options.hpp"
#include "options_parser.hpp"
#include "LagrangianState.h"
#include "ParticlePhysics.h"
#include "TimeIntegration.h"
#include "Inputfile.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char* argv[]) {
  printf("*********** DRIVER PROGRAM FOR LAGRANGIAN PARTICLE SOLVER ***********\n\n");

  // Parse input file options
  Options options;
  if (!parseOptions(argc,argv,options)){
   return 0;
  }
  cout << options << endl;

  // Load state
  MatrixXd input = load_csv<MatrixXd>(options.inputfile);

  // Pass parsed program options to simulation
  LagrangianState state(input);
  ParticlePhysics physics(options, state);
  TimeIntegration integrator(options, physics, state);

  // Solve
  integrator.euler();

  // Output
  state.writeXY(options.outputfile+"_final.csv");

  return 0;
}


// Test different solvers on mtx file
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <Eigen/Sparse>
#include <unsupported/Eigen/SparseExtra>

#ifdef KLU_SUPPORT
#include "../eigen_klu.h"
#endif

#ifdef EIGEN_UMFPACK_SUPPORT
#include "UmfPackSupport.h"
#endif

#include <chrono>

template<class Solver>
void testSolver(const Eigen::SparseMatrix<double>& A, const Eigen::VectorXd& b)
{
	std::cout << "\n\nSolving with solver " << typeid(Solver).name() << "\n";

	Solver solver;

	auto t0 = std::chrono::high_resolution_clock::now();

	solver.compute(A);
	auto x = solver.solve(b);

	auto t1 = std::chrono::high_resolution_clock::now();

	std::cout << "... took " << std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count() << " ms.\n";

	Eigen::VectorXd residual = A*x - b;

	printf("residual norm: %e\n", residual.norm());
}

int main(int argc, char** argv)
{
	if(argc <= 1)
	{
		fprintf(stderr, "Usage: solve_test <prefix>\n");
	}

	Eigen::SparseMatrix<double> A;
	Eigen::VectorXd b;

	A.makeCompressed();

	std::string prefix = argv[1];
	Eigen::loadMarket(A, prefix + ".mtx");
	Eigen::loadMarketVector(b, prefix + "_b.mtx");

#ifdef KLU_SUPPORT
	testSolver<Eigen::KLU<Eigen::SparseMatrix<double>>>(A, b);
#endif
#ifdef EIGEN_UMFPACK_SUPPORT
	testSolver<Eigen::UmfPackLU<Eigen::SparseMatrix<double>>>(A, b);
#endif

	return 0;
}


#include <test_common.h>
#include <Eigen/Dense>

#include <igl/copyleft/cgal/remesh_self_intersections.h>
#include <igl/copyleft/cgal/RemeshSelfIntersectionsParam.h>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

TEST_CASE("RemeshSelfIntersections: CubeWithFold", "[igl/copyleft/cgal]")
{
    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    igl::read_triangle_mesh(test_common::data_path("cube_with_fold.ply"), V, F);

    typedef CGAL::Exact_predicates_exact_constructions_kernel K;
    typedef Eigen::Matrix<K::FT, Eigen::Dynamic, Eigen::Dynamic> MatrixXe;

    MatrixXe VV;
    Eigen::MatrixXi FF, IF;
    Eigen::VectorXi J, IM;
    igl::copyleft::cgal::RemeshSelfIntersectionsParam param;
    igl::copyleft::cgal::remesh_self_intersections(V, F, param, VV, FF, IF, J, IM);
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_CENTERIZE_HPP
#define MCL_CENTERIZE_HPP 1

#include <Eigen/Core>
#include <limits>

namespace mcl
{

// Moves all of the vertices so that the center of the mesh
// is at the origin. Returns translation used.
template <typename DerivedV>
static inline void centerize(Eigen::MatrixBase<DerivedV> &V)
{
	int cols = V.cols();
	for (int i=0; i<cols; ++i)
	{
		typename DerivedV::Scalar ci = V.col(i).mean();
		V.col(i).array() -= ci;
	}
} // end centerize

// Returns the index of the center-most vertex
template <typename DerivedV>
static inline int get_center_index(const Eigen::MatrixBase<DerivedV> &V)
{
	typedef typename DerivedV::Scalar T;
	typedef Eigen::Matrix<T,3,1> Vec3t;
	int cols = std::min(3, int(V.cols()));

	Vec3t center = Vec3t::Zero();
	for (int i=0; i<cols; ++i)
		center[i] = V.col(i).mean();

	int min_idx = -1;
	T min_dist = std::numeric_limits<T>::max();

	int nv = V.rows();
	for (int i=0; i<nv; ++i)
	{
		Vec3t vi = Vec3t::Zero();
		for (int j=0; j<cols; ++j)
			vi[j] = V(i,j);

		T dist = (center-vi).norm();
		if (dist < min_dist)
		{
			min_dist = dist;
			min_idx = i;
		}
	}

	return min_idx;
}

// Scales all of the vertices in V to a target radius.
// Returns the (uniform) scaling used.
template <typename DerivedV>
inline double scale_to_sphere(Eigen::MatrixBase<DerivedV> &V, double radius)
{
	using namespace Eigen;
	centerize(V);
	int dim = V.cols();
	int nv = V.rows();
	if (nv == 0 || dim < 2 || dim > 3)
		return 1.0;

	auto get_v3 = [&](int idx)
	{
		Vector3d v = Vector3d::Zero();
		for(int i=0; i<dim; ++i)
			v[i]=V(idx,i);
		return v;
	};

	double rad = 1e-20;
	for (int i=0; i<nv; ++i)
	{
		Vector3d v = get_v3(i);
		double d = v.norm();
		if (d > rad)
			rad = d;
	}

	double scale = radius / rad;
	V *= scale;
	return scale;

} // end scale to sphere

} // end ns mcl

#endif


//==============================================================================
//         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_TOOLBOX_EXPONENTIAL_CONSTANTS_LOG_2_HPP_INCLUDED
#define NT2_TOOLBOX_EXPONENTIAL_CONSTANTS_LOG_2_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup expon_constant
 * \defgroup expon_constant_log_2 Log_2
 *
 * \par Description
 * Constant log_2 : \f$\log(2)\f$ constant.
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/log_2.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::log_2_(A0)>::type
 *     log_2();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Log_2
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Log_2, double
                                , 0, 0x3f317218
                                , 0x3fe62e42fefa39efLL
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Log_2, Log_2);
}

#endif


#pragma once

#include <map>
#include <set>
#include <utility>
#include <vector>

#include <boost/serialization/utility.hpp>
#include <boost/serialization/set.hpp>

#include <gmpxx.h>

struct eval_mat {
  friend class boost::serialization::access;

  std::set<std::pair<std::pair<size_t, size_t>, int>> values;

  template<class Archive>
  void serialize (Archive & ar, unsigned int const);

  void save (std::string const & filename) const;

  void load (std::string const & filename);

  std::set<size_t> row_set () const;

};

std::set<size_t> findDependentVariables (std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> const & matrix, size_t rows, size_t cols);

std::map<size_t, std::map<size_t, mpq_class>> solveLinearSystem (std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> const & matrix, size_t rows, size_t cols);


#include <Eigen/Dense>
#include <iostream>

int main() {
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(3, 2);
    std::cout << "A =" << std::endl << A << std::endl;
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(2, 3);
    std::cout << "B =" << std::endl << B << std::endl;
    Eigen::MatrixXd C = A*B;
    std::cout << "C =" << std::endl << C << std::endl;
    return 0;
}


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE DenseMIAFunctionTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#include "DenseMIA.h"
#include "Index.h"

template<class _data_type>
void functions_work(){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;

//    size_t dim1=10;
//    size_t dim2=11;
//    size_t dim3=20;
//    size_t dim4=15;
    size_t dim1=2;
    size_t dim2=3;
    size_t dim3=4;
    size_t dim4=15;

    LibMIA::DenseMIA<_data_type,2> e(dim1,dim2);
    LibMIA::DenseMIA<_data_type,2> e2(dim1,dim2);
    LibMIA::DenseMIA<_data_type,2> f;
    LibMIA::DenseMIA<_data_type,3> a(dim1,dim2,dim3);
    LibMIA::DenseMIA<_data_type,3> a2(dim1,dim2,dim3);
    LibMIA::DenseMIA<_data_type,3> b;
    LibMIA::DenseMIA<_data_type,4> c(dim1,dim2,dim3,dim4);
    LibMIA::DenseMIA<_data_type,4> c2(dim1,dim2,dim3,dim4);
    LibMIA::DenseMIA<_data_type,4> d;

    //****************In place permutation
    e.randu(-50,50);
    e2=e;
    f(j,i)=e(i,j);
    e2.inplace_permute(1,0);
    //e.print();
    //f.print();
    //e2.print();
    BOOST_CHECK_MESSAGE(f==e2,std::string("Second-order in place permutation 1 for ")+typeid(_data_type).name());

    a.randu(-50,50);
    //a.print();
    a2=a;

    b(j,k,i)=a(i,j,k);

    a2.inplace_permute(1,2,0);

    //b.print();
    //a2.print();
    BOOST_CHECK_MESSAGE(b==a2,std::string("Third-order in place permutation 1 for ")+typeid(_data_type).name());
    a2=a;
    b(k,i,j)=a(i,j,k);
    a2.inplace_permute(2,0,1);
    BOOST_CHECK_MESSAGE(b==a2,std::string("Third-order in place permutation 2 for ")+typeid(_data_type).name());

    c.randu(-50,50);
    c2=c;
    d(k,l,i,j)=c(i,j,k,l);
    c2.inplace_permute(2,3,0,1);
    BOOST_CHECK_MESSAGE(d==c2,std::string("Fourth-order in place permutation 1 for ")+typeid(_data_type).name());

    c2=c;
    d(j,l,i,k)=c(i,j,k,l);
    c2.inplace_permute(1,3,0,2);
    BOOST_CHECK_MESSAGE(d==c2,std::string("Fourth-order in place permutation 2 for ")+typeid(_data_type).name());

    auto lat1=d.toLatticeCopy(std::array<size_t, 1>{{3}},std::array<size_t, 2>{{2,1}},std::array<size_t, 1>{{0}});
    auto lat2=d.toLatticePermute(std::array<size_t, 1>{{3}},std::array<size_t, 2>{{2,1}},std::array<size_t, 1>{{0}});
    BOOST_CHECK_MESSAGE(lat1==lat2,std::string("Lattice Mapping test for ")+typeid(_data_type).name());

}

BOOST_AUTO_TEST_CASE( DenseMIAFunctionTests )
{

    functions_work<double>();

    functions_work<float>();
    functions_work<int>();
    functions_work<long>();

}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 */

// @sect3{Include files}

// 前面几个文件已经在前面的例子中讲过了，因此不再做进一步的评论。

#include <deal.II/base/quadrature_lib.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>

// 从下面的include文件中我们将导入H1-conforming有限元形状函数的声明。这个有限元系列被称为  FE_Q  ，在之前的所有例子中已经被用来定义通常的双线性或三线性元素，但我们现在将用它来定义双二次元元素。

#include <deal.II/fe/fe_q.h>

// 我们不会像前面的例子那样从文件中读取网格，而是使用库中的一个函数生成网格。然而，我们将希望在每一步中写出局部细化的网格（只是网格，而不是解决方案），所以我们需要以下的include文件，而不是 grid_in.h  。

#include <deal.II/grid/grid_out.h>

// 当使用局部细化网格时，我们会得到所谓的悬空节点。然而，标准的有限元方法假定离散的解空间是连续的，所以我们需要确保悬挂节点上的自由度符合一些约束条件，这样全局解是连续的。我们也要在这个对象中存储边界条件。下面的文件包含一个用来处理这些约束条件的类。

#include <deal.II/lac/affine_constraints.h>

// 为了在本地细化我们的网格，我们需要一个来自库的函数，根据我们计算的误差指标来决定哪些单元需要细化或粗化。这个函数被定义在这里。

#include <deal.II/grid/grid_refinement.h>

// 最后，我们需要一个简单的方法来实际计算基于某种误差估计的细化指标。虽然一般来说，适应性是非常具体的问题，但以下文件中的误差指标通常会对一大类问题产生相当好的适应网格。

#include <deal.II/numerics/error_estimator.h>

// 最后，这和以前的程序一样。

using namespace dealii;
// @sect3{The Step6 class template}

// 主类又是几乎没有变化的。然而，我们增加了两项内容：我们增加了 refine_grid 函数，该函数用于自适应地细化网格（而不是之前例子中的全局细化），还有一个变量，它将保存约束条件。

template <int dim>
class Step6
{
public:
  Step6();

  void run();

private:
  void setup_system();
  void assemble_system();
  void solve();
  void refine_grid();
  void output_results(const unsigned int cycle) const;

  Triangulation<dim> triangulation;

  FE_Q<dim>       fe;
  DoFHandler<dim> dof_handler;

// 这是主类中的新变量。我们需要一个对象，它持有一个约束条件的列表，以保持悬挂节点和边界条件。

  AffineConstraints<double> constraints;

  SparseMatrix<double> system_matrix;
  SparsityPattern      sparsity_pattern;

  Vector<double> solution;
  Vector<double> system_rhs;
};
// @sect3{Nonconstant coefficients}

//非恒定系数的实现是逐字复制自  step-5  。

template <int dim>
double coefficient(const Point<dim> &p)
{
  if (p.square() < 0.5 * 0.5)
    return 20;
  else
    return 1;
}

//  @sect3{The Step6 class implementation}
// @sect4{Step6::Step6}

// 这个类的构造函数与之前的基本相同，但这一次我们要使用二次元。为此，我们只需用所需的多项式度数（这里是 2 ）替换构造函数参数（在之前的所有例子中是 1 ）。

template <int dim>
Step6<dim>::Step6()
  : fe(2)
  , dof_handler(triangulation)
{}

//  @sect4{Step6::setup_system}

// 下一个函数设置了所有描述线性有限元问题的变量，如DoFHandler、矩阵和向量。与我们在 step-5 中所做的不同的是，我们现在还必须处理悬挂节点约束。这些约束几乎完全由库来处理，也就是说，你只需要知道它们的存在以及如何获得它们，但你不需要知道它们是如何形成的，也不需要知道对它们到底做了什么。

// 在函数的开头，你会发现所有与 step-5 中相同的东西：设置自由度（这次我们有二次元，但从用户代码的角度看与线性--或任何其他程度的情况没有区别），生成稀疏模式，并初始化解和右手向量。请注意，现在每行的稀疏模式将有更多的条目，因为现在每个单元有9个自由度（而不是只有4个），它们可以相互耦合。

template <int dim>
void Step6<dim>::setup_system()
{
  dof_handler.distribute_dofs(fe);

  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());

// 我们现在可以用悬挂节点的约束来填充AffineConstraints对象。由于我们将在一个循环中调用这个函数，所以我们首先清除上一个系统中的当前约束集，然后计算新的约束。

  constraints.clear();
  DoFTools::make_hanging_node_constraints(dof_handler, constraints);

// 现在我们准备用指标0（整个边界）来插值边界值，并将得到的约束存储在我们的 constraints 对象中。请注意，我们并不像在前面的步骤中那样，在装配后应用边界条件：相反，我们将所有的约束条件放在AffineConstraints对象中的我们的函数空间。我们可以按任何顺序向AffineConstraints对象添加约束：如果两个约束发生冲突，那么约束矩阵要么中止，要么通过Assert宏抛出一个异常。

  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           Functions::ZeroFunction<dim>(),
                                           constraints);

// 在所有约束条件被添加之后，需要对它们进行排序和重新排列，以便更有效地执行一些操作。这种后处理是用 close() 函数完成的，之后就不能再添加任何约束了。

  constraints.close();

// 现在我们首先建立我们的压缩稀疏模式，就像我们在前面的例子中做的那样。然而，我们并没有立即将其复制到最终的稀疏度模式中。 请注意，我们调用了make_sparsity_pattern的一个变体，它把AffineConstraints对象作为第三个参数。我们通过将参数 keep_constrained_dofs 设置为false（换句话说，我们永远不会写入矩阵中对应于受限自由度的条目），让该例程知道我们永远不会写入 constraints 所给的位置。如果我们在装配后对约束进行压缩，我们就必须通过 true 来代替，因为这样我们就会先写进这些位置，然后在压缩过程中再将它们设置为零。

  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler,
                                  dsp,
                                  constraints,
                                  /*keep_constrained_dofs =  */ false);

// 现在，矩阵的所有非零条目都是已知的（即那些来自定期组装矩阵的条目和那些通过消除约束引入的条目）。我们可以将我们的中间对象复制到稀疏模式中。

  sparsity_pattern.copy_from(dsp);

// 我们现在可以，最后，初始化稀疏矩阵。

  system_matrix.reinit(sparsity_pattern);
}
// @sect4{Step6::assemble_system}

// 接下来，我们要对矩阵进行组装。然而，为了将每个单元上的本地矩阵和向量复制到全局系统中，我们不再使用手写的循环。相反，我们使用 AffineConstraints::distribute_local_to_global() ，在内部执行这个循环，同时对对应于受限自由度的行和列进行高斯消除。

// 构成局部贡献的其余代码保持不变。然而，值得注意的是，在引擎盖下，有几件事与以前不同。首先，变量 dofs_per_cell 和返回值 quadrature_formula.size() 现在各为9，以前是4。引入这样的变量作为缩写是一个很好的策略，可以使代码在不同的元素下工作，而不需要改变太多的代码。其次， fe_values 对象当然也需要做其他事情，因为现在的形状函数是二次的，而不是线性的，在每个坐标变量中。不过，这也是完全由库来处理的事情。

template <int dim>
void Step6<dim>::assemble_system()
{
  const QGauss<dim> quadrature_formula(fe.degree + 1);

  FEValues<dim> fe_values(fe,
                          quadrature_formula,
                          update_values | update_gradients |
                            update_quadrature_points | update_JxW_values);

  const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      cell_matrix = 0;
      cell_rhs    = 0;

      fe_values.reinit(cell);

      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {
          const double current_coefficient =
            coefficient(fe_values.quadrature_point(q_index));
          for (const unsigned int i : fe_values.dof_indices())
            {
              for (const unsigned int j : fe_values.dof_indices())
                cell_matrix(i, j) +=
                  (current_coefficient *              // a(x_q)
                   fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                   fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                   fe_values.JxW(q_index));           // dx

              cell_rhs(i) += (1.0 *                               // f(x)
                              fe_values.shape_value(i, q_index) * // phi_i(x_q)
                              fe_values.JxW(q_index));            // dx
            }
        }

// 最后，将 @p cell_matrix 和 @p cell_rhs 中的贡献转移到全局对象中。

      cell->get_dof_indices(local_dof_indices);
      constraints.distribute_local_to_global(
        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
    }

// 现在我们已经完成了线性系统的组装。约束矩阵处理了应用边界条件的问题，也消除了悬挂的节点约束。受约束的节点仍然在线性系统中（在矩阵的对角线上有一个非零条目，选择的方式是使矩阵具有良好的条件，并且这一行的所有其他条目都被设置为零），但是计算出来的值是无效的（也就是说， system_rhs 中的相应条目目前是没有意义的）。我们在 solve 函数的最后为这些节点计算出正确的值。

}
// @sect4{Step6::solve}

// 我们继续逐步改进。解决线性系统的函数再次使用了SSOR预处理程序，除了我们必须加入悬空节点约束外，其他的都没有改变。如上所述，通过对矩阵的行和列进行特殊处理，从AffineConstraints对象中删除了对应于悬挂节点约束和边界值的自由度。这样一来，这些自由度的值在求解线性系统后就有了错误的、但定义明确的值。然后我们要做的就是利用约束条件给它们分配它们应该有的值。这个过程被称为 distributing 约束，从无约束的节点的值中计算出约束节点的值，只需要一个额外的函数调用，你可以在这个函数的末尾找到。

template <int dim>
void Step6<dim>::solve()
{
  SolverControl            solver_control(1000, 1e-12);
  SolverCG<Vector<double>> solver(solver_control);

  PreconditionSSOR<SparseMatrix<double>> preconditioner;
  preconditioner.initialize(system_matrix, 1.2);

  solver.solve(system_matrix, solution, system_rhs, preconditioner);

  constraints.distribute(solution);
}
// @sect4{Step6::refine_grid}

// 我们使用一个复杂的误差估计方案来细化网格，而不是全局细化。我们将使用KellyErrorEstimator类，该类实现了拉普拉斯方程的误差估计器；原则上它可以处理可变系数，但我们不会使用这些高级功能，而是使用其最简单的形式，因为我们对定量结果不感兴趣，只对生成局部细化网格的快速方法感兴趣。

// 尽管Kelly等人得出的误差估计器最初是为拉普拉斯方程开发的，但我们发现它也很适合于为一类广泛的问题快速生成局部细化网格。这个误差估计器使用了解梯度在单元面上的跳跃（这是一个测量二阶导数的方法），并将其按单元的大小进行缩放。因此，它是对每个单元的解的局部平滑性的测量，因此可以理解，它对双曲运输问题或波浪方程也能产生合理的网格，尽管这些网格与专门针对该问题的方法相比肯定是次优的。因此，这个误差估计器可以理解为测试自适应程序的一种快速方法。

// 估算器的工作方式是将描述自由度的 DoFHandler 对象和每个自由度的数值向量作为输入，为三角剖分的每个活动单元计算一个指标值（即每个活动单元一个数值）。为此，它需要两个额外的信息：一个面部正交公式，即 dim-1 维物体上的正交公式。我们再次使用3点高斯法则，这个选择与本程序中的双二次方有限元形状函数是一致和合适的。当然，什么是合适的正交规则取决于对误差估计器评估解场的方式的了解。如上所述，梯度的跳跃在每个面上都是集成的，对于本例中使用的二次元元素来说，这将是每个面上的二次元函数。然而，事实上，它是梯度跳动的平方，正如该类文件中所解释的那样，这是一个二次函数，对于它来说，3点高斯公式就足够了，因为它可以精确地整合5阶以下的多项式。)

// 其次，该函数需要一个边界指示器的列表，用于那些我们施加了 $\partial_n u(\mathbf x) = h(\mathbf x)$ 类诺伊曼值的边界，以及每个此类边界的函数 $h(\mathbf x)$ 。这些信息由一个从边界指标到描述诺伊曼边界值的函数对象的映射来表示。在本例程序中，我们不使用诺伊曼边界值，所以这个映射是空的，实际上是在函数调用期望得到相应函数参数的地方使用映射的默认构造器构造的。

// 输出是一个所有活动单元的值的向量。虽然非常精确地计算一个解的自由度的value可能是有意义的，但通常没有必要特别精确地计算一个单元上的解对应的error indicator。因此，我们通常使用一个浮点数的向量而不是一个双数的向量来表示误差指标。

template <int dim>
void Step6<dim>::refine_grid()
{
  Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

  KellyErrorEstimator<dim>::estimate(dof_handler,
                                     QGauss<dim - 1>(fe.degree + 1),
                                     {},
                                     solution,
                                     estimated_error_per_cell);

// 上述函数为 estimated_error_per_cell 数组中的每个单元格返回一个错误指标值。现在的细化工作如下：细化那些误差值最高的30%的单元，粗化那些误差值最低的3%的单元。

// 人们可以很容易地验证，如果第二个数字为零，这大约会导致在两个空间维度上的每一步的细胞翻倍，因为对于每一个30%的细胞，四个新的将被替换，而其余70%的细胞保持不动。在实践中，通常会产生一些更多的单元，因为不允许一个单元被精炼两次而相邻的单元没有被精炼；在这种情况下，相邻的单元也会被精炼。

// 在许多应用中，被粗化的单元格数量将被设置为大于3%的数值。一个非零的值是很有用的，特别是当初始（粗）网格由于某种原因已经相当精细时。在这种情况下，可能有必要在某些区域进行细化，而在另一些区域进行粗化是有用的。在我们这里，初始网格是非常粗的，所以粗化只需要在一些可能发生过度细化的区域。因此，一个小的、非零的值在这里是合适的。

// 下面的函数现在接受这些细化指标，并使用上述方法对三角形的一些单元进行细化或粗化标记。它来自一个实现了几种不同算法的类，可以根据单元的误差指标来细化三角形。

  GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                  estimated_error_per_cell,
                                                  0.3,
                                                  0.03);

// 在前一个函数退出后，一些单元被标记为细化，另一些单元被标记为粗化。然而，细化或粗化本身并没有被执行，因为有些情况下，进一步修改这些标志是有用的。在这里，我们不想做任何这样的事情，所以我们可以告诉三角计算执行单元格被标记的动作。

  triangulation.execute_coarsening_and_refinement();
}
// @sect4{Step6::output_results}

// 在每个网格的计算结束后，在我们继续下一个网格细化周期之前，我们要输出这个周期的结果。

// 我们已经在 step-1 中看到了如何实现对网格本身的输出。在这里，我们改变一些东西。

//
  我们使用两种不同的格式。gnuplot和VTU。

//
  我们在输出文件名中嵌入了周期号。

//
  对于gnuplot输出，我们设置了一个 GridOutFlags::Gnuplot 对象，以提供一些额外的可视化参数，使边缘看起来是弯曲的。这在  step-10  中有进一步的详细解释。

//

template <int dim>
void Step6<dim>::output_results(const unsigned int cycle) const
{
  {
    GridOut               grid_out;
    std::ofstream         output("grid-" + std::to_string(cycle) + ".gnuplot");
    GridOutFlags::Gnuplot gnuplot_flags(false, 5);
    grid_out.set_flags(gnuplot_flags);
    MappingQGeneric<dim> mapping(3);
    grid_out.write_gnuplot(triangulation, output, &mapping);
  }

  {
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "solution");
    data_out.build_patches();

    std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
    data_out.write_vtu(output);
  }
}
// @sect4{Step6::run}

//  main() 之前的最后一个函数又是该类的主要驱动，  run()  。它与  step-5  的函数类似，只是我们在程序中再次生成一个文件，而不是从磁盘中读取，我们自适应地而不是全局地细化网格，并且我们在本函数中输出最终网格上的解决方案。

// 该函数主循环的第一个块是处理网格生成。如果这是该程序的第一个循环，我们现在不是像上一个例子那样从磁盘上的文件中读取网格，而是再次使用库函数来创建它。域还是一个圆，中心在原点，半径为1（这是函数的两个隐藏参数，有默认值）。

// 你会注意到粗略的网格比我们在前面的例子中从文件中读出的网格质量要差：单元格的形成不太平均。然而，使用库函数，这个程序在任何空间维度上都可以工作，而以前不是这样的。

// 如果我们发现这不是第一个周期，我们要细化网格。与上一个例子程序中采用的全局细化不同，我们现在使用上述的自适应程序。

// 循环的其余部分看起来和以前一样。

template <int dim>
void Step6<dim>::run()
{
  for (unsigned int cycle = 0; cycle < 8; ++cycle)
    {
      std::cout << "Cycle " << cycle << ':' << std::endl;

      if (cycle == 0)
        {
          GridGenerator::hyper_ball(triangulation);
          triangulation.refine_global(1);
        }
      else
        refine_grid();

      std::cout << "   Number of active cells:       "
                << triangulation.n_active_cells() << std::endl;

      setup_system();

      std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                << std::endl;

      assemble_system();
      solve();
      output_results(cycle);
    }
}
// @sect3{The main function}

// 主函数的功能与之前的例子相比没有改变，但我们采取了额外的谨慎措施。有时，会出现一些问题（比如写输出文件时磁盘空间不足，试图分配向量或矩阵时内存不足，或者由于某种原因我们无法从文件中读取或写入文件），在这些情况下，库会抛出异常。由于这些是运行时的问题，而不是可以一劳永逸的编程错误，这种异常在优化模式下不会被关闭，与我们用来测试编程错误的 Assert 宏相反。如果没有被捕获，这些异常会传播到 main 函数的调用树上，如果它们在那里也没有被捕获，程序就会被中止。在很多情况下，比如内存或磁盘空间不足，我们什么也做不了，但我们至少可以打印一些文字，试图解释程序失败的原因。下面显示了一种方法。以这种方式编写任何较大的程序当然是有用的，你可以通过或多或少地复制这个函数来做到这一点，但 try 块除外，它实际上编码了本应用程序所特有的功能。

int main()
{

// 这个函数布局的总体思路如下：让我们试着像以前那样运行程序......

  try
    {
      Step6<2> laplace_problem_2d;
      laplace_problem_2d.run();
    }

// ......如果这应该是失败的，尽量收集尽可能多的信息。具体来说，如果被抛出的异常是一个从C++标准类派生出来的对象  exception, then we can use the what  成员函数，以获得一个描述异常被抛出原因的字符串。

// deal.II的异常类都是从标准类派生出来的，特别是 exc.what() 函数将返回与使用 Assert 宏抛出的异常所产生的字符串大致相同。在前面的例子中，你已经看到了这种异常的输出，然后你知道它包含了异常发生的文件和行号，以及其他一些信息。这也是下面的语句会打印的内容。

// 除此以外，除了用错误代码退出程序（这就是 return 1; 的作用），我们能做的并不多。

  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }

// 如果在某处抛出的异常不是从标准 exception 类派生出来的对象，那么我们根本无法做任何事情。那么我们就简单地打印一个错误信息并退出。

  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

// 如果我们走到这一步，就没有任何异常传播到主函数上（可能有异常，但它们在程序或库的某个地方被捕获）。因此，程序按预期执行，我们可以无误返回。

  return 0;
}


/**
 * @file radauthreetimestepping_main.cc
 * @brief NPDE homework RadauThreeTimestepping
 * @author Erick Schulz
 * @date 08/04/2019
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <iostream>
#include <memory>

#include "radauthreetimestepping.h"
#include "radauthreetimesteppingode.h"

using namespace RadauThreeTimestepping;

int main(int /*argc*/, char ** /*argv*/) {
  /* Solving the ODE problem */
  // This function prints to the terminal the convergence rates and average rate
  // of a convergence study performed for the ODE (d/dt)y = -y.
  testConvergenceTwoStageRadauLinScalODE();

  /* Solving the parabolic heat equation */
  // Create a Lehrfem++ square tensor product mesh
  lf::mesh::utils::TPTriagMeshBuilder builder(
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));
  // Set mesh parameters following the Builder pattern
  // Domain is the unit square

  builder.setBottomLeftCorner(Eigen::Vector2d{-1.0, -1.0})
      .setTopRightCorner(Eigen::Vector2d{1, 1})
      .setNumXCells(50)
      .setNumYCells(50);
  auto mesh_p = builder.Build();

  /* SAM_LISTING_BEGIN_1 */
  //====================
  // Your code goes here
  // augment the mian function that it calls the function solveHeatEvolution() from subproblem k
  // with m =50 and outputs the final temperature

  // finite element space:
  auto fe_space = std::make_shared<lf::uscalfe::FeSpacelangrangeO1<double>>(mesh_p);

  // obtain local-> global index mapping for the current finite elemet space
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};
  const lf::uscalfe::size_type N_dof(dofh.NumDofs());

  int m = 50;
  double final_time = 1.0;

  Eigen::VectorXd discrete_sol = solveHeatEvolution(dofh, m, final_time);

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

  return 0;
}


/*
 * math.hpp
 *
 *  Created on: Apr 30, 2021
 *      Author: jelavice
 */

#pragma once
#include <string>
#include <Eigen/Dense>
#include "icp_localization/common/time.hpp"

namespace icp_loco {

// Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF
// specification http://wiki.ros.org/urdf/XML/joint.
Eigen::Quaterniond fromRPY(double roll, double pitch, double yaw);
Eigen::Vector3d toRPY(const Eigen::Quaterniond &q);
Eigen::Quaterniond fromRPY(const Eigen::Vector3d &rpy);

template<typename T>
inline T getRollFromQuat(T w, T x, T y, T z)
{
  return std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
}

template<typename T>
inline T getPitchFromQuat(T w, T x, T y, T z)
{
  return std::asin(2 * (w * y - x * z));
}

template<typename T>
inline T getYawFromQuat(T w, T x, T y, T z)
{
  return std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
}

template<typename EigenVector>
EigenVector interpolateVector(const EigenVector &vStart,  const EigenVector &vEnd, const Time &timeStart,
                          const Time &timeEnd, const Time &queryTime)
{
  const double duration = toSeconds(timeEnd - timeStart);
  const double factor = toSeconds(queryTime - timeStart) / duration;
  const Eigen::Vector3d interp = vStart + (vEnd - vStart) * factor;
  return interp;
}

template<typename EigenQuaternion>
EigenQuaternion interpolateQuaternion(const EigenQuaternion &qStart,  const EigenQuaternion &qEnd, const Time &timeStart,
                          const Time &timeEnd, const Time &queryTime)
{
  const double duration = toSeconds(timeEnd - timeStart);
  const double factor = toSeconds(queryTime - timeStart) / duration;
  const Eigen::Quaterniond interp =
      Eigen::Quaterniond(qStart)
          .slerp(factor, Eigen::Quaterniond(qEnd));
  return interp;
}

template <typename T>
Eigen::Quaternion<T> angleAxisVectorToRotationQuaternion(
    const Eigen::Matrix<T, 3, 1>& angle_axis) {
  T scale = T(0.5);
  T w = T(1.);
  constexpr double kCutoffAngle = 1e-8;  // We linearize below this angle.
  if (angle_axis.squaredNorm() > kCutoffAngle) {
    const T norm = angle_axis.norm();
    scale = sin(norm / 2.) / norm;
    w = cos(norm / 2.);
  }
  const Eigen::Matrix<T, 3, 1> quaternion_xyz = scale * angle_axis;
  return Eigen::Quaternion<T>(w, quaternion_xyz.x(), quaternion_xyz.y(),
                              quaternion_xyz.z());
}

}  // namespace icp_loco


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

#include <boost/multiprecision/cpp_dec_float.hpp>

#include "eigen.hpp"

template <>
struct related_number<boost::multiprecision::cpp_dec_float_100>
{
   typedef boost::multiprecision::cpp_dec_float_50 type;
};

int main()
{
   using namespace boost::multiprecision;
   //test_float_type_2<double>();
   test_float_type_2<boost::multiprecision::cpp_dec_float_100>();
   return 0;
}


// Copyright (c) 2018 Inria
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
// Author(s)     : Marc Glisse

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/CGAL_Ipelet_base.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace CGAL_mst {

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef CGAL::Delaunay_triangulation_2<Kernel>              Triangulation;

const std::string Slab[] = {
  "MST", "Help"
};

const std::string Hmsg[] = {
  "Draw the minimum spanning tree of a set of points"
};

struct mstIpelet
  : CGAL::Ipelet_base<Kernel, 2> {
  mstIpelet() : CGAL::Ipelet_base<Kernel, 2>("Minimum spanning tree", Slab, Hmsg){}
  void protected_run(int);
};

template <typename T>
struct Is_finite {
  const T* t_;
  Is_finite()
    : t_(nullptr)
  {}
  Is_finite(const T& t)
    : t_(&t)
  { }
  template <typename VertexOrEdge>
  bool operator()(const VertexOrEdge& voe) const {
    return ! t_->is_infinite(voe);
  }
};
typedef Is_finite<Triangulation> Filter;
typedef boost::filtered_graph<Triangulation,Filter,Filter> Finite_triangulation;
typedef boost::graph_traits<Finite_triangulation>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Finite_triangulation>::vertex_iterator vertex_iterator;
typedef boost::graph_traits<Finite_triangulation>::edge_descriptor edge_descriptor;
// The BGL makes use of indices associated to the vertices
// We use a std::map to store the index
typedef std::map<vertex_descriptor,int> VertexIndexMap;
VertexIndexMap vertex_id_map;
// A std::map is not a property map, because it is not lightweight
typedef boost::associative_property_map<VertexIndexMap> VertexIdPropertyMap;
VertexIdPropertyMap vertex_index_pmap(vertex_id_map);

void mstIpelet::protected_run(int /*fn*/)
{
  std::list<Point_2> pt_list;

  read_active_objects(
    CGAL::dispatch_or_drop_output<Point_2>(
      std::back_inserter(pt_list)
    )
  );

  if (pt_list.empty()) {
    print_error_message("No mark selected");
    return;
  }

  Triangulation t(pt_list.begin(), pt_list.end());
  Filter is_finite(t);
  Finite_triangulation ft(t, is_finite, is_finite);

  vertex_iterator vit, ve;
  // Associate indices to the vertices
  int index = 0;
  // boost::tie assigns the first and second element of the std::pair
  // returned by boost::vertices to the variables vit and ve
  for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){
    vertex_descriptor vd = *vit;
    vertex_id_map[vd] = index++;
    }
  // We use the default edge weight which is the squared length of the edge
  // This property map is defined in graph_traits_Triangulation_2.h
  // In the function call you can see a named parameter: vertex_index_map
   std::list<edge_descriptor> mst;
   boost::kruskal_minimum_spanning_tree(ft,
                    std::back_inserter(mst),
                    vertex_index_map(vertex_index_pmap));
   for(std::list<edge_descriptor>::iterator it = mst.begin(); it != mst.end(); ++it){
     edge_descriptor ed = *it;
     vertex_descriptor svd = source(ed,t);
     vertex_descriptor tvd = target(ed,t);
     Triangulation::Vertex_handle sv = svd;
     Triangulation::Vertex_handle tv = tvd;
     draw_in_ipe(Kernel::Segment_2(sv->point(), tv->point()));
   }
}
}

CGAL_IPELET(CGAL_mst::mstIpelet)


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_on_sphere_traits_2.h>
#include <CGAL/Projection_on_sphere_traits_3.h>
#include <CGAL/Delaunay_triangulation_on_sphere_2.h>

#include <CGAL/utility.h>
#include <CGAL/Qt/DemosMainWindow.h>

#include <QApplication>
#include <QMainWindow>
#include <QFileDialog>
#include <QInputDialog>
#include <CGAL/Three/Three.h>

#include <boost/iterator/transform_iterator.hpp>

#include <list>
#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel          Kernel;
typedef Kernel::FT                                                   FT;
typedef Kernel::Point_3                                              Point_3;
typedef Kernel::Segment_3                                            Segment_3;

typedef CGAL::Projection_on_sphere_traits_3<Kernel>                  Projection_traits;
typedef CGAL::Delaunay_triangulation_on_sphere_2<Projection_traits>  Projected_DToS2;

typedef std::list<std::vector<Point_3> >                             Subsampled_arcs;

#include "Viewer.h"
#include "ui_Mainwindow.h"

template <class Output_iterator>
void read_points(const char* file_path, Output_iterator out)
{
  std::ifstream input(file_path);
  if(!input)
  {
    std::cerr << "Error while reading " << file_path << std::endl;
    std::exit(EXIT_FAILURE);
  }

  double x,y,z;
  while(input >> x >> y >> z)
    *out++ = Point_3(x, y, z);
}

class MainWindow
  : public CGAL::Qt::DemosMainWindow,
    public Ui::MainWindow
{
  Q_OBJECT
public:
  MainWindow()
  {
    setupUi(this);
  }

public slots:
  void open(QString filename)
  {
    std::vector<Point_3> lst_pt;
    read_points(filename.toUtf8().data(), std::back_inserter(lst_pt));

    const Point_3 center(0,0,0);
    const QString infos = QInputDialog::getText(nullptr, "Sphere", "Enter the sphere's information : (Radius center.x center.y center.z):",
                                                QLineEdit::Normal,"100.0 0.0 0.0 0.0");
    QStringList list = infos.split(QRegExp("\\s+"), CGAL_QT_SKIP_EMPTY_PARTS);
    if (list.isEmpty()) return;
    if (list.size()!=4){
      QMessageBox *msgBox = new QMessageBox;
      msgBox->setWindowTitle("Error");
      msgBox->setText("ERROR : Input should consists of 4 doubles: The radius first, then the coordinates of the center.");
      msgBox->exec();
      return;
    }

    double coords[4];
    for(int j=0; j<4; ++j)
    {
      bool ok;
      coords[j] = list.at(j).toDouble(&ok);
      if(!ok)
      {
          QMessageBox *msgBox = new QMessageBox;
          msgBox->setWindowTitle("Error");
          msgBox->setText("ERROR : Input is invalid.");
          msgBox->exec();
          return;
      }
    }

    Projection_traits traits(Point_3(coords[1], coords[2], coords[3]), coords[0]);
    Projected_DToS2 dtos(lst_pt.begin(), lst_pt.end(), traits);

    std::cout << dtos.number_of_vertices() << " vertices" << std::endl;

    // Instantiate the viewer
    viewer->open(lst_pt.begin(), lst_pt.end(), dtos);
  }

  void on_action_Quit_triggered()
  {
    close();
  }

  void on_action_Open_triggered()
  {
    QString filename = QFileDialog::getOpenFileName(this);
    if(!filename.isNull())
      open(filename);
  }
};

int main(int argc, char** argv)
{
  // Read command lines arguments
  QApplication application(argc,argv);
  MainWindow mainWindow;
  mainWindow.show();

  // Run main loop
  return application.exec();
}

#include "main.moc"


/**
 * @file
 * @brief NPDE homework ProjectionOntoGradients code
 * @author ?, Philippe Peter
 * @date December 2019
 * @copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <iostream>
#include <memory>

#include "projectionontogradients.h"

int main() {
  // for this exercise a main file is not required
  // but feel free to use it to call some of your functions for debugging
  // purposes
  const auto f = [](Eigen::Vector2d x) { return Eigen::Vector2d(-x(1), x(0)); };
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  // Compute solution
  const Eigen::VectorXd sol_vec =
      ProjectionOntoGradients::projectOntoGradients(dofh, f);
  std::cout << sol_vec << std::endl;

  std::cout << "You may use this main file to call your function "
               "ProjectionOntoGradients::projectOntoGradients"
            << std::endl;
}


#include <iostream>
#include <ctime>

#include <Eigen/Core>
#include <Eigen/Dense>

#define MATRIX_SIZE 50

int main() {

  // 2 X 3 float mat
  Eigen::Matrix<float, 2, 3> matrix_23;

  // 3 X 1 double mat
  Eigen::Vector3d v_3d;

  // 3 X 3 double mat
  Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();

  // dynamic matrix
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;

  Eigen::MatrixXd matrix_x;

  // op matrix
  matrix_23 << 1, 2, 3, 4, 5, 6;

  std::cout << matrix_23 << std::endl << std::endl;

  for (int i = 0; i < 1; i++) {
    for (int j = 0; j < 2; j++) {
      std::cout << matrix_23(i, j) << std::endl;
    }
  }

  v_3d << 3, 2, 1;

  // matrix_23 must float -> double
  Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
  std::cout << std::endl << result << std::endl << std::endl;

  // Error!
  // Eigen::Matrix<double, 2, 3> result = matrix_23.cast<double>() * v_3d;

  matrix_33 = Eigen::Matrix3d::Random();
  std::cout << matrix_33 << std::endl << std::endl << std::endl;

  std::cout << matrix_33.transpose() << std::endl << std::endl;
  std::cout << matrix_33.sum() << std::endl << std::endl;
  std::cout << matrix_33.trace() << std::endl << std::endl;
  std::cout << matrix_33.inverse() << std::endl << std::endl;
  std::cout << matrix_33.determinant() << std::endl << std::endl;
  std::cout << 10 * matrix_33 << std::endl << std::endl;

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);

  std::cout << "Eigen value = " << std::endl << eigen_solver.eigenvalues() << std::endl << std::endl;
  std::cout << "Eigen vector = " << std::endl << eigen_solver.eigenvectors() << std::endl << std::endl;

  Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_nn;
  matrix_nn = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  Eigen::Matrix<double, MATRIX_SIZE, 1> v_nd;

  v_nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

  std::clock_t time_stt = std::clock();

  Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_nn.inverse() * v_nd;
  std::cout << "time use in normal inverse is: "
    << 1000 * (std::clock() - time_stt) / (double)CLOCKS_PER_SEC
    << "ms" << std::endl << std::endl;

  time_stt = std::clock();
  x = matrix_nn.colPivHouseholderQr().solve(v_nd);
  std::cout << "time use in Qr composition is: "
    << 1000 * (std::clock() - time_stt) / (double)CLOCKS_PER_SEC
    << "ms" << std::endl << std::endl;

  return 0;
}


#ifndef RenderUtilities_h
#define RenderUtilities_h

#include "PngWrapper.hpp"

#include <Eigen/Dense>

class Camera;

uint8_t * render_scene(uint16_t width, uint16_t height,
                       const Eigen::Matrix<float, 3, Eigen::Dynamic>& vertices,
                       const Eigen::Matrix<float, 3, Eigen::Dynamic>& normals,
                       const Camera & camera, const Eigen::Vector3f & light_source);

PngWrapper * normals_as_png(uint16_t width, uint16_t height, const Eigen::Matrix<float,3,Eigen::Dynamic>& normals);
/*
 * Implement Lambertian colouring as per https://www.cs.unc.edu/~rademach/xroads-RT/RTarticle.html
 * @param width The with of the image
 * @param height The height of the imahe
 * @param vertices A width x height array of Vertex coordinates in global space
 * @param normals A width x height array of surface normals in global space
 * @param camera The Camera for which to render
 * @param light_source THe global position of the light source
 */
PngWrapper * scene_as_png(uint16_t width, uint16_t height,
                          const Eigen::Matrix<float,3,Eigen::Dynamic>& vertices,
                          const Eigen::Matrix<float,3,Eigen::Dynamic>& normals,
                          const Camera & camera,
                          const Eigen::Vector3f & light_source);

void save_normals_as_colour_png( std::string filename, uint16_t width, uint16_t height, const Eigen::Matrix<float,3,Eigen::Dynamic>& normals );
void save_rendered_scene_as_png( std::string filename, uint16_t width, uint16_t height, const Eigen::Matrix<float,3,Eigen::Dynamic>& vertices, const Eigen::Matrix<float,3,Eigen::Dynamic>& normals, const Camera & camera, const Eigen::Vector3f & light_source);

#endif // RenderUtilities_h